AP_Networking: make can multicast an endpoint by default

also add option to enable multicast with bridging to CAN bus in application and disabled in bootloader
ArduPilot · Dec 20, 2024 · 1bff293 · 1bff293
1 parent f0dbca0
commit 1bff293
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 16 deletions.
diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp
@@ -90,7 +90,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Networking options
     // @Description: Networking options
-    // @Bitmask: 0:EnablePPP Ethernet gateway, 1:Enable CAN1 multicast gateway, 2:Enable CAN2 multicast gateway
+    // @Bitmask: 0:EnablePPP Ethernet gateway, 1:Enable CAN1 multicast endpoint, 2:Enable CAN2 multicast endpoint, 3:Enable CAN1 multicast bridged, 4:Enable CAN2 multicast bridged
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 9,  AP_Networking,    param.options, 0),
@@ -200,13 +200,13 @@ void AP_Networking::init()
 #endif
 
 #if AP_NETWORKING_CAN_MCAST_ENABLED && !defined(HAL_BOOTLOADER_BUILD)
-    if (option_is_set(OPTION::CAN1_MCAST_GATEWAY) || option_is_set(OPTION::CAN2_MCAST_GATEWAY)) {
+    if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT) || option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) {
         // get mask of enabled buses
         uint8_t bus_mask = 0;
-        if (option_is_set(OPTION::CAN1_MCAST_GATEWAY)) {
+        if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) {
             bus_mask |= (1U<<0);
         }
-        if (option_is_set(OPTION::CAN2_MCAST_GATEWAY)) {
+        if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) {
             bus_mask |= (1U<<1);
         }
         mcast_server.start(bus_mask);

diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h
@@ -157,14 +157,21 @@ class AP_Networking
     enum class OPTION {
         PPP_ETHERNET_GATEWAY=(1U<<0),
 #if AP_NETWORKING_CAN_MCAST_ENABLED
-        CAN1_MCAST_GATEWAY=(1U<<1),
-        CAN2_MCAST_GATEWAY=(1U<<2),
+        CAN1_MCAST_ENDPOINT=(1U<<1),
+        CAN2_MCAST_ENDPOINT=(1U<<2),
+        CAN1_MCAST_BRIDGED=(1U<<3),
+        CAN2_MCAST_BRIDGED=(1U<<4),
 #endif
     };
     bool option_is_set(OPTION option) const {
         return (param.options.get() & int32_t(option)) != 0;
     }
 
+    bool is_can_mcast_ep_bridged(uint8_t bus) const {
+        return (option_is_set(OPTION::CAN1_MCAST_BRIDGED) && bus == 0) ||
+               (option_is_set(OPTION::CAN2_MCAST_BRIDGED) && bus == 1);
+    }
+
 private:
     static AP_Networking *singleton;
 

diff --git a/libraries/AP_Networking/AP_Networking_CAN.cpp b/libraries/AP_Networking/AP_Networking_CAN.cpp
@@ -108,7 +108,6 @@ void AP_Networking_CAN::mcast_server(void)
         }
         char address[] = MCAST_ADDRESS_BASE;
         const uint32_t buffer_size = 20; // good for fw upload
-        uint8_t callback_id = 0;
 
         address[strlen(address)-1] = '0' + bus;
         if (!mcast_sockets[bus]->connect(address, MCAST_PORT)) {
@@ -122,6 +121,14 @@ void AP_Networking_CAN::mcast_server(void)
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to register", unsigned(bus));
             goto de_allocate;
         }
+#ifndef HAL_BOOTLOADER_BUILD
+        // check if bridged mode is enabled
+        cbus->set_rx_cb_disabled(callback_id, !AP::network().is_can_mcast_ep_bridged(bus));
+#else
+        // never bridge in bootloader, as we can cause loops if multiple
+        // bootloaders with mcast are running on the same network
+        cbus->set_rx_cb_disabled(callback_id, true);
+#endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
         // tell the ethernet interface that we want to receive all
@@ -151,7 +158,8 @@ void AP_Networking_CAN::mcast_server(void)
         thread_sleep_us(delay_us);
 #endif
         for (uint8_t bus=0; bus<HAL_NUM_CAN_IFACES; bus++) {
-            if (mcast_sockets[bus] == nullptr) {
+            auto *cbus = get_caniface(bus);
+            if (mcast_sockets[bus] == nullptr || cbus == nullptr) {
                 continue;
             }
 
@@ -180,15 +188,28 @@ void AP_Networking_CAN::mcast_server(void)
             */
             AP_HAL::CANFrame frame;
             const uint16_t timeout_us = 2000;
-
+#ifndef HAL_BOOTLOADER_BUILD
+            // check if bridged mode is enabled
+            bool bridged = AP::network().is_can_mcast_ep_bridged(bus);
+            cbus->set_rx_cb_disabled(callback_id, !bridged);
+#else
+            // never bridge in bootloader, as we can cause loops if multiple
+            // bootloaders with mcast are running on the same network and CAN Bus
+            bool bridged = false;
+#endif
             while (frame_buffers[bus]->peek(frame)) {
-                auto *cbus = get_caniface(bus);
-                if (cbus == nullptr) {
-                    break;
+                bool retcode;
+                if (!bridged) {
+                    AP_HAL::CANIface::CanRxItem rx_item;
+                    rx_item.timestamp_us = AP_HAL::micros64();
+                    rx_item.flags = AP_HAL::CANIface::IsMAVCAN;
+                    rx_item.frame = frame;
+                    retcode = cbus->add_to_rx_queue(rx_item);
+                } else {
+                    retcode = cbus->send(frame,
+                                    AP_HAL::micros64() + timeout_us,
+                                    AP_HAL::CANIface::IsMAVCAN);
                 }
-                auto retcode = cbus->send(frame,
-                                                      AP_HAL::micros64() + timeout_us,
-                                                      AP_HAL::CANIface::IsMAVCAN);
                 if (retcode == 0) {
                     break;
                 }

diff --git a/libraries/AP_Networking/AP_Networking_CAN.h b/libraries/AP_Networking/AP_Networking_CAN.h
@@ -15,7 +15,7 @@ class AP_Networking_CAN {
     uint8_t bus_mask;
 
     AP_HAL::CANIface *get_caniface(uint8_t) const;
-
+    uint8_t callback_id;
 #ifdef HAL_BOOTLOADER_BUILD
     static void mcast_trampoline(void *ctx);
 #endif