CANSocket.cpp

#include "CANSocket.h"
 
#include <linux/can.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
 
#include <cstring>
#include <iomanip>
#include <sstream>
 
using namespace NET;
 
CANSocket::CANSocket( int type, int protocol)
: SimpleSocket( AF_CAN, type, protocol)
{}
 
void CANSocket::connect( std::string_view interface)
{
    sockaddr_can addr = {};
    addr.can_family = AF_CAN;
    addr.can_ifindex = getInterfaceIndex(interface);
 
    if( ::connect( m_socket, (sockaddr*) &addr, sizeof(addr)) < 0)
        throw SocketException("Connect failed (connect)");
}
 
void CANSocket::bind( std::string_view interface)
{
    sockaddr_can addr = {};
    addr.can_family = AF_CAN;
    addr.can_ifindex = getInterfaceIndex(interface);
 
    if( ::bind( m_socket, (sockaddr*) &addr, sizeof(addr)) < 0)
        throw SocketException("Set of interface failed (bind)");
}
 
std::string CANSocket::getLocalInterface() const
{
    sockaddr_can addr;
    socklen_t addr_len = sizeof(addr);
 
    if( ::getsockname( m_socket, (sockaddr*) &addr, &addr_len) < 0)
        throw SocketException("Fetch of interface failed (getsockname)");
 
    return getInterfaceName(addr);
}
 
std::string CANSocket::getForeignInterface() const
{
    sockaddr_can addr;
    socklen_t addr_len = sizeof(addr);
 
    if( ::getpeername( m_socket, (sockaddr*) &addr, &addr_len) < 0)
        throw SocketException("Fetch of interface failed (getpeername)");
 
    return getInterfaceName(addr);
}
 
std::string CANSocket::getInterfaceName( const sockaddr_can& addr) const
{
    struct ifreq ifr;
    ifr.ifr_ifindex = addr.can_ifindex;
 
    if( ioctl( m_socket, SIOCGIFNAME, &ifr) < 0)
        throw SocketException("ioctl failed (getInterfaceName)");
 
    return std::string(ifr.ifr_name);
}
 
int CANSocket::getInterfaceIndex( std::string_view interface) const
{
    const size_t len = interface.length();
    // binds to all interfaces
    if( len == 0) return 0;
 
    // needed space is size plus null character
    if( len >= IF_NAMESIZE)
        throw SocketException("Interface name is too long", false);
 
    struct ifreq ifr;
    std::memcpy( ifr.ifr_name, interface.data(), len);
    ifr.ifr_name[len] = 0;
 
    if( ioctl( m_socket, SIOCGIFINDEX, &ifr) < 0)
        throw SocketException("ioctl failed (getInterfaceIndex)");
 
    return ifr.ifr_ifindex;
}
 
std::string CANSocket::formatCANID( canid_t can_id)
{
    std::ostringstream oss;
    if( can_id & CAN_ERR_FLAG)
    {
        const canid_t error_mask = can_id & CAN_ERR_MASK;
        oss << "ERR:0x"
            << std::hex
            << std::uppercase
            << std::setw(8)
            << std::setfill('0')
            << error_mask;
 
        return oss.str();
    }
 
    const bool extended = (can_id & CAN_EFF_FLAG) != 0;
    const bool rtr      = (can_id & CAN_RTR_FLAG) != 0;
    const canid_t id =
        can_id & (extended ? CAN_EFF_MASK : CAN_SFF_MASK);
 
    oss << (extended ? "EFF:0x" : "SFF:0x")
        << std::hex
        << std::uppercase
        << std::setw(extended ? 8 : 3)
        << std::setfill('0')
        << id;
 
    if (rtr) oss << " RTR";
 
    return oss.str();
}