From 89c2b4828662ece84f48bdf9aa24db688fea36cb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 22 Sep 2024 07:58:21 +1000
Subject: [PATCH] AP_CANManager: ensure we only remove our own fwd
 registrations

keep a record of which bus we have registered a callback for and only
unregister with that bus. This prevents us unregistering a multicast
callback when disconnecting from MAVCAN
---
 libraries/AP_CANManager/AP_CANManager.cpp | 34 ++++++++++++++---------
 libraries/AP_CANManager/AP_CANManager.h   |  1 +
 2 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp
index dfb9dbc863..ebf574df99 100644
--- a/libraries/AP_CANManager/AP_CANManager.cpp
+++ b/libraries/AP_CANManager/AP_CANManager.cpp
@@ -426,39 +426,43 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
 {
     WITH_SEMAPHORE(can_forward.sem);
     const int8_t bus = int8_t(packet.param1)-1;
+
     if (bus == -1) {
         /*
           a request to stop forwarding
          */
-        for (auto can_iface : hal.can) {
-            if (can_iface && can_forward.callback_id != 0) {
-                can_iface->unregister_frame_callback(can_forward.callback_id);
-                can_forward.callback_id = 0;
-            }
+        if (can_forward.callback_id != 0) {
+            hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id);
+            can_forward.callback_id = 0;
         }
         return true;
     }
+
     if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) {
         return false;
     }
+
+    if (can_forward.callback_id != 0 && can_forward.callback_bus != bus) {
+        /*
+          the client is changing which bus they are monitoring, unregister from the previous bus
+         */
+        hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id);
+        can_forward.callback_id = 0;
+    }
+
     if (can_forward.callback_id == 0 &&
         !hal.can[bus]->register_frame_callback(
             FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &), can_forward.callback_id)) {
+        // failed to register the callback
         return false;
     }
+
+    can_forward.callback_bus = bus;
     can_forward.last_callback_enable_ms = AP_HAL::millis();
     can_forward.chan = chan;
     can_forward.system_id = msg.sysid;
     can_forward.component_id = msg.compid;
 
-    // remove registration on other buses, allowing for bus change in the GUI tool
-    for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
-        if (i != bus && hal.can[i] != nullptr && can_forward.callback_id != 0) {
-            hal.can[i]->unregister_frame_callback(can_forward.callback_id);
-            can_forward.callback_id = 0;
-        }
-    }
-
     return true;
 }
 
@@ -645,6 +649,10 @@ void AP_CANManager::handle_can_filter_modify(const mavlink_message_t &msg)
 void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
 {
     WITH_SEMAPHORE(can_forward.sem);
+    if (bus != can_forward.callback_bus) {
+        // we are not registered for forwarding this bus, discard frame
+        return;
+    }
     if (can_forward.frame_counter++ == 100) {
         // check every 100 frames for disabling CAN_FRAME send
         // we stop sending after 5s if the client stops
diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h
index 532b213b4d..f820d83174 100644
--- a/libraries/AP_CANManager/AP_CANManager.h
+++ b/libraries/AP_CANManager/AP_CANManager.h
@@ -186,6 +186,7 @@ private:
         uint16_t num_filter_ids;
         uint16_t *filter_ids;
         uint8_t callback_id;
+        uint8_t callback_bus;
     } can_forward;
 
     // buffer for MAVCAN frames