HAL_ChibiOS: modify hal can drivers for use by AP_Periph

libraries/AP_HAL_ChibiOS/CANFDIface.cpp

@@ -63,22 +63,27 @@
 
 #define MESSAGE_RAM_END_ADDR 0x4000B5FC
 
-extern const AP_HAL::HAL& hal;
+extern AP_HAL::HAL& hal;
 
 static_assert(STM32_FDCANCLK <= 80U*1000U*1000U, "FDCAN clock must be max 80MHz");
 
 using namespace ChibiOS;
 
+#if HAL_MAX_CAN_PROTOCOL_DRIVERS
 #define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANFDIface", fmt, ##args); } while (0)
+#else
+#define Debug(fmt, args...)
+#endif
 
 constexpr CANIface::CanType* const CANIface::Can[];
+static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr};
 
 static inline bool driver_initialised(uint8_t iface_index)
 {
     if (iface_index >= HAL_NUM_CAN_IFACES) {
         return false;
     }
-    if (hal.can[iface_index] == nullptr) {
+    if (can_ifaces[iface_index] == nullptr) {
         return false;
     }
     return true;
@@ -97,12 +102,12 @@ static inline void handleCANInterrupt(uint8_t iface_index, uint8_t line_index)
         if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF0N) ||
             (CANIface::Can[iface_index]->IR & FDCAN_IR_RF0F)) {
             CANIface::Can[iface_index]->IR = FDCAN_IR_RF0N | FDCAN_IR_RF0F;
-            ((ChibiOS::CANIface*)hal.can[iface_index])->handleRxInterrupt(0);
+            can_ifaces[iface_index]->handleRxInterrupt(0);
         }
         if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF1N) ||
             (CANIface::Can[iface_index]->IR & FDCAN_IR_RF1F)) {
             CANIface::Can[iface_index]->IR = FDCAN_IR_RF1N | FDCAN_IR_RF1F;
-            ((ChibiOS::CANIface*)hal.can[iface_index])->handleRxInterrupt(1);
+            can_ifaces[iface_index]->handleRxInterrupt(1);
         }
     } else {
         if (CANIface::Can[iface_index]->IR & FDCAN_IR_TC) {
@@ -111,22 +116,23 @@ static inline void handleCANInterrupt(uint8_t iface_index, uint8_t line_index)
             if (timestamp_us > 0) {
                 timestamp_us--;
             }
-            ((ChibiOS::CANIface*)hal.can[iface_index])->handleTxCompleteInterrupt(timestamp_us);
+            can_ifaces[iface_index]->handleTxCompleteInterrupt(timestamp_us);
         }
 
         if ((CANIface::Can[iface_index]->IR & FDCAN_IR_BO)) {
             CANIface::Can[iface_index]->IR = FDCAN_IR_BO;
-            ((ChibiOS::CANIface*)hal.can[iface_index])->handleBusOffInterrupt();
+            can_ifaces[iface_index]->handleBusOffInterrupt();
         }
     }
-    ((ChibiOS::CANIface*)hal.can[iface_index])->pollErrorFlagsFromISR();
+    can_ifaces[iface_index]->pollErrorFlagsFromISR();
 }
 
 uint32_t CANIface::FDCANMessageRAMOffset_ = 0;
 
 CANIface::CANIface(uint8_t index) :
     self_index_(index),
-    rx_queue_(HAL_CAN_RX_QUEUE_SIZE)
+    rx_bytebuffer_((uint8_t*)rx_buffer, sizeof(rx_buffer)),
+    rx_queue_(&rx_bytebuffer_)
 {
     if (index >= HAL_NUM_CAN_IFACES) {
          AP_HAL::panic("Bad CANIface index.");
@@ -451,6 +457,18 @@ uint16_t CANIface::getNumFilters() const
 bool CANIface::clock_init_ = false;
 bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
 {
+    Debug("Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
+    if (self_index_ > HAL_NUM_CAN_IFACES) {
+        Debug("CAN drv init failed");
+        return false;
+    }
+    if (can_ifaces[self_index_] == nullptr) {
+        can_ifaces[self_index_] = this;
+#if !defined(HAL_BOOTLOADER_BUILD)
+        hal.can[self_index_] = this;
+#endif
+    }
+
     //Only do it once
     //Doing it second time will reset the previously initialised bus
     if (!clock_init_) {
@@ -604,7 +622,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
             }
             if (event_handle_ != nullptr) {
                 stats.num_events++;
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
                 evt_src_.signalI(1 << self_index_);
+#endif
             }
         }
     }
@@ -703,7 +723,9 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index)
     }
     if (event_handle_ != nullptr) {
         stats.num_events++;
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
         evt_src_.signalI(1 << self_index_);
+#endif
     }
 }
 
