can_socket.hpp

#pragma once
#include <ossia/network/sockets/configuration.hpp>
 
#if defined(__linux__)
#include <ossia/network/sockets/writers.hpp>
 
#include <boost/asio/buffer.hpp>
#include <boost/asio/error.hpp>
#include <boost/asio/io_context.hpp>
#include <boost/asio/posix/stream_descriptor.hpp>
#include <boost/asio/post.hpp>
 
#include <linux/can.h>
#include <linux/can/error.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
 
#include <nano_signal_slot.hpp>
#include <unistd.h>
 
#include <cstring>
#include <system_error>
 
namespace ossia::net
{
 
struct can_message
{
  uint32_t id{};
 
  uint8_t size{};
 
  bool extended{};
 
  bool remote{};
 
  bool error{};
 
  bool fd{};
 
  bool bitrate_switch{};
 
  bool error_state{};
 
  uint8_t data[CANFD_MAX_DLEN]{};
};
 
class can_socket
{
public:
  using socket = boost::asio::posix::stream_descriptor;
 
  can_socket(const can_configuration& conf, boost::asio::io_context& ctx)
      : m_context{ctx}
      , m_conf{conf}
      , m_socket{ctx}
  {
  }
 
  can_socket(const can_socket&) = delete;
  can_socket(can_socket&&) = delete;
  can_socket& operator=(const can_socket&) = delete;
  can_socket& operator=(can_socket&&) = delete;
 
  ~can_socket()
  {
    // Before touching the socket: close() posts close_impl() to the io_context
    // thread, and the usual shutdown is close() then dropping the owner. Both
    // inside basic_descriptor::close() on one impl segfaults in
    // epoll_reactor::deregister_descriptor.
    m_lifetime.reset();
    close_impl();
  }
 
  void open()
  {
    int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if(fd < 0)
      throw_errno("socket(PF_CAN, SOCK_RAW, CAN_RAW)");
 
    try
    {
      // Interfaces are addressed by index, not by name, so the name has to be
      // resolved first; this is also our "does this interface exist" check.
      ifreq ifr{};
      if(m_conf.interface_name.size() >= sizeof(ifr.ifr_name))
        throw std::system_error{
            std::make_error_code(std::errc::filename_too_long),
            "CAN interface name too long: " + m_conf.interface_name};
      std::strncpy(
          ifr.ifr_name, m_conf.interface_name.c_str(), sizeof(ifr.ifr_name) - 1);
      if(::ioctl(fd, SIOCGIFINDEX, &ifr) < 0)
        throw_errno("no such CAN interface: " + m_conf.interface_name);
 
      apply_options(fd);
 
      sockaddr_can addr{};
      addr.can_family = AF_CAN;
      addr.can_ifindex = ifr.ifr_ifindex;
      if(::bind(fd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
        throw_errno("bind(" + m_conf.interface_name + ")");
    }
    catch(...)
    {
      ::close(fd);
      throw;
    }
 
    m_socket.assign(fd);
  }
 
  void connect()
  {
    open();
    boost::asio::post(m_context, [this, alive = watch()] {
      if(alive.expired())
        return;
      on_open();
    });
  }
 
  void close()
  {
    if(m_socket.is_open())
    {
      m_socket.cancel();
      boost::asio::post(m_context, [this, alive = watch()] {
        if(alive.expired())
          return;
        close_impl();
        on_close();
      });
    }
  }
 
  bool connected() const noexcept { return m_socket.is_open(); }
 
  boost::system::error_code write(const can_message& msg)
  {
    boost::system::error_code ec;
    if(!m_socket.is_open())
      return boost::asio::error::not_connected;
 
    // Classic frames must be written as `can_frame` even on a CAN_RAW_FD_FRAMES
    // socket: the kernel decides which kind of frame it received from the size
    // of the write, so sending a 72-byte canfd_frame with fd=false would turn
    // it into an FD frame on the bus.
    if(msg.fd)
    {
      canfd_frame f{};
      f.can_id = raw_id(msg);
      f.len = msg.size > CANFD_MAX_DLEN ? CANFD_MAX_DLEN : msg.size;
      if(msg.bitrate_switch)
        f.flags |= CANFD_BRS;
      if(msg.error_state)
        f.flags |= CANFD_ESI;
      std::memcpy(f.data, msg.data, f.len);
      m_socket.write_some(boost::asio::buffer(&f, sizeof(f)), ec);
    }
    else
    {
      can_frame f{};
      f.can_id = raw_id(msg);
      f.can_dlc = msg.size > CAN_MAX_DLEN ? CAN_MAX_DLEN : msg.size;
      if(!msg.remote)
        std::memcpy(f.data, msg.data, f.can_dlc);
      m_socket.write_some(boost::asio::buffer(&f, sizeof(f)), ec);
    }
 
    if(ec)
      on_write_error(ec);
    return ec;
  }
 
  template <typename F>
  void receive(F f)
  {
    struct proc : stream_processor<can_socket, F>
    {
    };
    do_receive(proc{*this, std::move(f)});
  }
 
  Nano::Signal<void()> on_open;
  Nano::Signal<void()> on_close;
  Nano::Signal<void()> on_fail;
 
  Nano::Signal<void(boost::system::error_code)> on_write_error;
 
  boost::asio::io_context& m_context;
  can_configuration m_conf;
  socket m_socket;
 
private:
  // Every handler below holds a weak reference to this token and gives up if it
  // has expired: closing a descriptor does not make asio forget the operations
  // that were pending on it. See lifetime_token.
  lifetime_token m_lifetime;
  std::weak_ptr<void> watch() const noexcept { return m_lifetime.watch(); }
 
  // CANFD_MTU (72) is the largest datagram that can be received here, as we
  // never enable CAN_RAW_XL_FRAMES; the buffer is sized for a CAN XL frame
  // anyway so that adding XL later needs no change here, and so that an
  // unexpectedly large datagram is *detected* (unknown size -> dropped) rather
  // than silently truncated into something that parses as an FD frame.
  static constexpr std::size_t max_frame_size = 2060;
  alignas(64) uint8_t m_readbuf[max_frame_size];
 
  [[noreturn]] static void throw_errno(const std::string& what)
  {
    throw std::system_error{errno, std::generic_category(), what};
  }
 
  static uint32_t raw_id(const can_message& msg) noexcept
  {
    uint32_t id = msg.extended ? (msg.id & CAN_EFF_MASK) | CAN_EFF_FLAG
                               : (msg.id & CAN_SFF_MASK);
    // RTR does not exist in CAN FD: the flag bit is reused there, so setting it
    // on an FD frame would corrupt the identifier.
    if(msg.remote && !msg.fd)
      id |= CAN_RTR_FLAG;
    return id;
  }
 
  void apply_options(int fd)
  {
    // Applied before bind(): older kernels additionally refuse
    // CAN_RAW_FD_FRAMES on an already-bound socket whose device has an MTU
    // below CANFD_MTU, and doing everything here keeps open() atomic -- either
    // the socket comes back fully configured or it throws.
    if(m_conf.fd)
    {
      const int on = 1;
      if(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0)
        throw_errno("this kernel or interface does not support CAN FD");
    }
 
    if(!m_conf.filters.empty())
    {
      // An empty list is *not* the same as not calling this: setting zero
      // filters tells the kernel to drop every non-error frame.
      std::vector<can_filter> filters;
      filters.reserve(m_conf.filters.size());
      for(auto& f : m_conf.filters)
        filters.push_back(can_filter{.can_id = f.id, .can_mask = f.mask});
 
      if(::setsockopt(
             fd, SOL_CAN_RAW, CAN_RAW_FILTER, filters.data(),
             filters.size() * sizeof(can_filter))
         < 0)
        throw_errno("could not apply the CAN filters");
    }
 
    {
      const can_err_mask_t mask = m_conf.error_frames ? CAN_ERR_MASK : 0;
      if(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &mask, sizeof(mask)) < 0)
        throw_errno("could not set the CAN error filter");
    }
 
    {
      const int loopback = m_conf.loopback ? 1 : 0;
      if(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback))
         < 0)
        throw_errno("could not set CAN_RAW_LOOPBACK");
    }
 
