Create separate option for bridging mcast and hw can

libraries/AP_HAL/CANIface.cpp

@@ -64,9 +64,9 @@ int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotoni
 #ifndef HAL_BOOTLOADER_BUILD
     WITH_SEMAPHORE(callbacks.sem);
 #endif
-    for (auto &cb : callbacks.cb) {
-        if (cb != nullptr) {
-            cb(get_iface_num(), out_frame);
+    for (uint8_t i=0; i<ARRAY_SIZE(callbacks.cb); i++) {
+        if (callbacks.cb[i] != nullptr && !callbacks.rx_cb_disable[i]) {
+            callbacks.cb[i](get_iface_num(), out_frame);
         }
     }
     return 1;
@@ -129,6 +129,7 @@ void AP_HAL::CANIface::unregister_frame_callback(uint8_t callback_id)
     const uint8_t idx = callback_id - 1;
     if (idx < ARRAY_SIZE(callbacks.cb)) {
         callbacks.cb[idx] = nullptr;
+        callbacks.rx_cb_disable[idx] = false;
     }
 }
 

libraries/AP_HAL/CANIface.h

@@ -262,19 +262,20 @@ class AP_HAL::CANIface
     FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
 
     // register a frame callback function
-    virtual bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
-    virtual void unregister_frame_callback(uint8_t cb_id);
+    bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
+    void unregister_frame_callback(uint8_t cb_id);
+    virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
+    void set_rx_cb_disabled(uint8_t cb_id, bool rx_cb_disable) { callbacks.rx_cb_disable[cb_id-1] = rx_cb_disable; }
 
 protected:
     virtual int8_t get_iface_num() const = 0;
-    virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
-
     struct {
 #ifndef HAL_BOOTLOADER_BUILD
         HAL_Semaphore sem;
 #endif
         // allow up to 3 callbacks per interface
         FrameCb cb[3];
+        bool rx_cb_disable[3];
     } callbacks;
 
     uint32_t bitrate_;

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);

libraries/AP_Networking/AP_Networking.h

@@ -157,14 +157,23 @@ 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;
     }
 
+#if AP_NETWORKING_CAN_MCAST_ENABLED
+    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);
+    }
+#endif
+
 private:
     static AP_Networking *singleton;
 

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;
                 }

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