@@ -766,6 +788,7 @@ uint32_t CANIface::getErrorCount() const
            stats.tx_timedout;
 }
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
 ChibiOS::EventSource CANIface::evt_src_;
 bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
 {
@@ -774,6 +797,7 @@ bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
     event_handle_->set_source(&evt_src_);
     return event_handle_->register_event(1 << self_index_);
 }
+#endif
 
 bool CANIface::isRxBufferEmpty() const
 {
@@ -857,6 +881,7 @@ bool CANIface::select(bool &read, bool &write,
     return true; // Return value doesn't matter as long as it is non-negative
 }
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
 uint32_t CANIface::get_stats(char* data, uint32_t max_size)
 {
     if (data == nullptr) {
@@ -886,7 +911,7 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
                             stats.num_events);
     return ret;
 }
-
+#endif
 
 /*
  * Interrupt handlers

libraries/AP_HAL_ChibiOS/CANFDIface.h

@@ -106,6 +106,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     static uint32_t FDCANMessageRAMOffset_;
 
     CanType* can_;
+    
+    CanRxItem rx_buffer[HAL_CAN_RX_QUEUE_SIZE];
+    ByteBuffer rx_bytebuffer_;
     ObjectBuffer<CanRxItem> rx_queue_;
     CanTxItem pending_tx_[NumTxMailboxes];
     uint8_t peak_tx_mailbox_index_;
@@ -113,7 +116,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     bool initialised_;
     bool had_activity_;
     AP_HAL::EventHandle* event_handle_;
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     static ChibiOS::EventSource evt_src_;
+#endif
     const uint8_t self_index_;
 
     bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
@@ -208,13 +213,14 @@ public:
                 const AP_HAL::CANFrame* const pending_tx,
                 uint64_t blocking_deadline) override;
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     // setup event handle for waiting on events
     bool set_event_handle(AP_HAL::EventHandle* handle) override;
 
     // fetch stats text and return the size of the same,
     // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt 
     uint32_t get_stats(char* data, uint32_t max_size) override;
-
+#endif
     /************************************
      * Methods used inside interrupt    *
      ************************************/

libraries/AP_HAL_ChibiOS/CANIface.h

@@ -96,15 +96,18 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     static const uint32_t TSR_ABRQx[NumTxMailboxes];
 
     ChibiOS::bxcan::CanType* can_;
-    uint64_t error_cnt_;
+
+    CanRxItem rx_buffer[HAL_CAN_RX_QUEUE_SIZE];
+    ByteBuffer rx_bytebuffer_;
     ObjectBuffer<CanRxItem> rx_queue_;
     CanTxItem pending_tx_[NumTxMailboxes];
-    bool irq_init_;
-    bool initialised_;
-    bool had_activity_;
+    bool irq_init_:1;
+    bool initialised_:1;
+    bool had_activity_:1;
+#ifndef HAL_BUILD_AP_PERIPH
     AP_HAL::EventHandle* event_handle_;
     static ChibiOS::EventSource evt_src_;
-
+#endif
     const uint8_t self_index_;
 
     bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
@@ -132,6 +135,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
 
     void initOnce(bool enable_irq);
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     struct {
         uint32_t tx_requests;
         uint32_t tx_rejected;
@@ -145,6 +149,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
         uint32_t num_busoff_err;
         uint32_t num_events;
     } stats;
+#endif
 
 public:
     /******************************************
@@ -165,10 +170,11 @@ public:
     int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
                     CanIOFlags& out_flags) override;
 
+#if !defined(HAL_BOOTLOADER_BUILD)
     // Set Filters to ignore frames not to be handled by us
     bool configureFilters(const CanFilterConfig* filter_configs,
                           uint16_t num_configs) override;
-
+#endif
     // In BxCAN the Busoff error is cleared automatically,
     // so always return false
     bool is_busoff() const override
@@ -185,7 +191,9 @@ public:
     }
 
     // Get total number of Errors discovered
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     uint32_t getErrorCount() const override;
+#endif
 
     // returns true if init was successfully called
     bool is_initialized() const override
@@ -201,13 +209,14 @@ public:
                 const AP_HAL::CANFrame* const pending_tx,
                 uint64_t blocking_deadline) override;
     
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     // setup event handle for waiting on events
     bool set_event_handle(AP_HAL::EventHandle* handle) override;
 
     // fetch stats text and return the size of the same,
     // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt 
     uint32_t get_stats(char* data, uint32_t max_size) override;
-
+#endif
     /************************************
      * Methods used inside interrupt    *
      ************************************/

libraries/AP_HAL_ChibiOS/CanIface.cpp

@@ -50,13 +50,6 @@
 # if !defined(STM32H7XX)
 #include "CANIface.h"
 
-#define CAN1_TX_IRQHandler      STM32_CAN1_TX_HANDLER
-#define CAN1_RX0_IRQHandler     STM32_CAN1_RX0_HANDLER
-#define CAN1_RX1_IRQHandler     STM32_CAN1_RX1_HANDLER
-#define CAN2_TX_IRQHandler      STM32_CAN2_TX_HANDLER
-#define CAN2_RX0_IRQHandler     STM32_CAN2_RX0_HANDLER
-#define CAN2_RX1_IRQHandler     STM32_CAN2_RX1_HANDLER
-
 /* STM32F3's only CAN inteface does not have a number. */
 #if defined(STM32F3XX)
 #define RCC_APB1ENR_CAN1EN     RCC_APB1ENR_CANEN
@@ -64,18 +57,37 @@
 #define CAN1_TX_IRQn           CAN_TX_IRQn
 #define CAN1_RX0_IRQn          CAN_RX0_IRQn
 #define CAN1_RX1_IRQn          CAN_RX1_IRQn
-#define CAN1_TX_IRQHandler     CAN_TX_IRQHandler
-#define CAN1_RX0_IRQHandler    CAN_RX0_IRQHandler
-#define CAN1_RX1_IRQHandler    CAN_RX1_IRQHandler
+#define CAN1_TX_IRQ_Handler      STM32_CAN1_TX_HANDLER
+#define CAN1_RX0_IRQ_Handler     STM32_CAN1_RX0_HANDLER
+#define CAN1_RX1_IRQ_Handler     STM32_CAN1_RX1_HANDLER
+#else
+#define CAN1_TX_IRQ_Handler      STM32_CAN1_TX_HANDLER
+#define CAN1_RX0_IRQ_Handler     STM32_CAN1_RX0_HANDLER
+#define CAN1_RX1_IRQ_Handler     STM32_CAN1_RX1_HANDLER
+#define CAN2_TX_IRQ_Handler      STM32_CAN2_TX_HANDLER
+#define CAN2_RX0_IRQ_Handler     STM32_CAN2_RX0_HANDLER
+#define CAN2_RX1_IRQ_Handler     STM32_CAN2_RX1_HANDLER
+#endif // #if defined(STM32F3XX)
+
+#if HAL_MAX_CAN_PROTOCOL_DRIVERS
+#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANIface", fmt, ##args); } while (0)
+#else
+#define Debug(fmt, args...)
 #endif
 
-#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANIface", fmt, ##args); } while (0)
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
+#define PERF_STATS(x) (x++)
+#else
+#define PERF_STATS(x)
+#endif
 
-extern const AP_HAL::HAL& hal;
+
+extern AP_HAL::HAL& hal;
 
 using namespace ChibiOS;
 
 constexpr bxcan::CanType* const CANIface::Can[];
+static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr};
 
 static inline void handleTxInterrupt(uint8_t iface_index)
 {
@@ -86,8 +98,8 @@ static inline void handleTxInterrupt(uint8_t iface_index)
     if (precise_time > 0) {
         precise_time--;
     }
-    if (hal.can[iface_index] != nullptr) {
-        ((ChibiOS::CANIface*)hal.can[iface_index])->handleTxInterrupt(precise_time);
+    if (can_ifaces[iface_index] != nullptr) {
+        can_ifaces[iface_index]->handleTxInterrupt(precise_time);
     }
 }
 
@@ -100,8 +112,8 @@ static inline void handleRxInterrupt(uint8_t iface_index, uint8_t fifo_index)
     if (precise_time > 0) {
         precise_time--;
     }
-    if (hal.can[iface_index] != UAVCAN_NULLPTR) {
-        ((CANIface*)hal.can[iface_index])->handleRxInterrupt(fifo_index, precise_time);
+    if (can_ifaces[iface_index] != nullptr) {
+        can_ifaces[iface_index]->handleRxInterrupt(fifo_index, precise_time);
     }
 }
 
@@ -117,7 +129,8 @@ const uint32_t CANIface::TSR_ABRQx[CANIface::NumTxMailboxes] = {
 
 CANIface::CANIface(uint8_t index) :
     self_index_(index),
-    rx_queue_(HAL_CAN_RX_QUEUE_SIZE)
+    rx_bytebuffer_((uint8_t*)rx_buffer, sizeof(rx_buffer)),
+    rx_queue_(&rx_bytebuffer_)
 {
     if (index >= HAL_NUM_CAN_IFACES) {
         AP_HAL::panic("Bad CANIface index.");
@@ -126,7 +139,6 @@ CANIface::CANIface(uint8_t index) :
     }
 }
 
-
 bool CANIface::computeTimings(uint32_t target_bitrate, Timings& out_timings)
 {
     if (target_bitrate < 1) {
@@ -293,7 +305,7 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
     } else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) {
         txmailbox = 2;
     } else {
-        stats.tx_rejected++;
+        PERF_STATS(stats.tx_rejected);
         return 0;       // No transmission for you.
     }
 
@@ -350,6 +362,7 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
     return 1;
 }
 
+#if !defined(HAL_BOOTLOADER_BUILD)
 bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
                                 uint16_t num_configs)
 {
@@ -418,6 +431,7 @@ bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
 
     return false;
 }
