From 017e3f0c48e421ad4da836c82643786c05f6ba3a Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 28 Oct 2023 12:58:32 -0500 Subject: [PATCH 1/6] AP_Compass: fix C++ One Definition Rule violations Two structs with the same name must have exactly the same definition, no matter where they occur in the program. Move each sample register struct definition into the associated class definition so they are in a different namespace and no longer identicially named, thus fixing this issue. --- libraries/AP_Compass/AP_Compass_AK09916.cpp | 9 +-------- libraries/AP_Compass/AP_Compass_AK09916.h | 8 ++++++++ libraries/AP_Compass/AP_Compass_AK8963.cpp | 7 +------ libraries/AP_Compass/AP_Compass_AK8963.h | 6 ++++++ libraries/AP_Compass/AP_Compass_LSM9DS1.cpp | 5 ----- libraries/AP_Compass/AP_Compass_LSM9DS1.h | 5 +++++ 6 files changed, 21 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index bb6732365a25d..8630da5e9747b 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -58,13 +58,6 @@ extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal; -struct PACKED sample_regs { - uint8_t st1; - int16_t val[3]; - uint8_t tmps; - uint8_t st2; -}; - AP_Compass_AK09916::AP_Compass_AK09916(AP_AK09916_BusDriver *bus, bool force_external, enum Rotation rotation) @@ -477,7 +470,7 @@ bool AP_AK09916_BusDriver_Auxiliary::configure() bool AP_AK09916_BusDriver_Auxiliary::start_measurements() { - if (_bus->register_periodic_read(_slave, REG_ST1, sizeof(sample_regs)) < 0) { + if (_bus->register_periodic_read(_slave, REG_ST1, sizeof(AP_Compass_AK09916::sample_regs)) < 0) { return false; } diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index b4cca6e97346e..5c80cff9ef675 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/libraries/AP_Compass/AP_Compass_AK09916.h @@ -75,6 +75,14 @@ class AP_Compass_AK09916 : public AP_Compass_Backend void read() override; + /* Must be public so the BusDriver can access its definition */ + struct PACKED sample_regs { + uint8_t st1; + int16_t val[3]; + uint8_t tmps; + uint8_t st2; + }; + private: AP_Compass_AK09916(AP_AK09916_BusDriver *bus, bool force_external, enum Rotation rotation); diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 4c4ced9e3f5c8..ad6a8c6d234ef 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -49,11 +49,6 @@ #define AK8963_MILLIGAUSS_SCALE 10.0f -struct PACKED sample_regs { - int16_t val[3]; - uint8_t st2; -}; - extern const AP_HAL::HAL &hal; AP_Compass_AK8963::AP_Compass_AK8963(AP_AK8963_BusDriver *bus, @@ -379,7 +374,7 @@ bool AP_AK8963_BusDriver_Auxiliary::configure() bool AP_AK8963_BusDriver_Auxiliary::start_measurements() { - if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(sample_regs)) < 0) { + if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(AP_Compass_AK8963::sample_regs)) < 0) { return false; } diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 7b61768d84225..c0f020756571a 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -39,6 +39,12 @@ class AP_Compass_AK8963 : public AP_Compass_Backend void read() override; + /* Must be public so the BusDriver can access its definition */ + struct PACKED sample_regs { + int16_t val[3]; + uint8_t st2; + }; + private: AP_Compass_AK8963(AP_AK8963_BusDriver *bus, enum Rotation rotation); diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index eeebe00fdca5c..60db0f54de622 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -47,11 +47,6 @@ #define LSM9DS1M_INT_THS_L_M 0x32 #define LSM9DS1M_INT_THS_H_M 0x33 -struct PACKED sample_regs { - uint8_t status; - int16_t val[3]; -}; - extern const AP_HAL::HAL &hal; AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(AP_HAL::OwnPtr dev, diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.h b/libraries/AP_Compass/AP_Compass_LSM9DS1.h index 2cb91c74c6de4..7017883bd62ad 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.h +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.h @@ -42,6 +42,11 @@ class AP_Compass_LSM9DS1 : public AP_Compass_Backend uint8_t _compass_instance; float _scaling; enum Rotation _rotation; + + struct PACKED sample_regs { + uint8_t status; + int16_t val[3]; + }; }; #endif From c8090c9db31daa0ccbac9b02b62f200768a4339f Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Wed, 15 Nov 2023 11:19:40 -0600 Subject: [PATCH 2/6] AP_HAL: mark can array mutable Allows update of CAN interface during driver initialization without UB-inducing casting away of const. --- libraries/AP_HAL/HAL.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index f8827444204f4..d17409bcf0a5c 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -141,9 +141,9 @@ class AP_HAL::HAL { AP_HAL::Flash *flash; AP_HAL::DSP *dsp; #if HAL_NUM_CAN_IFACES > 0 - AP_HAL::CANIface* can[HAL_NUM_CAN_IFACES]; + mutable AP_HAL::CANIface* can[HAL_NUM_CAN_IFACES]; #else - AP_HAL::CANIface** can; + mutable AP_HAL::CANIface** can; #endif // access to serial ports using SERIALn_ numbering From b29f862ba376dee24925048feae5331396898bd9 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 28 Oct 2023 18:59:55 -0500 Subject: [PATCH 3/6] AP_HAL_ChibiOS/CAN*Iface: fix C++ One Definition Rule violations Switch to const extern hal reference, then use mutability of can array to store the CAN driver reference during driver init and eliminate UB. --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 2 +- libraries/AP_HAL_ChibiOS/CanIface.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 2d261f7242084..0430058d92474 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -85,7 +85,7 @@ #error "Unsupported MCU for FDCAN" #endif -extern AP_HAL::HAL& hal; +extern const AP_HAL::HAL& hal; #define STR(x) #x #define XSTR(x) STR(x) diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 677f484804943..49bcfdb47f47d 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -84,7 +84,7 @@ #endif -extern AP_HAL::HAL& hal; +extern const AP_HAL::HAL& hal; using namespace ChibiOS; From bbd4949905a8336a34d6d5a24a5f1d55db288b22 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Wed, 15 Nov 2023 11:23:47 -0600 Subject: [PATCH 4/6] AP_CANManager: remove const_cast to store CAN interface Eliminates UB, now possible with mutable can array in AP_HAL. --- libraries/AP_CANManager/AP_CANManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 78be64a97c66c..323ba2ed454eb 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -149,7 +149,7 @@ void AP_CANManager::init() if (hal.can[i] == nullptr) { // So if this interface is not allocated allocate it here, // also pass the index of the CANBus - const_cast (hal).can[i] = new HAL_CANIface(i); + hal.can[i] = new HAL_CANIface(i); } // Initialise the interface we just allocated @@ -305,7 +305,7 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver if (hal.can[i] == nullptr) { // if this interface is not allocated allocate it here, // also pass the index of the CANBus - const_cast (hal).can[i] = new HAL_CANIface(i); + hal.can[i] = new HAL_CANIface(i); } // Initialise the interface we just allocated From 784f435d9a1d11886282354abcaf647a86d1f734 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Wed, 15 Nov 2023 11:24:59 -0600 Subject: [PATCH 5/6] AP_DroneCAN: remove const_cast to store CAN interface Eliminates UB, now possible with mutable can array in AP_HAL. --- .../AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp index 97a7850d41efd..6308046b4215e 100644 --- a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp +++ b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp @@ -117,7 +117,7 @@ static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan void DroneCAN_sniffer::init(void) { - const_cast (hal).can[driver_index] = new HAL_CANIface(driver_index); + hal.can[driver_index] = new HAL_CANIface(driver_index); if (hal.can[driver_index] == nullptr) { AP_HAL::panic("Couldn't allocate CANManager, something is very wrong"); From 8e4070dfc0e5d3f94e77babdb1d0573f96d10a4b Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Wed, 15 Nov 2023 11:56:29 -0600 Subject: [PATCH 6/6] AP_HAL: use array of UART drivers instead of consecutive variables Avoids UB-inducing assumption that UART drivers are consecutive in the serial() function. --- libraries/AP_HAL/HAL.cpp | 1 - libraries/AP_HAL/HAL.h | 38 +++++++++++++++----------------------- 2 files changed, 15 insertions(+), 24 deletions(-) diff --git a/libraries/AP_HAL/HAL.cpp b/libraries/AP_HAL/HAL.cpp index a2d1216d2b389..92fa3588e698f 100644 --- a/libraries/AP_HAL/HAL.cpp +++ b/libraries/AP_HAL/HAL.cpp @@ -15,7 +15,6 @@ HAL::FunCallbacks::FunCallbacks(void (*setup_fun)(void), void (*loop_fun)(void)) // access serial ports using SERIALn numbering AP_HAL::UARTDriver* AP_HAL::HAL::serial(uint8_t sernum) const { - UARTDriver **uart_array = const_cast(&uartA); // this mapping captures the historical use of uartB as SERIAL3 const uint8_t mapping[] = { 0, 2, 3, 1, 4, 5, 6, 7, 8, 9 }; static_assert(sizeof(mapping) == num_serial, "num_serial must match mapping"); diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index d17409bcf0a5c..a3701eb349555 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -53,16 +53,17 @@ class AP_HAL::HAL { AP_HAL::CANIface** _can_ifaces) #endif : - uartA(_uartA), - uartB(_uartB), - uartC(_uartC), - uartD(_uartD), - uartE(_uartE), - uartF(_uartF), - uartG(_uartG), - uartH(_uartH), - uartI(_uartI), - uartJ(_uartJ), + uart_array{ + _uartA, + _uartB, + _uartC, + _uartD, + _uartE, + _uartF, + _uartG, + _uartH, + _uartI, + _uartJ}, i2c_mgr(_i2c_mgr), spi(_spi), wspi(_wspi), @@ -112,19 +113,6 @@ class AP_HAL::HAL { virtual void run(int argc, char * const argv[], Callbacks* callbacks) const = 0; -private: - // the uartX ports must be contiguous in ram for the serial() method to work - AP_HAL::UARTDriver* uartA; - AP_HAL::UARTDriver* uartB UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartC UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartD UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartE UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartF UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartG UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartH UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartI UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartJ UNUSED_PRIVATE_MEMBER; - public: AP_HAL::I2CDeviceManager* i2c_mgr; AP_HAL::SPIDeviceManager* spi; @@ -151,6 +139,10 @@ class AP_HAL::HAL { static constexpr uint8_t num_serial = 10; +private: + AP_HAL::UARTDriver* uart_array[num_serial]; + +public: #if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL AP_HAL::SIMState *simstate; #endif