From 4a102e2f2be3f977840dd85050c74a1c4ea03ba2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 8 Sep 2024 16:11:30 +1000
Subject: [PATCH] AP_HAL: support more than 1 registered CAN callback

this allows for CAN MCAST and MAVCAN at the same time
---
 libraries/AP_HAL/CANIface.cpp | 58 +++++++++++++++++++++++++++++------
 libraries/AP_HAL/CANIface.h   | 12 ++++++--
 2 files changed, 58 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp
index 7bd1a8ebbd..74fa742f9c 100644
--- a/libraries/AP_HAL/CANIface.cpp
+++ b/libraries/AP_HAL/CANIface.cpp
@@ -58,9 +58,16 @@ bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const
  */
 int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags)
 {
-    auto cb = frame_callback;
-    if (cb && (out_flags & IsMAVCAN)==0) {
-        cb(get_iface_num(), out_frame);
+    if ((out_flags & IsMAVCAN) != 0) {
+        return 1;
+    }
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    for (auto &cb : callbacks.cb) {
+        if (cb != nullptr) {
+            cb(get_iface_num(), out_frame);
+        }
     }
     return 1;
 }
@@ -70,28 +77,59 @@ int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotoni
  */
 int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags)
 {
-    auto cb = frame_callback;
-    if (cb) {
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    bool added_to_rx_queue = false;
+    for (auto &cb : callbacks.cb) {
+        if (cb == nullptr) {
+            continue;
+        }
         if ((flags & IsMAVCAN) == 0) {
             cb(get_iface_num(), frame);
-        } else {
+        } else if (!added_to_rx_queue) {
             CanRxItem rx_item;
             rx_item.frame = frame;
             rx_item.timestamp_us = AP_HAL::micros64();
             rx_item.flags = AP_HAL::CANIface::IsMAVCAN;
             add_to_rx_queue(rx_item);
+            added_to_rx_queue = true;
         }
     }
     return 1;
 }
 
 /*
-  register a callback for for sending CAN_FRAME messages
+  register a callback for for sending CAN_FRAME messages.
+  On success the returned callback_id can be used to unregister the callback
  */
-bool AP_HAL::CANIface::register_frame_callback(FrameCb cb)
+bool AP_HAL::CANIface::register_frame_callback(FrameCb cb, uint8_t &callback_id)
 {
-    frame_callback = cb;
-    return true;
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    for (uint8_t i=0; i<ARRAY_SIZE(callbacks.cb); i++) {
+        if (callbacks.cb[i] == nullptr) {
+            callbacks.cb[i] = cb;
+            callback_id = i+1;
+            return true;
+        }
+    }
+    return false;
+}
+
+/*
+  unregister a callback for for sending CAN_FRAME messages
+ */
+void AP_HAL::CANIface::unregister_frame_callback(uint8_t callback_id)
+{
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    const uint8_t idx = callback_id - 1;
+    if (idx < ARRAY_SIZE(callbacks.cb)) {
+        callbacks.cb[idx] = nullptr;
+    }
 }
 
 AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) :
diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h
index 1b2e88fb33..b92274a6a5 100644
--- a/libraries/AP_HAL/CANIface.h
+++ b/libraries/AP_HAL/CANIface.h
@@ -262,13 +262,21 @@ public:
     FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
 
     // register a frame callback function
-    virtual bool register_frame_callback(FrameCb cb);
+    virtual bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
+    virtual void unregister_frame_callback(uint8_t cb_id);
 
 protected:
     virtual int8_t get_iface_num() const = 0;
     virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
 
-    FrameCb frame_callback;
+    struct {
+#ifndef HAL_BOOTLOADER_BUILD
+        HAL_Semaphore sem;
+#endif
+        // allow up to 2 callbacks per interface
+        FrameCb cb[2];
+    } callbacks;
+
     uint32_t bitrate_;
     OperatingMode mode_;
 };