+#endif
 
 bool CANIface::waitMsrINakBitStateChange(bool target_state)
 {
@@ -447,13 +461,13 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const
         rx_item.frame = txi.frame;
         rx_item.timestamp_us = timestamp_us;
         rx_item.flags = AP_HAL::CANIface::Loopback;
-        stats.tx_loopback++;
+        PERF_STATS(stats.tx_loopback);
         rx_queue_.push(rx_item);
     }
 
     if (txok && !txi.pushed) {
         txi.pushed = true;
-        stats.tx_success++;
+        PERF_STATS(stats.tx_success);
     }
 }
 
@@ -476,10 +490,12 @@ void CANIface::handleTxInterrupt(const uint64_t utc_usec)
         handleTxMailboxInterrupt(2, txok, utc_usec);
     }
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     if (event_handle_ != nullptr) {
-        stats.num_events++;
+        PERF_STATS(stats.num_events);
         evt_src_.signalI(1 << self_index_);
     }
+#endif
     pollErrorFlagsFromISR();
 }
 
@@ -494,7 +510,7 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
      * Register overflow as a hardware error
      */
     if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
-        stats.rx_errors++;
+        PERF_STATS(stats.rx_errors);
     }
 
     /*
@@ -535,18 +551,19 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
     rx_item.timestamp_us = timestamp_us;
     rx_item.flags = 0;
     if (rx_queue_.push(rx_item)) {
-        stats.rx_received++;
+        PERF_STATS(stats.rx_received);
     } else {
-        stats.rx_overflow++;
+        PERF_STATS(stats.rx_overflow);
     }
 
     had_activity_ = true;
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     if (event_handle_ != nullptr) {
-        stats.num_events++;
+        PERF_STATS(stats.num_events);
         evt_src_.signalI(1 << self_index_);
     }
-
+#endif
     pollErrorFlagsFromISR();
 }
 
@@ -555,7 +572,6 @@ void CANIface::pollErrorFlagsFromISR()
     const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
     if (lec != 0) {
         can_->ESR = 0;
-        error_cnt_++;
 
         // Serving abort requests
         for (int i = 0; i < NumTxMailboxes; i++) {
@@ -563,7 +579,7 @@ void CANIface::pollErrorFlagsFromISR()
             if (txi.aborted && txi.abort_on_error) {
                 can_->TSR = TSR_ABRQx[i];
                 txi.aborted = true;
-                stats.tx_abort++;
+                PERF_STATS(stats.tx_abort);
             }
         }
     }
@@ -580,7 +596,7 @@ void CANIface::discardTimedOutTxMailboxes(uint64_t current_time)
         if (txi.deadline < current_time) {
             can_->TSR = TSR_ABRQx[i];  // Goodnight sweet transmission
             pending_tx_[i].aborted = true;
-            stats.tx_timedout++;
+            PERF_STATS(stats.tx_timedout);
         }
     }
 }
@@ -637,6 +653,7 @@ bool CANIface::isRxBufferEmpty() const
     return rx_queue_.available() == 0;
 }
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
 uint32_t CANIface::getErrorCount() const
 {
     CriticalSectionLocker lock;
@@ -657,6 +674,8 @@ bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
     return event_handle_->register_event(1 << self_index_);
 }
 
+#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
+
 void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const
 {
     write = false;
@@ -687,6 +706,9 @@ bool CANIface::select(bool &read, bool &write,
     if ((read && in_read) || (write && in_write)) {
         return true;
     }
+
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
+    // we don't support blocking select in AP_Periph and bootloader
     while (time < blocking_deadline) {
         if (event_handle_ == nullptr) {
             break;
@@ -698,6 +720,7 @@ bool CANIface::select(bool &read, bool &write,
         }
         time = AP_HAL::micros();
     }
+#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
     return true;
 }
 
@@ -750,20 +773,26 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
         Debug("CAN drv init failed");
         return false;
     }
+    if (can_ifaces[self_index_] == nullptr) {
+        can_ifaces[self_index_] = this;
+#if !defined(HAL_BOOTLOADER_BUILD)
+        hal.can[self_index_] = this;
+#endif
+    }
 
-    if (hal.can[0] == nullptr) {
-        const_cast <AP_HAL::HAL&> (hal).can[0] = new CANIface(0);
+    if (can_ifaces[0] == nullptr) {
+        can_ifaces[0] = new CANIface(0);
         Debug("Failed to allocate CAN iface 0");
-        if (hal.can[0] == nullptr) {
+        if (can_ifaces[0] == nullptr) {
             return false;
         }
     }
-    if (self_index_ == 1 && !hal.can[0]->is_initialized()) {
+    if (self_index_ == 1 && !can_ifaces[0]->is_initialized()) {
         Debug("Iface 0 is not initialized yet but we need it for Iface 1, trying to init it");
         Debug("Enabling CAN iface 0");
-        ((CANIface*)hal.can[0])->initOnce(false);
+        can_ifaces[0]->initOnce(false);
         Debug("Initing iface 0...");
-        if (!hal.can[0]->init(bitrate, mode)) {
+        if (!can_ifaces[0]->init(bitrate, mode)) {
             Debug("Iface 0 init failed");
             return false;;
         }
@@ -793,7 +822,6 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
      * Object state - interrupts are disabled, so it's safe to modify it now
      */
     rx_queue_.clear();
