[Backport 4.6] Create separate option for bridging mcast and hw can

libraries/AP_CANManager/AP_CANManager.cpp

@@ -466,7 +466,7 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
 
     if (can_forward.callback_id == 0 &&
         !hal.can[bus]->register_frame_callback(
-            FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &), can_forward.callback_id)) {
+            FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags), can_forward.callback_id)) {
         // failed to register the callback
         return false;
     }
@@ -558,7 +558,7 @@ void AP_CANManager::process_frame_buffer(void)
         }
         const int16_t retcode = hal.can[frame.bus]->send(frame.frame,
                                                          AP_HAL::micros64() + timeout_us,
-                                                         AP_HAL::CANIface::IsMAVCAN);
+                                                         AP_HAL::CANIface::IsForwardedFrame);
         if (retcode == 0) {
             // no space in the CAN output slots, try again later
             break;
@@ -662,7 +662,7 @@ void AP_CANManager::handle_can_filter_modify(const mavlink_message_t &msg)
   handler for CAN frames from the registered callback, sending frames
   out as CAN_FRAME or CANFD_FRAME messages
  */
-void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
+void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags)
 {
     WITH_SEMAPHORE(can_forward.sem);
     if (bus != can_forward.callback_bus) {
@@ -721,7 +721,7 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
 /*
   handler for CAN frames for frame logging
  */
-void AP_CANManager::can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
+void AP_CANManager::can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags)
 {
 #if HAL_CANFD_SUPPORTED
     if (frame.canfd) {
@@ -762,7 +762,7 @@ void AP_CANManager::check_logging_enable(void)
         }
         if (enabled && logging_id == 0) {
             can->register_frame_callback(
-                FUNCTOR_BIND_MEMBER(&AP_CANManager::can_logging_callback, void, uint8_t, const AP_HAL::CANFrame &),
+                FUNCTOR_BIND_MEMBER(&AP_CANManager::can_logging_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags),
                 logging_id);
         } else if (!enabled && logging_id != 0) {
             can->unregister_frame_callback(logging_id);

libraries/AP_CANManager/AP_CANManager.h

@@ -187,7 +187,7 @@ class AP_CANManager
       handler for CAN frames from the registered callback, sending frames
       out as CAN_FRAME messages
     */
-    void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame);
+    void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags);
 
     struct {
         mavlink_channel_t chan;
@@ -216,7 +216,7 @@ class AP_CANManager
     /*
       handler for CAN frames for logging
     */
-    void can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame);
+    void can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags);
     void check_logging_enable(void);
 #endif
 };

libraries/AP_HAL/CANIface.cpp

@@ -54,26 +54,28 @@ bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const
 }
 
 /*
-  parent class receive handling for MAVCAN
+  parent class receive handling for forwarding received frames to registered callbacks
  */
 int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags)
 {
-    if ((out_flags & IsMAVCAN) != 0) {
+    if ((out_flags & IsForwardedFrame) != 0) {
+        // this frame was forwarded from another interface to this one, so we should not forward it back
         return 1;
     }
 #ifndef HAL_BOOTLOADER_BUILD
     WITH_SEMAPHORE(callbacks.sem);
 #endif
     for (auto &cb : callbacks.cb) {
         if (cb != nullptr) {
-            cb(get_iface_num(), out_frame);
+            // forward the frame to the registered callbacks and mark it as forwarded
+            cb(get_iface_num(), out_frame, out_flags | IsForwardedFrame);
         }
     }
     return 1;
 }
 
 /*
-  parent class send handling for MAVCAN
+  parent class send handling for Forwarded frames
  */
 int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags)
 {
@@ -85,13 +87,15 @@ int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanI
         if (cb == nullptr) {
             continue;
         }
-        if ((flags & IsMAVCAN) == 0) {
-            cb(get_iface_num(), frame);
+        if ((flags & IsForwardedFrame) == 0) {
+            // call the frame callback from send only if the frame originated from this node
+            cb(get_iface_num(), frame, flags);
         } else if (!added_to_rx_queue) {
+            // the frame was forwarded from another interface, so add it to the receive queue
             CanRxItem rx_item;
             rx_item.frame = frame;
             rx_item.timestamp_us = AP_HAL::micros64();
-            rx_item.flags = AP_HAL::CANIface::IsMAVCAN;
+            rx_item.flags = AP_HAL::CANIface::IsForwardedFrame;
             add_to_rx_queue(rx_item);
             added_to_rx_queue = true;
         }

libraries/AP_HAL/CANIface.h

@@ -125,7 +125,7 @@ class AP_HAL::CANIface
     typedef uint16_t CanIOFlags;
     static const CanIOFlags Loopback = 1;
     static const CanIOFlags AbortOnError = 2;
-    static const CanIOFlags IsMAVCAN = 4;
+    static const CanIOFlags IsForwardedFrame = 4;
 
     // Single Rx Frame with related info
     struct CanRxItem {
@@ -259,7 +259,7 @@ class AP_HAL::CANIface
     // return true if init was called and successful
     virtual bool is_initialized() const = 0;
 
-    FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
+    FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &, CanIOFlags);
 
     // register a frame callback function
     virtual bool register_frame_callback(FrameCb cb, uint8_t &cb_id);

libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h

@@ -57,3 +57,4 @@
 #define STM32_DMA_REQUIRED 1
 #endif
 
+#define AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED 0

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),
@@ -199,14 +199,14 @@ void AP_Networking::init()
     start_tests();
 #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 AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
+    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,25 @@ 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),
+#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
+        CAN1_MCAST_BRIDGED=(1U<<3),
+        CAN2_MCAST_BRIDGED=(1U<<4),
+#endif
 #endif
     };
     bool option_is_set(OPTION option) const {
         return (param.options.get() & int32_t(option)) != 0;
     }
 
