Line data    Source code

       1             : // Copyright 2024 Accenture.
       2             : 
       3             : #include "can/SocketCanTransceiver.h"
       4             : 
       5             : #include <can/CanLogger.h>
       6             : #include <can/canframes/ICANFrameSentListener.h>
       7             : #include <linux/can.h>
       8             : #include <linux/can/raw.h>
       9             : #include <net/if.h>
      10             : #include <sys/ioctl.h>
      11             : #include <sys/socket.h>
      12             : 
      13             : #include <fcntl.h>
      14             : #include <signal.h>
      15             : #include <type_traits>
      16             : #include <unistd.h>
      17             : 
      18             : static_assert(
      19             :     std::is_standard_layout<::can::CANFrame>::value
      20             :         && std::is_trivially_destructible<::can::CANFrame>::value,
      21             :     "check for UB while passing through TxQueue");
      22             : 
      23             : namespace can
      24             : {
      25             : 
      26             : using ::util::logger::CAN;
      27             : using ::util::logger::Logger;
      28             : 
      29             : namespace
      30             : {
      31             : 
      32             : template<typename F>
      33           0 : void signalGuarded(F&& function)
      34             : {
      35             :     sigset_t set, oldSet;
      36           0 :     sigfillset(&set);
      37           0 :     pthread_sigmask(SIG_SETMASK, &set, &oldSet);
      38           0 :     ::std::forward<F>(function)();
      39           0 :     pthread_sigmask(SIG_SETMASK, &oldSet, nullptr);
      40           0 : }
      41             : 
      42             : } // namespace
      43             : 
      44             : // needed if ODR-used
      45             : size_t const SocketCanTransceiver::TX_QUEUE_SIZE_BYTES;
      46             : 
      47           1 : SocketCanTransceiver::SocketCanTransceiver(DeviceConfig const& config)
      48           1 : : AbstractCANTransceiver(config.busId)
      49           1 : , _txQueue()
      50           1 : , _txReader(_txQueue)
      51           1 : , _txWriter(_txQueue)
      52           1 : , _config(config)
      53           1 : , _fileDescriptor(-1)
      54           1 : , _writable(false)
      55           1 : {}
      56             : 
      57           0 : ICanTransceiver::ErrorCode SocketCanTransceiver::init()
      58             : {
      59           0 :     if (!isInState(State::CLOSED))
      60             :     {
      61             :         return ErrorCode::CAN_ERR_ILLEGAL_STATE;
      62             :     }
      63           0 :     setState(State::INITIALIZED);
      64           0 :     return ErrorCode::CAN_ERR_OK;
      65             : }
      66             : 
      67           0 : ICanTransceiver::ErrorCode SocketCanTransceiver::open()
      68             : {
      69           0 :     if (!isInState(State::INITIALIZED))
      70             :     {
      71             :         return ErrorCode::CAN_ERR_ILLEGAL_STATE;
      72             :     }
      73           0 :     signalGuarded([this] { guardedOpen(); });
      74           0 :     setState(State::OPEN);
      75           0 :     _writable.store(true);
      76           0 :     return ErrorCode::CAN_ERR_OK;
      77             : }
      78             : 
      79           0 : ICanTransceiver::ErrorCode SocketCanTransceiver::open(CANFrame const& frame)
      80             : {
      81           0 :     estd_assert(false);
      82             :     return ErrorCode::CAN_ERR_ILLEGAL_STATE;
      83             : }
      84             : 
      85           0 : ICanTransceiver::ErrorCode SocketCanTransceiver::close()
      86             : {
      87           0 :     if (!isInState(State::OPEN) && !isInState(State::MUTED))
      88             :     {
      89             :         return ErrorCode::CAN_ERR_ILLEGAL_STATE;
      90             :     }
      91           0 :     signalGuarded([this] { guardedClose(); });
      92           0 :     _writable.store(false);
      93           0 :     setState(State::CLOSED);
      94           0 :     return ErrorCode::CAN_ERR_OK;
      95             : }
      96             : 
      97           0 : void SocketCanTransceiver::shutdown() {}
      98             : 
      99           0 : ICanTransceiver::ErrorCode SocketCanTransceiver::write(CANFrame const& frame)
     100             : {
     101           0 :     if (!_writable.load(std::memory_order_relaxed))
     102             :     {
     103             :         return ErrorCode::CAN_ERR_ILLEGAL_STATE;
     104             :     }
     105           0 :     FrameWithListener slot{frame, nullptr};
     106           0 :     ::estd::slice<uint8_t> memory = _txWriter.allocate(sizeof(slot));
     107           0 :     if (memory.size() == 0)
     108             :     {
     109             :         return ErrorCode::CAN_ERR_TX_HW_QUEUE_FULL;
     110             :     }
     111           0 :     ::std::memcpy(memory.data(), &slot, sizeof(slot));
     112           0 :     _txWriter.commit();
     113           0 :     return ErrorCode::CAN_ERR_OK;
     114             : }
     115             : 
     116             : ICanTransceiver::ErrorCode
     117           0 : SocketCanTransceiver::write(CANFrame const& frame, ICANFrameSentListener& listener)
     118             : {
     119           0 :     if (!_writable.load(std::memory_order_relaxed))
     120             :     {
     121             :         return ErrorCode::CAN_ERR_ILLEGAL_STATE;
     122             :     }
     123           0 :     FrameWithListener slot{frame, &listener};
     124           0 :     ::estd::slice<uint8_t> memory = _txWriter.allocate(sizeof(slot));
     125           0 :     if (memory.size() == 0)
     126             :     {
     127             :         return ErrorCode::CAN_ERR_TX_HW_QUEUE_FULL;
     128             :     }
     129           0 :     ::std::memcpy(memory.data(), &slot, sizeof(slot));
     130           0 :     _txWriter.commit();
     131           0 :     return ErrorCode::CAN_ERR_OK;
     132             : }
     133             : 
     134           0 : ICanTransceiver::ErrorCode SocketCanTransceiver::mute()
     135             : {
     136           0 :     if (!isInState(State::OPEN))
     137             :     {
     138             :         return ErrorCode::CAN_ERR_ILLEGAL_STATE;
     139             :     }
     140           0 :     _writable.store(false);
     141           0 :     setState(State::MUTED);
     142           0 :     return ErrorCode::CAN_ERR_OK;
     143             : }
     144             : 
     145           0 : ICanTransceiver::ErrorCode SocketCanTransceiver::unmute()
     146             : {
     147           0 :     if (!isInState(State::MUTED))
     148             :     {
     149             :         return ErrorCode::CAN_ERR_ILLEGAL_STATE;
     150             :     }
     151           0 :     setState(State::OPEN);
     152           0 :     _writable.store(true);
     153           0 :     return ErrorCode::CAN_ERR_OK;
     154             : }
     155             : 
     156           0 : uint32_t SocketCanTransceiver::getBaudrate() const { return 500000U; }
     157             : 
     158           0 : uint16_t SocketCanTransceiver::getHwQueueTimeout() const { return 1U; }
     159             : 
     160           0 : void SocketCanTransceiver::run(int maxSentPerRun, int maxReceivedPerRun)
     161             : {
     162           0 :     signalGuarded([this, maxSentPerRun, maxReceivedPerRun]
     163           0 :                   { guardedRun(maxSentPerRun, maxReceivedPerRun); });
     164           0 : }
     165             : 
     166           0 : void SocketCanTransceiver::guardedOpen()
     167             : {
     168           0 :     char const* const name = _config.name;
     169           0 :     int error              = 0;
     170           0 :     int const fd           = socket(PF_CAN, SOCK_RAW, CAN_RAW);
     171           0 :     if (fd < 0)
     172             :     {
     173           0 :         Logger::error(
     174             :             CAN, "[SocketCanTransceiver] Failed to create socket (node=%s, error=%d)", name, fd);
     175           0 :         return;
     176             :     }
     177             : 
     178           0 :     struct ifreq ifr;
     179           0 :     ::std::strcpy(ifr.ifr_name, name);
     180           0 :     error = ioctl(fd, SIOCGIFINDEX, &ifr);
     181           0 :     if (error < 0)
     182             :     {
     183           0 :         Logger::error(
     184             :             CAN, "[SocketCanTransceiver] Failed to ioctl socket (node=%s, error=%d)", name, error);
     185           0 :         return;
     186             :     }
     187             : 
     188           0 :     int const enable_canfd = 1;
     189           0 :     error = setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd));
     190           0 :     if (error < 0)
     191             :     {
     192           0 :         Logger::error(
     193             :             CAN,
     194             :             "[SocketCanTransceiver] Failed to setsockopt socket (node=%s, error=%d)",
     195             :             name,
     196             :             error);
     197           0 :         return;
     198             :     }
     199             : 
     200           0 :     error = fcntl(fd, F_SETFL, O_NONBLOCK);
     201           0 :     if (error < 0)
     202             :     {
     203           0 :         Logger::error(
     204             :             CAN,
     205             :             "[SocketCanTransceiver] Failed to switch to non-blocking mode (node=%s, error=%d)",
     206             :             name,
     207             :             error);
     208           0 :         return;
     209             :     }
     210             : 
     211           0 :     struct sockaddr_can addr;
     212           0 :     ::std::memset(&addr, 0, sizeof(addr));
     213           0 :     addr.can_family  = AF_CAN;
     214           0 :     addr.can_ifindex = ifr.ifr_ifindex;
     215           0 :     error            = bind(fd, (struct sockaddr*)&addr, sizeof(addr));
     216           0 :     if (error < 0)
     217             :     {
     218           0 :         Logger::error(
     219             :             CAN, "[SocketCanTransceiver] Failed to bind socket (node=%s, error=%d)", name, error);
     220           0 :         return;
     221             :     }
     222             : 
     223           0 :     _fileDescriptor = fd;
     224             : }
     225             : 
     226           0 : void SocketCanTransceiver::guardedClose()
     227             : {
     228           0 :     ::close(_fileDescriptor);
     229           0 :     _fileDescriptor = -1;
     230           0 : }
     231             : 
     232           0 : void SocketCanTransceiver::guardedRun(int maxSentPerRun, int maxReceivedPerRun)
     233             : {
     234             :     // MUTED condition does not affect the messages already in the write queue;
     235             :     // the idea is that once we confirmed that we had accepted the message for delivery,
     236             :     // we shall try to deliver it.
     237           0 :     for (int count = 0; count < maxSentPerRun; ++count)
     238             :     {
     239           0 :         ::estd::slice<uint8_t const> canFrameSlice = _txReader.peek();
     240           0 :         if (canFrameSlice.size() == 0)
     241             :         {
     242             :             break;
     243             :         }
     244           0 :         FrameWithListener slot;
     245           0 :         ::std::memcpy(static_cast<void*>(&slot), canFrameSlice.data(), sizeof(slot));
     246           0 :         CANFrame& canFrame = slot.frame;
     247           0 :         can_frame socketCanFrame;
     248           0 :         socketCanFrame.can_id  = canFrame.getId();
     249           0 :         int length             = canFrame.getPayloadLength();
     250           0 :         socketCanFrame.can_dlc = length;
     251           0 :         ::std::memcpy(socketCanFrame.data, canFrame.getPayload(), length);
     252           0 :         ::std::memset(socketCanFrame.data + length, 0, sizeof(socketCanFrame.data) - length);
     253           0 :         length = ::write(_fileDescriptor, reinterpret_cast<char*>(&socketCanFrame), CAN_MTU);
     254           0 :         if (length != CAN_MTU)
     255             :         {
     256             :             break;
     257             :         }
     258           0 :         _txReader.release();
     259           0 :         if (slot.listener != nullptr)
     260             :         {
     261           0 :             slot.listener->canFrameSent(canFrame);
     262             :         }
     263           0 :         notifySentListeners(canFrame);
     264             :     }
     265             : 
     266           0 :     for (int count = 0; count < maxReceivedPerRun; ++count)
     267             :     {
     268           0 :         alignas(can_frame) uint8_t buffer[CANFD_MTU];
     269           0 :         int const length = read(_fileDescriptor, reinterpret_cast<char*>(buffer), CANFD_MTU);
     270           0 :         if (length < 0)
     271             :         {
     272             :             break;
     273             :         }
     274           0 :         if (length == CAN_MTU)
     275             :         {
     276           0 :             can_frame const& socketCanFrame = *reinterpret_cast<can_frame const*>(buffer);
     277           0 :             Logger::debug(
     278             :                 CAN,
     279             :                 "[SocketCanTransceiver] received CAN frame, id=0x%X, length=%d",
     280           0 :                 (int)socketCanFrame.can_id,
     281           0 :                 (int)socketCanFrame.can_dlc);
     282           0 :             CANFrame canFrame;
     283           0 :             canFrame.setId(socketCanFrame.can_id);
     284           0 :             canFrame.setPayload(socketCanFrame.data, socketCanFrame.can_dlc);
     285           0 :             canFrame.setPayloadLength(socketCanFrame.can_dlc);
     286           0 :             canFrame.setTimestamp(0);
     287             : 
     288           0 :             notifyListeners(canFrame);
     289             :         }
     290             :     }
     291           0 : }
     292             : 
     293             : } // namespace can