From 710b9ea956dcfcf292884a03e7d6cc00bf23b68c Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Fri, 20 Dec 2024 14:51:51 +1100
Subject: [PATCH] AP_HAL: add option to disable rx callback for CAN iface

---
 libraries/AP_HAL/CANIface.cpp | 7 ++++---
 libraries/AP_HAL/CANIface.h   | 9 +++++----
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp
index 74fa742f9cb77..2d13aa4e0815c 100644
--- a/libraries/AP_HAL/CANIface.cpp
+++ b/libraries/AP_HAL/CANIface.cpp
@@ -64,9 +64,9 @@ int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotoni
 #ifndef HAL_BOOTLOADER_BUILD
     WITH_SEMAPHORE(callbacks.sem);
 #endif
-    for (auto &cb : callbacks.cb) {
-        if (cb != nullptr) {
-            cb(get_iface_num(), out_frame);
+    for (uint8_t i=0; i<ARRAY_SIZE(callbacks.cb); i++) {
+        if (callbacks.cb[i] != nullptr && !callbacks.rx_cb_disable[i]) {
+            callbacks.cb[i](get_iface_num(), out_frame);
         }
     }
     return 1;
@@ -129,6 +129,7 @@ void AP_HAL::CANIface::unregister_frame_callback(uint8_t callback_id)
     const uint8_t idx = callback_id - 1;
     if (idx < ARRAY_SIZE(callbacks.cb)) {
         callbacks.cb[idx] = nullptr;
+        callbacks.rx_cb_disable[idx] = false;
     }
 }
 
diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h
index 629432bd1fac1..cbde2c132fe19 100644
--- a/libraries/AP_HAL/CANIface.h
+++ b/libraries/AP_HAL/CANIface.h
@@ -262,19 +262,20 @@ class AP_HAL::CANIface
     FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
 
     // register a frame callback function
-    virtual bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
-    virtual void unregister_frame_callback(uint8_t cb_id);
+    bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
+    void unregister_frame_callback(uint8_t cb_id);
+    virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
+    void set_rx_cb_disabled(uint8_t cb_id, bool rx_cb_disable) { callbacks.rx_cb_disable[cb_id-1] = rx_cb_disable; }
 
 protected:
     virtual int8_t get_iface_num() const = 0;
-    virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
-
     struct {
 #ifndef HAL_BOOTLOADER_BUILD
         HAL_Semaphore sem;
 #endif
         // allow up to 3 callbacks per interface
         FrameCb cb[3];
+        bool rx_cb_disable[3];
     } callbacks;
 
     uint32_t bitrate_;