Line data    Source code

       1             : /*
       2             :   multicast UDP transport for SITL CAN
       3             :  */
       4             : #include "CAN_Multicast.h"
       5             : #include "SITL_Multicast.h"
       6             : 
       7             : #if HAL_NUM_CAN_IFACES
       8             : 
       9             : #include <net/if.h>
      10             : #include <unistd.h>
      11             : #include <sys/ioctl.h>
      12             : #include <errno.h>
      13             : #include <stdlib.h>
      14             : #include <sys/socket.h>
      15             : #include <netinet/in.h>
      16             : #include <netinet/udp.h>
      17             : #include <arpa/inet.h>
      18             : #include <fcntl.h>
      19             : #include <AP_Math/crc.h>
      20             : 
      21             : #define MCAST_ADDRESS_BASE "239.65.82.0"
      22             : #define MCAST_PORT 57732U
      23             : #define MCAST_MAGIC 0x2934U
      24             : #define MCAST_FLAG_CANFD 0x0001
      25             : #define MCAST_MAX_PKT_LEN 74 // 64 byte data + 10 byte header
      26             : 
      27             : struct PACKED mcast_pkt {
      28             :     uint16_t magic;
      29             :     uint16_t crc;
      30             :     uint16_t flags;
      31             :     uint32_t message_id;
      32             :     uint8_t data[MCAST_MAX_PKT_LEN-10];
      33             : };
      34             : 
      35             : /*
      36             :   initialise multicast transport
      37             :  */
      38           5 : bool CAN_Multicast::init(uint8_t instance)
      39             : {
      40             :     // setup incoming multicast socket
      41           5 :     char address[] = MCAST_ADDRESS_BASE;
      42             : 
      43           5 :     address[strlen(address)-1] = '0' + instance;
      44           5 :     sock.set_multicast_interface_address(sitl_multicast_interface_address());
      45           5 :     return sock.connect(address, sitl_can_multicast_port(MCAST_PORT));
      46             : }
      47             : 
      48             : /*
      49             :   send a CAN frame
      50             :  */
      51       18063 : bool CAN_Multicast::send(const AP_HAL::CANFrame &frame)
      52             : {
      53       18063 :     struct mcast_pkt pkt {};
      54       18063 :     pkt.magic = MCAST_MAGIC;
      55       18063 :     pkt.flags = 0;
      56             : #if HAL_CANFD_SUPPORTED
      57       18063 :     if (frame.canfd) {
      58           0 :         pkt.flags |= MCAST_FLAG_CANFD;
      59             :     }
      60             : #endif
      61       18063 :     pkt.message_id = frame.id;
      62       18063 :     const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
      63       18063 :     memcpy(pkt.data, frame.data, data_length);
      64       18063 :     pkt.crc = crc16_ccitt((uint8_t*)&pkt.flags, data_length+6, 0xFFFFU);
      65             : 
      66       18063 :     return sock.send((void*)&pkt, data_length+10) == data_length+10;
      67             : }
      68             : 
      69             : /*
      70             :   receive a CAN frame
      71             :  */
      72       80842 : bool CAN_Multicast::receive(AP_HAL::CANFrame &frame)
      73             : {
      74             :     struct mcast_pkt pkt;
      75       80842 :     ssize_t ret = sock.recv((void*)&pkt, sizeof(pkt), 0);
      76       80842 :     if (ret < 10) {
      77       80690 :         return false;
      78             :     }
      79         152 :     if (pkt.magic != MCAST_MAGIC) {
      80           0 :         return false;
      81             :     }
      82         152 :     if (pkt.crc != crc16_ccitt((uint8_t*)&pkt.flags, ret-4, 0xFFFFU)) {
      83           0 :         return false;
      84             :     }
      85             : 
      86             :     // run constructor to initialise
      87         152 :     new(&frame) AP_HAL::CANFrame(pkt.message_id, pkt.data, ret-10, (pkt.flags & MCAST_FLAG_CANFD) != 0);
      88             : 
      89         152 :     if (sem_handle != nullptr) {
      90         152 :         sem_handle->signal();
      91             :     }
      92             : 
      93         152 :     return true;
      94             : }
      95             : 
      96             : #endif // HAL_NUM_CAN_IFACES