    {
      const int own = m_conf.receive_own_messages ? 1 : 0;
      if(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &own, sizeof(own)) < 0)
        throw_errno("could not set CAN_RAW_RECV_OWN_MSGS");
    }
  }
 
  void close_impl() noexcept
  {
    if(m_socket.is_open())
    {
      boost::system::error_code ec;
      m_socket.close(ec);
    }
  }
 
  template <typename F>
  void do_receive(F f)
  {
    m_socket.async_read_some(
        boost::asio::buffer(m_readbuf),
        [this, alive = watch(),
         f = std::move(f)](boost::system::error_code ec, std::size_t sz) mutable {
      if(alive.expired())
        return;
 
      if(!f.validate_stream(ec))
        return;
 
      // Unlike no_framing, which forwards whatever it read on any other error,
      // we stop here: on a CAN socket the remaining errors are fatal for the
      // socket (ENETDOWN when the interface goes down, ENODEV when it is
      // removed) and re-arming would spin.
      if(ec)
      {
        on_fail();
        return;
      }
 
      if(can_message msg; decode(m_readbuf, sz, msg))
        f(msg);
 
      do_receive(std::move(f));
    });
  }
 
 
  static bool decode(const uint8_t* data, std::size_t sz, can_message& msg) noexcept
  {
    uint32_t raw{};
    if(sz == CAN_MTU)
    {
      can_frame f{};
      std::memcpy(&f, data, sizeof(f));
      raw = f.can_id;
      msg.fd = false;
      msg.remote = (raw & CAN_RTR_FLAG) != 0;
      msg.size = f.can_dlc > CAN_MAX_DLEN ? CAN_MAX_DLEN : f.can_dlc;
      if(!msg.remote)
        std::memcpy(msg.data, f.data, msg.size);
    }
    else if(sz == CANFD_MTU)
    {
      canfd_frame f{};
      std::memcpy(&f, data, sizeof(f));
      raw = f.can_id;
      msg.fd = true;
      msg.remote = false; // no RTR in CAN FD
      msg.bitrate_switch = (f.flags & CANFD_BRS) != 0;
      msg.error_state = (f.flags & CANFD_ESI) != 0;
      msg.size = f.len > CANFD_MAX_DLEN ? CANFD_MAX_DLEN : f.len;
      std::memcpy(msg.data, f.data, msg.size);
    }
    else
    {
      // Neither CAN_MTU nor CANFD_MTU: a frame kind we do not know how to read
      // (CAN XL, or a future layout). Dropping it is better than reinterpreting
      // its bytes as one we do know.
      return false;
    }
 
    msg.error = (raw & CAN_ERR_FLAG) != 0;
    if(msg.error)
    {
      // For error frames the identifier is not an identifier at all: it is a
      // bitmask of the error classes (CAN_ERR_TX_TIMEOUT, CAN_ERR_BUSOFF...).
      msg.extended = false;
      msg.id = raw & CAN_ERR_MASK;
    }
    else
    {
      msg.extended = (raw & CAN_EFF_FLAG) != 0;
      msg.id = msg.extended ? (raw & CAN_EFF_MASK) : (raw & CAN_SFF_MASK);
    }
    return true;
  }
};
 
}
#endif