Line data    Source code

       1             : /*
       2             :   socketcan transport for SITL CAN
       3             :  */
       4             : #include "CAN_SocketCAN.h"
       5             : 
       6             : #if HAL_NUM_CAN_IFACES && HAL_CAN_WITH_SOCKETCAN
       7             : 
       8             : #include <net/if.h>
       9             : #include <poll.h>
      10             : #include <string.h>
      11             : #include <unistd.h>
      12             : #include <sys/ioctl.h>
      13             : #include <linux/can.h>
      14             : #include <errno.h>
      15             : #include <stdlib.h>
      16             : #include "CAN_SocketCAN.h"
      17             : 
      18             : /*
      19             :   initialise socketcan transport
      20             :  */
      21           0 : bool CAN_SocketCAN::init(uint8_t instance)
      22             : {
      23           0 :     struct sockaddr_can addr {};
      24           0 :     struct ifreq ifr {};
      25             :     int ret;
      26             : 
      27           0 :     fd = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW);
      28           0 :     if (fd < 0) {
      29           0 :         goto fail;
      30             :     }
      31             : 
      32           0 :     snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "vcan%u", instance);
      33           0 :     ret = ioctl(fd, SIOCGIFINDEX, &ifr);
      34           0 :     if (ret == -1) {
      35           0 :         goto fail;
      36             :     }
      37             : 
      38           0 :     addr.can_family = AF_CAN;
      39           0 :     addr.can_ifindex = ifr.ifr_ifindex;
      40             : 
      41           0 :     ret = bind(fd, (struct sockaddr*)&addr, sizeof(addr));
      42           0 :     if (ret == -1) {
      43           0 :         goto fail;
      44             :     }
      45             : 
      46           0 :     return true;
      47             : 
      48           0 : fail:
      49           0 :     if (fd != -1) {
      50           0 :         close(fd);
      51           0 :         fd = -1;
      52             :     }
      53           0 :     return false;
      54             : }
      55             : 
      56             : /*
      57             :   send a CAN frame
      58             :  */
      59           0 : bool CAN_SocketCAN::send(const AP_HAL::CANFrame &frame)
      60             : {
      61           0 :     if (frame.canfd) {
      62             :         // not supported on socketcan
      63           0 :         return false;
      64             :     }
      65           0 :     struct can_frame transmit_frame {};
      66           0 :     transmit_frame.can_id = frame.id;
      67           0 :     transmit_frame.can_dlc = frame.dlc;
      68             : 
      69           0 :     const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
      70           0 :     memcpy(transmit_frame.data, frame.data, data_length);
      71             : 
      72           0 :     return ::write(fd, &transmit_frame, sizeof(transmit_frame)) == sizeof(transmit_frame);
      73             : }
      74             : 
      75             : /*
      76             :   receive a CAN frame
      77             :  */
      78           0 : bool CAN_SocketCAN::receive(AP_HAL::CANFrame &frame)
      79             : {
      80             :     struct can_frame receive_frame;
      81           0 :     const ssize_t ret = ::read(fd, &receive_frame, sizeof(receive_frame));
      82           0 :     if (ret != sizeof(receive_frame)) {
      83           0 :         return false;
      84             :     }
      85             : 
      86             :     // run constructor to initialise
      87           0 :     new(&frame) AP_HAL::CANFrame(receive_frame.can_id, receive_frame.data, receive_frame.can_dlc, false);
      88             : 
      89           0 :     if (sem_handle != nullptr) {
      90           0 :         sem_handle->signal();
      91             :     }
      92           0 :     return true;
      93             : }
      94             : 
      95             : #endif // HAL_NUM_CAN_IFACES