-    error_cnt_ = 0;
 
     for (uint32_t i=0; i < NumTxMailboxes; i++) {
         pending_tx_[i] = CanTxItem();
@@ -867,6 +895,7 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
     return true;
 }
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
 uint32_t CANIface::get_stats(char* data, uint32_t max_size)
 {
     if (data == nullptr) {
@@ -897,6 +926,7 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
     memset(&stats, 0, sizeof(stats));
     return ret;
 }
+#endif
 
 /*
  * Interrupt handlers
@@ -904,24 +934,24 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
 extern "C"
 {
 
-    CH_IRQ_HANDLER(CAN1_TX_IRQHandler);
-    CH_IRQ_HANDLER(CAN1_TX_IRQHandler)
+    CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler);
+    CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler)
     {
         CH_IRQ_PROLOGUE();
         handleTxInterrupt(0);
         CH_IRQ_EPILOGUE();
     }
 
-    CH_IRQ_HANDLER(CAN1_RX0_IRQHandler);
-    CH_IRQ_HANDLER(CAN1_RX0_IRQHandler)
+    CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler);
+    CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler)
     {
         CH_IRQ_PROLOGUE();
         handleRxInterrupt(0, 0);
         CH_IRQ_EPILOGUE();
     }
 
-    CH_IRQ_HANDLER(CAN1_RX1_IRQHandler);
-    CH_IRQ_HANDLER(CAN1_RX1_IRQHandler)
+    CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler);
+    CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler)
     {
         CH_IRQ_PROLOGUE();
         handleRxInterrupt(0, 1);
@@ -930,36 +960,36 @@ extern "C"
 
 #if HAL_NUM_CAN_IFACES > 1
 
-#if !defined(CAN2_TX_IRQHandler)
+#if !defined(CAN2_TX_IRQ_Handler)
 # error "Misconfigured build1"
 #endif
 
-#if !defined(CAN2_RX0_IRQHandler)
+#if !defined(CAN2_RX0_IRQ_Handler)
 # error "Misconfigured build2"
 #endif
 
-#if !defined(CAN2_RX1_IRQHandler)
+#if !defined(CAN2_RX1_IRQ_Handler)
 # error "Misconfigured build3"
 #endif
 
-    CH_IRQ_HANDLER(CAN2_TX_IRQHandler);
-    CH_IRQ_HANDLER(CAN2_TX_IRQHandler)
+    CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler);
+    CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler)
     {
         CH_IRQ_PROLOGUE();
         handleTxInterrupt(1);
         CH_IRQ_EPILOGUE();
     }
 
-    CH_IRQ_HANDLER(CAN2_RX0_IRQHandler);
-    CH_IRQ_HANDLER(CAN2_RX0_IRQHandler)
+    CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler);
+    CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler)
     {
         CH_IRQ_PROLOGUE();
         handleRxInterrupt(1, 0);
         CH_IRQ_EPILOGUE();
     }
 
-    CH_IRQ_HANDLER(CAN2_RX1_IRQHandler);
-    CH_IRQ_HANDLER(CAN2_RX1_IRQHandler)
+    CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler);
+    CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler)
     {
         CH_IRQ_PROLOGUE();
         handleRxInterrupt(1, 1);

libraries/AP_HAL_ChibiOS/EventSource.cpp

@@ -1,5 +1,6 @@
 #include "EventSource.h"
-#include <AP_HAL/AP_HAL.h>
+
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
 
 using namespace ChibiOS;
 
@@ -31,4 +32,5 @@ void EventSource::signalI(uint32_t evt_mask)
     ch_evt_src_.broadcastFlagsI(evt_mask);
     chSysUnlockFromISR();
 }
-#endif
+#endif //#if CH_CFG_USE_EVENTS == TRUE
+#endif //#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)

libraries/AP_HAL_ChibiOS/EventSource.h

@@ -1,5 +1,7 @@
 #pragma once
+#include <AP_HAL/AP_HAL.h>
 
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
 #include <stdint.h>
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <AP_HAL/AP_HAL_Macros.h>
@@ -22,4 +24,5 @@ public:
     // Wait on an Event handle, method for internal use by EventHandle
     bool wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) override;
 };
-#endif
+#endif //#if CH_CFG_USE_EVENTS == TRUE
+#endif //#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)

libraries/AP_HAL_ChibiOS/stdio.cpp

@@ -34,7 +34,9 @@
 #include "UARTDriver.h"
 #endif
 
+#ifndef HAL_BOOTLOADER_BUILD
 extern const AP_HAL::HAL& hal;
+#endif
 
 int __wrap_snprintf(char *str, size_t size, const char *fmt, ...)
 {
@@ -42,7 +44,11 @@ int __wrap_snprintf(char *str, size_t size, const char *fmt, ...)
    int done;
 
    va_start (arg, fmt);
+#ifdef HAL_BOOTLOADER_BUILD
+   done = chvsnprintf(str, size, fmt, arg);
+#else
    done =  hal.util->vsnprintf(str, size, fmt, arg);
+#endif
    va_end (arg);
 
    return done;
@@ -85,7 +91,7 @@ int __wrap_vprintf(const char *fmt, va_list arg)
 {
 #ifdef HAL_STDOUT_SERIAL
   return chvprintf((BaseSequentialStream*)&HAL_STDOUT_SERIAL, fmt, arg);
-#elif HAL_USE_SERIAL_USB == TRUE
+#elif HAL_USE_SERIAL_USB == TRUE && !defined(HAL_BOOTLOADER_BUILD)
   usb_initialise();
   return chvprintf((BaseSequentialStream*)&SDU1, fmt, arg);
 #else

libraries/AP_HAL_ChibiOS/system.cpp

@@ -217,6 +217,7 @@ void init()
 
 void panic(const char *errormsg, ...)
 {
+#ifndef HAL_BOOTLOADER_BUILD
     va_list ap;
 
     va_start(ap, errormsg);
@@ -228,6 +229,12 @@ void panic(const char *errormsg, ...)
         vprintf(errormsg, ap);
         hal.scheduler->delay(500);
     }
+#else
+    // we don't support variable args in bootlaoder
+    chSysHalt(errormsg);
+    // we will never get here, this just to silence a warning
+    while (1) {}
+#endif
 }
 
 uint32_t micros()