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