+#if AP_NETWORKING_CAN_MCAST_BRIDGING_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

@@ -117,7 +117,7 @@ void AP_Networking_CAN::mcast_server(void)
         }
 
         if (!cbus->register_frame_callback(
-                FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &),
+                FUNCTOR_BIND_MEMBER(&AP_Networking_CAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags),
                 callback_id)) {
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to register", unsigned(bus));
             goto de_allocate;
@@ -180,14 +180,30 @@ void AP_Networking_CAN::mcast_server(void)
             */
             AP_HAL::CANFrame frame;
             const uint16_t timeout_us = 2000;
+#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
+            const bool bridged = AP::network().is_can_mcast_ep_bridged(bus);
+#endif
 
             while (frame_buffers[bus]->peek(frame)) {
-                auto retcode = get_caniface(bus)->send(frame,
-                                                      AP_HAL::micros64() + timeout_us,
-                                                      AP_HAL::CANIface::IsMAVCAN);
-                if (retcode == 0) {
+                auto *cbus = get_caniface(bus);
+                if (cbus == nullptr) {
                     break;
                 }
+#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
+                if (bridged) {
+                    auto retcode = cbus->send(frame, AP_HAL::micros64() + timeout_us,
+                                                 AP_HAL::CANIface::IsForwardedFrame);
+                    if (retcode == 0) {
+                        break;
+                    }
+                } else
+#endif
+                {
+                    // only call the AP_HAL::CANIface send if we are not in bridged mode
+                    cbus->AP_HAL::CANIface::send(frame, AP_HAL::micros64() + timeout_us,
+                                                 AP_HAL::CANIface::IsForwardedFrame);
+                }
+
                 // we either sent it or there was an error, either way we discard the frame
                 frame_buffers[bus]->pop();
             }
@@ -199,12 +215,27 @@ void AP_Networking_CAN::mcast_server(void)
   handler for CAN frames from the registered callback, sending frames
   out as multicast UDP
  */
-void AP_Networking_CAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
+void AP_Networking_CAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags)
 {
     if (bus >= HAL_NUM_CAN_IFACES || mcast_sockets[bus] == nullptr) {
         return;
     }
 
+#if AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
+    // check if bridged mode is enabled
+    const bool bridged = 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 and CAN Bus
+    const bool bridged = false;
+#endif
+
+    if ((flags & AP_HAL::CANIface::IsForwardedFrame) && !bridged) {
+        // we don't forward frames that we received from the multicast
+        // server if not in bridged mode
+        return;
+    }
+
     struct mcast_pkt pkt {};
     pkt.magic = MCAST_MAGIC;
     pkt.flags = 0;

libraries/AP_Networking/AP_Networking_CAN.h

@@ -9,7 +9,7 @@ class AP_Networking_CAN {
 
 private:
     void mcast_server(void);
-    void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame);
+    void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags);
     SocketAPM *mcast_sockets[HAL_NUM_CAN_IFACES];
 
     uint8_t bus_mask;

libraries/AP_Networking/AP_Networking_Config.h

@@ -107,6 +107,10 @@
 #define AP_NETWORKING_CAN_MCAST_ENABLED 0
 #endif
 
+#ifndef AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED
+#define AP_NETWORKING_CAN_MCAST_BRIDGING_ENABLED AP_NETWORKING_CAN_MCAST_ENABLED
+#endif
+
 #if AP_NETWORKING_TESTS_ENABLED
 #ifndef AP_NETWORKING_TEST_IP
 #define AP_NETWORKING_TEST_IP "192.168.144.2"