From ef4fce80fbc4ecbce18e386fea558efd4077348b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 8 Sep 2024 14:20:26 +1000
Subject: [PATCH] AP_Bootloader: support CAN multicast server

allows for fw update of network peripheral devices
---
 Tools/AP_Bootloader/can.cpp     | 30 +++++++++++++++++++++++++-----
 Tools/AP_Bootloader/can.h       |  4 ++++
 Tools/AP_Bootloader/network.cpp |  7 +++++++
 Tools/AP_Bootloader/support.cpp | 11 +++++++++++
 Tools/AP_Bootloader/support.h   |  1 +
 5 files changed, 48 insertions(+), 5 deletions(-)

diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp
index 5e8515dc71..0ee6ee7f98 100644
--- a/Tools/AP_Bootloader/can.cpp
+++ b/Tools/AP_Bootloader/can.cpp
@@ -50,7 +50,7 @@ static CANConfig cancfg = {
 // pipelining is not faster when using ChibiOS CAN driver
 #define FW_UPDATE_PIPELINE_LEN 1
 #else
-static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
+ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
 #endif
 
 #ifndef CAN_APP_VERSION_MAJOR
@@ -863,16 +863,13 @@ void can_update()
 }
 
 // printf to CAN LogMessage for debugging
-void can_printf(const char *fmt, ...)
+void can_vprintf(const char *fmt, va_list ap)
 {
     // only on H7 for now, where we have plenty of flash
 #if defined(STM32H7)
     uavcan_protocol_debug_LogMessage pkt {};
     uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE];
-    va_list ap;
-    va_start(ap, fmt);
     uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap);
-    va_end(ap);
     pkt.text.len = MIN(n, sizeof(pkt.text.data));
 
     uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, true);
@@ -887,5 +884,28 @@ void can_printf(const char *fmt, ...)
 #endif // defined(STM32H7)
 }
 
+// printf to CAN LogMessage for debugging
+void can_printf(const char *fmt, ...)
+{
+    // only on H7 for now, where we have plenty of flash
+#if defined(STM32H7)
+    va_list ap;
+    va_start(ap, fmt);
+    can_vprintf(fmt, ap);
+    va_end(ap);
+#endif // defined(STM32H7)
+}
+
+void can_printf_severity(uint8_t severity, const char *fmt, ...)
+{
+    // only on H7 for now, where we have plenty of flash
+#if defined(STM32H7)
+    va_list ap;
+    va_start(ap, fmt);
+    can_vprintf(fmt, ap);
+    va_end(ap);
+#endif
+}
+
 
 #endif // HAL_USE_CAN
diff --git a/Tools/AP_Bootloader/can.h b/Tools/AP_Bootloader/can.h
index 8f3945ed18..837536b12b 100644
--- a/Tools/AP_Bootloader/can.h
+++ b/Tools/AP_Bootloader/can.h
@@ -2,4 +2,8 @@ void can_start();
 void can_update();
 void can_set_node_id(uint8_t node_id);
 bool can_check_update(void);
+extern "C" {
+void can_vprintf(const char *fmt, va_list ap);
 void can_printf(const char *fmt, ...);
+void can_printf_severity(uint8_t severity, const char *fmt, ...);
+};
diff --git a/Tools/AP_Bootloader/network.cpp b/Tools/AP_Bootloader/network.cpp
index 64270187a8..1d7a312227 100644
--- a/Tools/AP_Bootloader/network.cpp
+++ b/Tools/AP_Bootloader/network.cpp
@@ -8,6 +8,7 @@
 
 #include <AP_Networking/AP_Networking.h>
 #include <AP_Networking/AP_Networking_ChibiOS.h>
+#include <AP_Networking/AP_Networking_CAN.h>
 
 #include <lwip/ip_addr.h>
 #include <lwip/tcpip.h>
@@ -29,6 +30,7 @@
 #include <stdio.h>
 #include <stdarg.h>
 #include <AP_HAL_ChibiOS/hwdef/common/flash.h>
+#include <AP_HAL_ChibiOS/CANIface.h>
 
 #ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR
 #define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46"
@@ -59,6 +61,8 @@
 
 #define MIN(a,b) ((a)<(b)?(a):(b))
 
+static AP_Networking_CAN mcast_server;
+
 void BL_Network::link_up_cb(void *p)
 {
     auto *driver = (BL_Network *)p;
@@ -67,6 +71,9 @@ void BL_Network::link_up_cb(void *p)
 #endif
     char ipstr[IP4_STR_LEN];
     can_printf("IP %s", SocketAPM::inet_addr_to_str(ntohl(driver->thisif->ip_addr.addr), ipstr, sizeof(ipstr)));
+
+    // start mcast CAN server
+    mcast_server.start((1U<<HAL_NUM_CAN_IFACES)-1);
 }
 
 void BL_Network::link_down_cb(void *p)
diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp
index 55e33f6bc7..cc720919bd 100644
--- a/Tools/AP_Bootloader/support.cpp
+++ b/Tools/AP_Bootloader/support.cpp
@@ -364,6 +364,17 @@ void thread_sleep_ms(uint32_t ms)
     }
 }
 
+void thread_sleep_us(uint32_t us)
+{
+    while (us > 0) {
+        // don't sleep more than 65 at a time, to cope with 16 bit
+        // timer
+        const uint32_t dt = us > 6500? 6500: us;
+        chThdSleepMicroseconds(dt);
+        us -= dt;
+    }
+}
+
 // generate a pulse sequence forever, for debugging
 void led_pulses(uint8_t npulses)
 {
diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h
index 8928ff9442..15b19c0030 100644
--- a/Tools/AP_Bootloader/support.h
+++ b/Tools/AP_Bootloader/support.h
@@ -48,6 +48,7 @@ void led_off(unsigned led);
 void led_toggle(unsigned led);
 
 void thread_sleep_ms(uint32_t ms);
+void thread_sleep_us(uint32_t us);
 
 void custom_startup(void);