From 27b202e9847266a65e7d04a4e497f0b8a528a340 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Mon, 16 Sep 2024 14:20:02 +1000
Subject: [PATCH] AP_Periph: make can broadcast threadsafe, like can_printf
 call from lua thread

---
 Tools/AP_Periph/AP_Periph.h |  2 ++
 Tools/AP_Periph/can.cpp     | 12 ++++++++----
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index d5f7344589..dc3f6754e4 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -583,6 +583,8 @@ public:
 #if AP_AHRS_ENABLED
     AP_AHRS ahrs;
 #endif
+
+    HAL_Semaphore canard_broadcast_semaphore;
 };
 
 #ifndef CAN_APP_NODE_NAME
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 0a184888cb..55ceefc9d8 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1055,6 +1055,7 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
                                     uint16_t payload_len,
                                     uint8_t iface_mask)
 {
+    WITH_SEMAPHORE(canard_broadcast_semaphore);
     const bool is_dna = data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID;
     if (!is_dna && canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) {
         return false;
@@ -1939,12 +1940,15 @@ void AP_Periph_FW::can_update()
         // allow for user enabling/disabling CANFD at runtime
         dronecan.canard.tao_disabled = g.can_fdmode == 1;
 #endif
-
-        processTx();
-        processRx();
+        {
+            WITH_SEMAPHORE(canard_broadcast_semaphore);
+            processTx();
+            processRx();
 #if HAL_PERIPH_CAN_MIRROR
-        processMirror();
+            processMirror();
 #endif // HAL_PERIPH_CAN_MIRROR
+
+        }
     }
 }