From 8219b50023d9f4d5cadcf1b22c0f79d52ddd407e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 9 Feb 2023 09:17:45 +1100
Subject: [PATCH] AP_CANManager: add an output buffer for MAVCAN

this fixes firmware update of peripheral nodes using MAVCAN
---
 libraries/AP_CANManager/AP_CANManager.cpp | 67 +++++++++++++++++++++--
 libraries/AP_CANManager/AP_CANManager.h   | 12 +++-
 2 files changed, 72 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp
index 29221d0465..2bb83fa7cf 100644
--- a/libraries/AP_CANManager/AP_CANManager.cpp
+++ b/libraries/AP_CANManager/AP_CANManager.cpp
@@ -416,9 +416,29 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
 /*
   handle a CAN_FRAME packet
  */
-void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
+void AP_CANManager::handle_can_frame(const mavlink_message_t &msg)
 {
-    const uint16_t timeout_us = 2000;
+    if (frame_buffer == nullptr) {
+        // allocate frame buffer
+        WITH_SEMAPHORE(_sem);
+        // 20 is good for firmware upload
+        uint8_t buffer_size = 20;
+        while (frame_buffer == nullptr && buffer_size > 0) {
+            // we'd like 20 frames, but will live with less
+            frame_buffer = new ObjectBuffer<BufferFrame>(buffer_size);
+            if (frame_buffer != nullptr) {
+                // register a callback for when frames can't be sent immediately
+                hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::process_frame_buffer, void));
+                break;
+            }
+            buffer_size /= 2;
+        }
+        if (frame_buffer == nullptr) {
+            // disard the frames
+            return;
+        }
+    }
+
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_CAN_FRAME: {
         mavlink_can_frame_t p;
@@ -426,8 +446,12 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
         if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
             return;
         }
-        AP_HAL::CANFrame frame{p.id, p.data, p.len};
-        hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
+        struct BufferFrame frame {
+            bus : p.bus,
+            frame : AP_HAL::CANFrame(p.id, p.data, p.len)
+        };
+        WITH_SEMAPHORE(_sem);
+        frame_buffer->push(frame);
         break;
     }
     case MAVLINK_MSG_ID_CANFD_FRAME: {
@@ -436,11 +460,42 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
         if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
             return;
         }
-        AP_HAL::CANFrame frame{p.id, p.data, p.len, true};
-        hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
+        struct BufferFrame frame {
+            bus : p.bus,
+            frame : AP_HAL::CANFrame(p.id, p.data, p.len, true)
+        };
+        WITH_SEMAPHORE(_sem);
+        frame_buffer->push(frame);
         break;
     }
     }
+    process_frame_buffer();
+}
+
+/*
+  process the frame buffer
+ */
+void AP_CANManager::process_frame_buffer(void)
+{
+    while (frame_buffer) {
+        WITH_SEMAPHORE(_sem);
+        struct BufferFrame frame;
+        const uint16_t timeout_us = 2000;
+        if (!frame_buffer->peek(frame)) {
+            // no frames in the queue
+            break;
+        }
+        const int16_t retcode = hal.can[frame.bus]->send(frame.frame,
+                                                         AP_HAL::native_micros64() + timeout_us,
+                                                         frame.frame.isCanFDFrame()?AP_HAL::CANIface::IsMAVCAN:0);
+        if (retcode == 0) {
+            // no space in the CAN output slots, try again later
+            break;
+        }
+        // retcode == 1 means sent, -1 means a frame that can't be
+        // sent. Either way we should remove from the queue
+        frame_buffer->pop();
+    }
 }
 
 /*
diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h
index ac04afc49d..44cab3fe39 100644
--- a/libraries/AP_CANManager/AP_CANManager.h
+++ b/libraries/AP_CANManager/AP_CANManager.h
@@ -25,6 +25,7 @@
 #include "AP_SLCANIface.h"
 #include "AP_CANDriver.h"
 #include <GCS_MAVLink/GCS.h>
+#include <AP_HAL/utility/RingBuffer.h>
 
 class AP_CANManager
 {
@@ -111,7 +112,7 @@ public:
 
 #if HAL_GCS_ENABLED
     bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_long_t &packet, const mavlink_message_t &msg);
-    void handle_can_frame(const mavlink_message_t &msg) const;
+    void handle_can_frame(const mavlink_message_t &msg);
     void handle_can_filter_modify(const mavlink_message_t &msg);
 #endif
 
@@ -188,6 +189,15 @@ private:
         uint16_t num_filter_ids;
         uint16_t *filter_ids;
     } can_forward;
+
+    // buffer for MAVCAN frames
+    struct BufferFrame {
+        uint8_t bus;
+        AP_HAL::CANFrame frame;
+    };
+    ObjectBuffer<BufferFrame> *frame_buffer;
+
+    void process_frame_buffer(void);
 #endif // HAL_GCS_ENABLED
 };