From 1bee4630cc3ad284b2c345d50e63a97109602dde Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 17 Jan 2024 09:12:42 +1100
Subject: [PATCH] AP_Bootloader: show IP as CAN debug msg in bootloader

this also saves a few bytes of flash for normal periph bootloader
---
 Tools/AP_Bootloader/can.cpp     | 79 +++++++++++++++++++++++++++------
 Tools/AP_Bootloader/can.h       |  1 +
 Tools/AP_Bootloader/network.cpp |  5 ++-
 3 files changed, 70 insertions(+), 15 deletions(-)

diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp
index 8e1aaea200..3f9947d7b4 100644
--- a/Tools/AP_Bootloader/can.cpp
+++ b/Tools/AP_Bootloader/can.cpp
@@ -75,6 +75,10 @@ static void processTx(void);
 #define FW_UPDATE_PIPELINE_LEN 4
 #endif
 
+#if CH_CFG_USE_MUTEXES == TRUE
+static HAL_Semaphore can_mutex;
+#endif
+
 static struct {
     uint32_t rtt_ms;
     uint32_t ofs;
@@ -625,6 +629,29 @@ static void processRx(void)
 }
 #endif //#if HAL_USE_CAN
 
+/*
+  wrapper around broadcast
+ */
+static void canard_broadcast(uint64_t data_type_signature,
+                             uint16_t data_type_id,
+                             uint8_t &transfer_id,
+                             uint8_t priority,
+                             const void* payload,
+                             uint16_t payload_len)
+{
+#if CH_CFG_USE_MUTEXES == TRUE
+    WITH_SEMAPHORE(can_mutex);
+#endif
+    canardBroadcast(&canard,
+                    data_type_signature,
+                    data_type_id,
+                    &transfer_id,
+                    priority,
+                    payload,
+                    payload_len);
+}
+
+
 /*
   handle waiting for a node ID
  */
@@ -663,13 +690,12 @@ static void can_handle_DNA(void)
     memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size);
 
     // Broadcasting the request
-    canardBroadcast(&canard,
-                    UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
-                    UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
-                    &node_id_allocation_transfer_id,
-                    CANARD_TRANSFER_PRIORITY_LOW,
-                    &allocation_request[0],
-                    (uint16_t) (uid_size + 1));
+    canard_broadcast(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
+                     UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
+                     node_id_allocation_transfer_id,
+                     CANARD_TRANSFER_PRIORITY_LOW,
+                     &allocation_request[0],
+                     (uint16_t) (uid_size + 1));
 
     // Preparing for timeout; if response is received, this value will be updated from the callback.
     node_id_allocation_unique_id_offset = 0;
@@ -684,13 +710,12 @@ static void send_node_status(void)
 
     static uint8_t transfer_id;  // Note that the transfer ID variable MUST BE STATIC (or heap-allocated)!
 
-    canardBroadcast(&canard,
-                    UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
-                    UAVCAN_PROTOCOL_NODESTATUS_ID,
-                    &transfer_id,
-                    CANARD_TRANSFER_PRIORITY_LOW,
-                    buffer,
-                    len);
+    canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
+                     UAVCAN_PROTOCOL_NODESTATUS_ID,
+                     transfer_id,
+                     CANARD_TRANSFER_PRIORITY_LOW,
+                     buffer,
+                     len);
 }
 
 
@@ -832,4 +857,30 @@ void can_update()
     } while (fw_update.node_id != 0);
 }
 
+// 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)
+    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);
+    static uint8_t logmsg_transfer_id;
+
+    canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
+                     UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
+                     logmsg_transfer_id,
+                     CANARD_TRANSFER_PRIORITY_LOW,
+                     buffer,
+                     len);
+#endif // defined(STM32H7)
+}
+
+
 #endif // HAL_USE_CAN
diff --git a/Tools/AP_Bootloader/can.h b/Tools/AP_Bootloader/can.h
index 96b662a072..8f3945ed18 100644
--- a/Tools/AP_Bootloader/can.h
+++ b/Tools/AP_Bootloader/can.h
@@ -2,3 +2,4 @@ void can_start();
 void can_update();
 void can_set_node_id(uint8_t node_id);
 bool can_check_update(void);
+void can_printf(const char *fmt, ...);
diff --git a/Tools/AP_Bootloader/network.cpp b/Tools/AP_Bootloader/network.cpp
index 9b6c433f2e..5fa3d25dc6 100644
--- a/Tools/AP_Bootloader/network.cpp
+++ b/Tools/AP_Bootloader/network.cpp
@@ -25,6 +25,7 @@
 #include "bl_protocol.h"
 #include <AP_CheckFirmware/AP_CheckFirmware.h>
 #include "app_comms.h"
+#include "can.h"
 
 #ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR
 #define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46"
@@ -57,10 +58,12 @@
 
 void BL_Network::link_up_cb(void *p)
 {
+    auto *driver = (BL_Network *)p;
 #if AP_BOOTLOADER_NETWORK_USE_DHCP
-    auto *driver = (AP_Networking_ChibiOS *)p;
     dhcp_start(driver->thisif);
 #endif
+    char ipstr[IP4_STR_LEN];
+    can_printf("IP %s", SocketAPM::inet_addr_to_str(ntohl(driver->thisif->ip_addr.addr), ipstr, sizeof(ipstr)));
 }
 
 void BL_Network::link_down_cb(void *p)