From 124c9d5d9ba0ebc0f932536251c936cc5ee91126 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 7 Feb 2022 08:22:53 +1100
Subject: [PATCH] HAL_ChibiOS: support mavcan message forwarding

---
 libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 10 ++++++----
 libraries/AP_HAL_ChibiOS/CANFDIface.h   |  9 +++++++++
 libraries/AP_HAL_ChibiOS/CANIface.h     |  9 +++++++++
 libraries/AP_HAL_ChibiOS/CanIface.cpp   | 10 ++++++----
 4 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
index 471b81da7a..b825a16b40 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
@@ -378,7 +378,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
     pending_tx_[index].aborted        = false;
     pending_tx_[index].setup          = true;
     pending_tx_[index].pushed         = false;
-    return 1;
+
+    return AP_HAL::CANIface::send(frame, tx_deadline, flags);
 }
 
 int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
@@ -391,7 +392,8 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
     out_frame    = rx_item.frame;
     out_timestamp_us = rx_item.timestamp_us;
     out_flags    = rx_item.flags;
-    return 1;
+
+    return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
 }
 
 bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
@@ -761,7 +763,7 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
                 rx_item.frame = pending_tx_[i].frame;
                 rx_item.timestamp_us = timestamp_us;
                 rx_item.flags = AP_HAL::CANIface::Loopback;
-                rx_queue_.push(rx_item);
+                add_to_rx_queue(rx_item);
             }
             if (event_handle_ != nullptr) {
                 stats.num_events++;
@@ -855,7 +857,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index)
     rx_item.frame = frame;
     rx_item.timestamp_us = timestamp_us;
     rx_item.flags = 0;
-    if (rx_queue_.push(rx_item)) {
+    if (add_to_rx_queue(rx_item)) {
         stats.rx_received++;
     } else {
         stats.rx_overflow++;
diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h
index 25cc3b0539..b133c4cc26 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.h
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h
@@ -240,6 +240,15 @@ public:
     // CAN Peripheral register structure, pointing at base
     // register. Indexed by locical interface number
     static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
+
+protected:
+    bool add_to_rx_queue(const CanRxItem &rx_item) override {
+        return rx_queue_.push(rx_item);
+    }
+
+    int8_t get_iface_num(void) const override {
+        return self_index_;
+    }
 };
 
 
diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h
index 5c652af9a3..8a21f85441 100644
--- a/libraries/AP_HAL_ChibiOS/CANIface.h
+++ b/libraries/AP_HAL_ChibiOS/CANIface.h
@@ -238,6 +238,15 @@ public:
 
     // CAN Peripheral register structure
     static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
+
+protected:
+    bool add_to_rx_queue(const CanRxItem &rx_item) override {
+        return rx_queue_.push(rx_item);
+    }
+
+    int8_t get_iface_num(void) const override {
+        return self_index_;
+    }
 };
 #endif //HAL_NUM_CAN_IFACES
 #endif //# if defined(STM32H7XX) || defined(STM32G4)
diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp
index 3ad9bde5a1..44386b8c73 100644
--- a/libraries/AP_HAL_ChibiOS/CanIface.cpp
+++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp
@@ -362,7 +362,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
     txi.abort_on_error = (flags & AbortOnError) != 0;
     // setup frame initial state
     txi.pushed         = false;
-    return 1;
+
+    return AP_HAL::CANIface::send(frame, tx_deadline, flags);
 }
 
 int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
@@ -375,7 +376,8 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
     out_frame    = rx_item.frame;
     out_timestamp_us = rx_item.timestamp_us;
     out_flags    = rx_item.flags;
-    return 1;
+
+    return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
 }
 
 #if !defined(HAL_BOOTLOADER_BUILD)
@@ -481,7 +483,7 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const
         rx_item.timestamp_us = timestamp_us;
         rx_item.flags = AP_HAL::CANIface::Loopback;
         PERF_STATS(stats.tx_loopback);
-        rx_queue_.push(rx_item);
+        add_to_rx_queue(rx_item);
     }
 
     if (txok && !txi.pushed) {
@@ -569,7 +571,7 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
     rx_item.frame = frame;
     rx_item.timestamp_us = timestamp_us;
     rx_item.flags = 0;
-    if (rx_queue_.push(rx_item)) {
+    if (add_to_rx_queue(rx_item)) {
         PERF_STATS(stats.rx_received);
     } else {
         PERF_STATS(stats.rx_overflow);