diff --git a/CMakeLists.txt b/CMakeLists.txt index 9adb439..590e500 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.2) -project(libicsneo-socketcan-daemon VERSION 3.2.0) +project(libicsneo-socketcan-daemon VERSION 3.3.0) set(CMAKE_CXX_STANDARD 17) @@ -27,5 +27,5 @@ include_directories(BEFORE ${CMAKE_CURRENT_BINARY_DIR}) add_subdirectory("third-party/libicsneo") -add_executable(libicsneo-socketcan-daemon src/main.cpp) +add_executable(libicsneo-socketcan-daemon src/main.cpp src/netlink.c) target_link_libraries(libicsneo-socketcan-daemon icsneocpp) diff --git a/src/main.cpp b/src/main.cpp index 7b954a5..ed08d3a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -25,6 +26,8 @@ #include #include +#include "netlink.h" + #define LOG(LVL, MSG) do{if(runningAsDaemon) syslog(LVL, MSG); \ else fprintf(stderr, MSG);}while(0) #define LOGF(LVL, MSG, ...) do{if(runningAsDaemon) syslog(LVL, MSG, __VA_ARGS__); \ @@ -40,6 +43,8 @@ #define SIOCGVERSION 0x3008 #define SIOCGCLIENTVEROK 0x3009 #define SIOCSBAUDRATE 0x300A +#define SIOCSERRCOUNT 0x300B +#define SIOCSIFSETTINGS 0x300C #define RX_BOX_SIZE (sharedMemSize / (maxInterfaces * 2)) #define TX_BOX_SIZE (sharedMemSize / 4) @@ -67,19 +72,144 @@ struct intrepid_pending_tx_info { size_t bytes; }; +#define ICS_MAGIC 0x49435343 // ICSC + +struct add_can_if_info { + char alias[IFALIASZ]; + __u32 magic; + __u32 ctrl_mode; + struct can_clock clock; + struct can_bittiming_const bittiming_const; + struct can_bittiming_const data_bittiming_const; +}; + +struct can_err_report { + int device; + enum can_state state; + struct can_berr_counter err_count; +}; + +struct can_dev_settings { + int device; + struct can_bittiming bittiming; + struct can_bittiming data_bittiming; + __u32 ctrl_mode; + bool termination; +}; + +static struct can_clock clock_bxcan = { + .freq = 80000000, +}; +static struct can_bittiming_const bittiming_const_bxcan = { + .name = "bxcan-31X", + .tseg1_min = 2, /* Time segment 1 = prop_seg + phase_seg1 */ + .tseg1_max = 256, + .tseg2_min = 2, /* Time segment 2 = phase_seg2 */ + .tseg2_max = 128, + .sjw_max = 128, + .brp_min = 1, + .brp_max = 512, + .brp_inc = 1, +}; +static struct can_bittiming_const data_bittiming_const_bxcan = { + .name = "bxcan-31X", + .tseg1_min = 1, /* Time segment 1 = prop_seg + phase_seg1 */ + .tseg1_max = 32, + .tseg2_min = 1, /* Time segment 2 = phase_seg2 */ + .tseg2_max = 16, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 32, + .brp_inc = 1, +}; + +static struct can_clock clock_dspic = { + .freq = 40000000, +}; + +static struct can_bittiming_const bittiming_const_dspic = { + .name = "dspic33fj", + .tseg1_min = 1, /* Time segment 1 = prop_seg + phase_seg1 */ + .tseg1_max = 8, + .tseg2_min = 1, /* Time segment 2 = phase_seg2 */ + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 2, + .brp_max = 128, + .brp_inc = 1, +}; + +static struct can_dev_info { + devicetype_t device_type; + struct can_clock *clock; + struct can_bittiming_const *bittiming_const; + struct can_bittiming_const *data_bittiming_const; +} dev_infos[] = { + { icsneo::DeviceType::Enum::ECU_AVB, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADMars, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::VCAN4_1, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADPluto, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::VCAN4_2EL, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::FIRE3, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADJupiter, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::VCAN4_IND, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADGigastar, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RED2, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RAD_A2B, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADEpsilon, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADMoon3, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADComet, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::FIRE3_FlexRay, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::VCAN4_4, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::VCAN4_2, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::FIRE2, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADGalaxy, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::RADStar2, &clock_bxcan, &bittiming_const_bxcan, &data_bittiming_const_bxcan }, + { icsneo::DeviceType::Enum::FIRE, &clock_dspic, &bittiming_const_dspic, NULL }, + { icsneo::DeviceType::Enum::VCAN3, &clock_dspic, &bittiming_const_dspic, NULL }, + { icsneo::DeviceType::Enum::RED, &clock_dspic, &bittiming_const_dspic, NULL }, +}; + +#define ARRAY_SIZE(x) (sizeof(x)/sizeof(*x)) + +static struct can_dev_info* get_infos_for_device(devicetype_t device_type) +{ + for (size_t i = 0; i < ARRAY_SIZE(dev_infos); ++i) { + if (dev_infos[i].device_type == device_type) { + return &dev_infos[i]; + } + } + return NULL; +} + class NetworkInterface { public: - NetworkInterface(const std::string& desiredName, icsneo::Network::Type device) : type(device), name(desiredName) { - char ifname[IFALIASZ + 1] = {0}; - strncpy(ifname, name.c_str(), IFALIASZ); + NetworkInterface(const std::string& desiredName, icsneo::Network::Type device, devicetype_t device_type) + : type(device), name(desiredName) { + struct add_can_if_info info = { + .magic = ICS_MAGIC, + }; + strncpy(info.alias, name.c_str(), IFALIASZ); if(device == icsneo::Network::Type::CAN) { - kernelHandle = ioctl(driver, SIOCSADDCANIF, ifname); // this will call the intrepid_dev_ioctl() + struct can_dev_info *dev_info = get_infos_for_device(device_type); + if (dev_info) { + info.ctrl_mode = CAN_CTRLMODE_BERR_REPORTING; + info.clock = *(dev_info->clock); + info.bittiming_const = *(dev_info->bittiming_const); + if (dev_info->data_bittiming_const) { + info.ctrl_mode |= CAN_CTRLMODE_FD | CAN_CTRLMODE_FD_NON_ISO; + info.data_bittiming_const = *(dev_info->data_bittiming_const); + } + } + kernelHandle = ioctl(driver, SIOCSADDCANIF, &info); // this will call the intrepid_dev_ioctl() } else if(device == icsneo::Network::Type::Ethernet) { - kernelHandle = ioctl(driver, SIOCSADDETHIF, ifname); // this will call the intrepid_dev_ioctl() + kernelHandle = ioctl(driver, SIOCSADDETHIF, &info.alias); // this will call the intrepid_dev_ioctl() } if(openedSuccessfully()) { + ifindex = ioctl(driver, SIOCGIFINDEX, kernelHandle); + LOGF(LOG_INFO, "Ifindex for device %s is %d\n", name.c_str(), ifindex); rxBox = GET_RX_BOX(kernelHandle); rxBoxCurrentPosition = rxBox; } @@ -98,33 +228,35 @@ public: LOG(LOG_DEBUG, "Removing interface which was not opened successfully\n"); } - bool reportBaudrates(int64_t baudrate, int64_t fd_baudrate) { - struct baudrate_info { - int handle; - int64_t baudrates[2]; - } info; - - info.handle = kernelHandle; - info.baudrates[0] = baudrate; - /* set fd baudrate to zero if equal to baudrate - * this will disable fd mode in kernel */ - info.baudrates[1] = (fd_baudrate==baudrate)?0:fd_baudrate; - if (ioctl(driver, SIOCSBAUDRATE, &info) != 0) { - LOGF(LOG_INFO, "Unable to set baudrate for device %s\n", name.c_str()); - return false; - } - - return true; - } NetworkInterface(const NetworkInterface&) = delete; NetworkInterface& operator =(const NetworkInterface&) = delete; bool openedSuccessfully() const { return kernelHandle >= 0; } int getKernelHandle() const { return kernelHandle; } + int getIfIndex() const { return ifindex; } const std::string& getName() const { return name; } uint8_t* getRxBox() { return rxBox; } const uint8_t* getRxBox() const { return rxBox; } + void reportErrorCount(const std::shared_ptr& msg) { + LOGF(LOG_INFO, "%s CAN error count tx:%d rx:%d busoff:%d\n", + name.c_str(), msg->transmitErrorCount, msg->receiveErrorCount, + msg->busOff); + struct can_err_report err = { + .device = kernelHandle, + .state = (msg->busOff)?CAN_STATE_BUS_OFF:CAN_STATE_ERROR_ACTIVE, + .err_count = { + .txerr = msg->transmitErrorCount, + .rxerr = msg->receiveErrorCount, + }, + }; + + if(ioctl(driver, SIOCSERRCOUNT, &err) < 0) { + LOGF(LOG_DEBUG, "error report ioctl failed %d\n", kernelHandle); + return; + } + } + template void addReceivedMessageToQueue(const std::shared_ptr& msg) { const auto neomessageGeneric = icsneo::CreateNeoMessage(msg); @@ -158,14 +290,209 @@ public: } } + void update_bittiming(struct can_bittiming *bt) + { + + struct can_clock clock = { + .freq = 80000000, + }; + __u64 v64 = (__u64)bt->brp * 1000 * 1000 * 1000; + v64 = v64 / clock.freq; + bt->tq = (__u32)v64; + + __u32 tseg = 1 + bt->prop_seg + bt->phase_seg1 + bt->phase_seg2; + bt->bitrate = clock.freq / (bt->brp * tseg); + bt->sample_point = 1000 * (tseg - bt->phase_seg2) / tseg; + + bt->sjw = std::max(1U, std::min(bt->phase_seg1, bt->phase_seg2 / 2)); + } + + + void storeCanSettings(const CAN_SETTINGS *can, const CANFD_SETTINGS *canfd, bool termination) { + LOGF(LOG_INFO, "Baudrate:%d TqSeg1:%d TqSeg2:%d TqProp:%d TqSync:%d BRP:%d ifdelay:%d\n", + can->Baudrate, can->TqSeg1, can->TqSeg2, can->TqProp, can->TqSync, can->BRP, can->innerFrameDelay25us); + LOGF(LOG_INFO, "FD Baudrate:%d TqSeg1:%d TqSeg2:%d TqProp:%d TqSync:%d BRP:%d\n", + canfd->FDBaudrate, canfd->FDTqSeg1, canfd->FDTqSeg2, canfd->FDTqProp, canfd->FDTqSync, canfd->FDBRP); + LOGF(LOG_INFO, "FDMode:0x%x TransceiverMode:0x%x\n", canfd->FDMode, can->transceiver_mode); + + bit_timing.prop_seg = can->TqProp; + bit_timing.phase_seg1 = can->TqSeg1; + bit_timing.phase_seg2 = can->TqSeg2; + bit_timing.brp = can->BRP + 1; + bit_timing.sjw = can->TqSync; + + data_bit_timing.prop_seg = canfd->FDTqProp; + data_bit_timing.phase_seg1 = canfd->FDTqSeg1; + data_bit_timing.phase_seg2 = canfd->FDTqSeg2; + data_bit_timing.brp = canfd->FDBRP + 1; + data_bit_timing.sjw = canfd->FDTqSync; + + this->termination = termination; + + ctrl_mode = 0; + switch (canfd->FDMode) { + case NO_CANFD: + break; + ctrl_mode = 0; + case CANFD_ENABLED: + case CANFD_BRS_ENABLED: + ctrl_mode = CAN_CTRLMODE_FD_NON_ISO; + break; + case CANFD_ENABLED_ISO: + case CANFD_BRS_ENABLED_ISO: + ctrl_mode = CAN_CTRLMODE_FD; + break; + } + + switch (can->transceiver_mode) { + case LOOPBACK: + ctrl_mode |= CAN_CTRLMODE_LOOPBACK; + break; + case LISTEN_ONLY: + case LISTEN_ALL: + ctrl_mode |= CAN_CTRLMODE_LISTENONLY; + break; + } + + update_bittiming(&bit_timing); + if (canfd->FDMode != NO_CANFD) { + update_bittiming(&data_bit_timing); + } else { + data_bit_timing.bitrate = 0; + } + + struct can_dev_settings settings = { + .device = kernelHandle, + .bittiming = bit_timing, + .data_bittiming = data_bit_timing, + .ctrl_mode = ctrl_mode, + .termination = termination, + }; + if(ioctl(driver, SIOCSIFSETTINGS, &settings) < 0) { + LOGF(LOG_DEBUG, "device settings ioctl failed %d\n", kernelHandle); + return; + } + } + + void setBittiming(struct can_bittiming *timing, std::shared_ptr device, icsneo::Network::NetID netid) { + if (timing->prop_seg == bit_timing.prop_seg + && timing->phase_seg1 == bit_timing.phase_seg1 + && timing->phase_seg2 == bit_timing.phase_seg2 + && timing->sjw == bit_timing.sjw + && timing->brp == bit_timing.brp) { + LOG(LOG_INFO, "no change in bittiming\n"); + return; + } + + + CAN_SETTINGS *settings = device->settings->getMutableCANSettingsFor(netid); + + settings->SetBaudrate = USE_TQ; + settings->TqSeg1 = timing->phase_seg1; + settings->TqSeg2 = timing->phase_seg2; + settings->TqSync = timing->sjw; + settings->TqProp = timing->prop_seg; + settings->BRP = timing->brp - 1; + + LOGF(LOG_INFO, "Set Bittiming TqSeg1:%d TqSeg2:%d TqProp:%d TqSync:%d BRP:%d\n", + settings->TqSeg1, settings->TqSeg2, settings->TqProp, settings->TqSync, settings->BRP); + + if (! device->settings->apply() ) { + LOGF(LOG_ERR, "Unable to set bit timings for %s", name.c_str()); + } + + bit_timing = *timing; + } + + void setDataBittiming(struct can_bittiming *timing, std::shared_ptr device, icsneo::Network::NetID netid) { + if (timing->prop_seg == data_bit_timing.prop_seg + && timing->phase_seg1 == data_bit_timing.phase_seg1 + && timing->phase_seg2 == data_bit_timing.phase_seg2 + && timing->sjw == data_bit_timing.sjw + && timing->brp == data_bit_timing.brp) { + return; + } + CANFD_SETTINGS *settings = device->settings->getMutableCANFDSettingsFor(netid); + + settings->FDTqSeg1 = timing->phase_seg1; + settings->FDTqSeg2 = timing->phase_seg2; + settings->FDTqSync = timing->sjw; + settings->FDTqProp = timing->prop_seg; + settings->FDBRP = timing->brp - 1; + + if (! device->settings->apply() ) { + LOGF(LOG_ERR, "Unable to set data bit timings for %s", name.c_str()); + } + + data_bit_timing = *timing; + } + + void setCtrlMode(uint32_t mode, std::shared_ptr device, icsneo::Network::NetID netid) { + if (mode == ctrl_mode) { + return; + } + + if ((mode & (CAN_CTRLMODE_FD_NON_ISO | CAN_CTRLMODE_FD)) + != (ctrl_mode & (CAN_CTRLMODE_FD_NON_ISO | CAN_CTRLMODE_FD))) { + CANFD_SETTINGS *settings = device->settings->getMutableCANFDSettingsFor(netid); + if (mode & CAN_CTRLMODE_FD_NON_ISO) { + if (bit_timing.bitrate == data_bit_timing.bitrate) { + settings->FDMode = CANFD_ENABLED; + } else { + settings->FDMode = CANFD_BRS_ENABLED; + } + } else if (mode & CAN_CTRLMODE_FD) { + if (bit_timing.bitrate == data_bit_timing.bitrate) { + settings->FDMode = CANFD_ENABLED_ISO; + } else { + settings->FDMode = CANFD_BRS_ENABLED_ISO; + } + } else { + settings->FDMode = NO_CANFD; + } + } + + if ((mode & (CAN_CTRLMODE_LISTENONLY | CAN_CTRLMODE_LOOPBACK)) + != (ctrl_mode & (CAN_CTRLMODE_LISTENONLY | CAN_CTRLMODE_LOOPBACK))) { + CAN_SETTINGS *settings = device->settings->getMutableCANSettingsFor(netid); + if (mode & CAN_CTRLMODE_LISTENONLY) { + settings->transceiver_mode = LISTEN_ONLY; + } else if (mode & CAN_CTRLMODE_LOOPBACK) { + settings->transceiver_mode = LOOPBACK; + } else { + settings->transceiver_mode = NORMAL; + } + } + + if (! device->settings->apply() ) { + LOGF(LOG_ERR, "Unable to set controller mode for %s", name.c_str()); + } + ctrl_mode = mode; + } + + void setTermination(bool termination, std::shared_ptr device, icsneo::Network::NetID netid) { + if (termination != this->termination) { + if (! device->settings->setTerminationFor(netid, termination) || + ! device->settings->apply() ) { + LOGF(LOG_ERR, "Unable to set termination for %s", name.c_str()); + } + this->termination = termination; + } + } + private: icsneo::Network::Type type; std::string name; int kernelHandle = -1; + int ifindex = -1; std::mutex rxBoxLock; uint8_t* rxBox = nullptr; uint8_t* rxBoxCurrentPosition = nullptr; size_t rxBoxMessageCount = 0; + struct can_bittiming bit_timing; + struct can_bittiming data_bit_timing; + uint32_t ctrl_mode; + bool termination; }; class OpenDevice { @@ -310,7 +637,7 @@ void searchForDevices() { if(firstTimeFailedToOpen) LOGF(LOG_INFO, "Creating network interface %s\n", interfaceName.c_str()); - newDevice.interfaces[net.getNetID()] = std::make_shared(interfaceName, net.getType()); + newDevice.interfaces[net.getNetID()] = std::make_shared(interfaceName, net.getType(), newDevice.device->getType()); LOGF(LOG_INFO, "Created network interface %s\n", interfaceName.c_str()); } bool failedToCreateNetworkInterfaces = false; @@ -325,18 +652,20 @@ void searchForDevices() { LOGF(LOG_INFO, "%s failed to create network interfaces. Will keep trying...\n", newDevice.device->describe().c_str()); failedToOpen.push_back(serial); } + continue; } - if (driverMinor > 0) { - for(const auto& net : supportedNetworks) { - if (net.getType() != icsneo::Network::Type::CAN) - continue; - newDevice.interfaces[net.getNetID()]->reportBaudrates( - newDevice.device->settings->getBaudrateFor(net.getNetID()), - newDevice.device->settings->getFDBaudrateFor(net.getNetID()) - ); + if (driverMinor > 0) { + for(const auto& net : supportedNetworks) { + if (net.getType() != icsneo::Network::Type::CAN) + continue; + const CAN_SETTINGS *can = newDevice.device->settings->getCANSettingsFor(net.getNetID()); + const CANFD_SETTINGS *fd = newDevice.device->settings->getCANFDSettingsFor(net.getNetID()); + bool termination = newDevice.device->settings->isTerminationEnabledFor(net.getNetID()) + .value_or(false); + newDevice.interfaces[net.getNetID()]->storeCanSettings(can, fd, termination); + } } - } // Create rx listener newDevice.device->addMessageCallback(std::make_shared([serial](std::shared_ptr message) { @@ -350,6 +679,11 @@ void searchForDevices() { break; } } + if(frame->type == icsneo::Message::Type::CANErrorCount) { + const auto errmsg = std::static_pointer_cast(message); + openDevice->interfaces.at(frame->network.getNetID())->reportErrorCount(errmsg); + return; + } if(frame->type != icsneo::Message::Type::Frame) { LOG(LOG_ERR, "Dropping message: received invalid message type, expected RawMessage\n"); return; @@ -520,6 +854,12 @@ int main(int argc, char** argv) { return EXIT_FAILURE; } + int netlink_socket = open_netlink_socket(); + if (netlink_socket < 0) { + LOGF(LOG_ERR, "Unable to open netlink socket\nError %d: %s\n", errno, strerror(errno)); + return EXIT_FAILURE; + } + // Daemonize if necessary if(runningAsDaemon) { LOG(LOG_INFO, "The daemon will now continue to run in the background\n"); @@ -539,17 +879,49 @@ int main(int argc, char** argv) { fd_set fds; FD_ZERO(&fds); FD_SET(driver, &fds); + FD_SET(netlink_socket, &fds); + + int max_fd = (driver > netlink_socket)?driver:netlink_socket; struct timeval timeout = {}; timeout.tv_sec = 1; - auto ret = select(driver + 1, &fds, NULL, NULL, &timeout); + auto ret = select(max_fd + 1, &fds, NULL, NULL, &timeout); if(ret == -1) { // Fatal error LOGF(LOG_ERR, "Error waiting for tx messages: %s\n", strerror(errno)); stopRunning = true; break; - } else if(ret != 0) { + } + if (FD_ISSET(netlink_socket, &fds)) { + // Kernel sent some information via netlink, handle it. + read_netlink_msgs(netlink_socket, [](int ifindex, int type, void *data) { + for(auto& dev : openDevices) { + for(auto& netifPair : dev.interfaces) { + auto netid = netifPair.first; + auto iface = netifPair.second; + if (iface->getIfIndex() != ifindex) { + continue; + } + switch (type) { + case IFLA_CAN_BITTIMING: + iface->setBittiming((struct can_bittiming *) data, dev.device, netid); + break; + case IFLA_CAN_DATA_BITTIMING: + iface->setDataBittiming((struct can_bittiming *) data, dev.device, netid); + break; + case IFLA_CAN_TERMINATION: + iface->setTermination(*((uint16_t *) data) != 0, dev.device, netid); + break; + case IFLA_CAN_CTRLMODE: + iface->setCtrlMode(((struct can_ctrlmode *) data)->flags, dev.device, netid); + break; + } + } + } + }); + } + if (FD_ISSET(driver, &fds)) { // Kernel says there are some new transmit messages waiting to go out. // Call read() to find out which box they're in and how many struct intrepid_pending_tx_info info; @@ -583,6 +955,9 @@ int main(int argc, char** argv) { } else if (! dev.device->settings->setFDBaudrateFor(netid, info.bytes)) { LOGF(LOG_ERR, "Unable to set fd baudrate for device %s\n", netifPair.second->getName().c_str()); + } else if (! dev.device->settings->setTerminationFor(netid, false)) { + LOGF(LOG_ERR, "Unable to set termination for device %s\n", + netifPair.second->getName().c_str()); } else if (! dev.device->settings->apply()) { LOGF(LOG_ERR, "Unable to apply settings for device %s\n", netifPair.second->getName().c_str()); diff --git a/src/netlink.c b/src/netlink.c new file mode 100644 index 0000000..04324a1 --- /dev/null +++ b/src/netlink.c @@ -0,0 +1,99 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "netlink.h" + +bool read_netlink_msgs(int s, msg_callback callback) +{ + struct nlmsghdr buf[8192/sizeof(struct nlmsghdr)]; + + struct nlmsghdr *nh; + int len = recv(s, buf, sizeof(buf), 0); + + for (nh = (struct nlmsghdr *) buf; NLMSG_OK (nh, len); + nh = NLMSG_NEXT (nh, len)) { + /* The end of multipart message */ + if (nh->nlmsg_type == NLMSG_DONE) { + printf("Done\n"); + return true; + } + + if (nh->nlmsg_type == NLMSG_ERROR) { + printf("Error\n"); + } else if (nh->nlmsg_type == RTM_NEWLINK) { + struct ifinfomsg *iface = NLMSG_DATA(nh); + int attrlen = nh->nlmsg_len - NLMSG_LENGTH(sizeof(*iface)); + if (iface->ifi_type == ARPHRD_CAN) { + for (struct rtattr *rta = IFLA_RTA(iface); RTA_OK(rta, attrlen); rta = RTA_NEXT(rta, attrlen)) { + switch(rta->rta_type) { + case IFLA_LINKINFO: + { + int attr2len = RTA_PAYLOAD(rta); + char *kind = NULL; + void *data = NULL; + int data_len; + for (struct rtattr *rta2 = RTA_DATA(rta); RTA_OK(rta2, attr2len); + rta2 = RTA_NEXT(rta2, attr2len)) { + switch (rta2->rta_type) { + case IFLA_INFO_KIND: + kind = RTA_DATA(rta2); + break; + case IFLA_INFO_DATA: + data = RTA_DATA(rta2); + data_len = RTA_PAYLOAD(rta2); + break; + } + } + if (kind && strcmp(kind, "can") == 0 && data) { + attr2len = data_len; + for (struct rtattr *rta2 = data; RTA_OK(rta2, attr2len); + rta2 = RTA_NEXT(rta2, attr2len)) { + if (callback) { + callback(iface->ifi_index, rta2->rta_type, RTA_DATA(rta2)); + } + } + } + } + break; + } + } + } + } + } +} + +int open_netlink_socket() +{ + int s = socket(AF_NETLINK, SOCK_RAW|SOCK_CLOEXEC, NETLINK_ROUTE); + + if (s < 0) { + return -1; + } + + struct sockaddr_nl addr = { + .nl_family = AF_NETLINK, + .nl_groups = RTMGRP_LINK, + }; + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + return -1; + } + + + int group = RTMGRP_LINK; + if (setsockopt(s, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &group, sizeof(group))) { + return -1; + } + + return s; +} diff --git a/src/netlink.h b/src/netlink.h new file mode 100644 index 0000000..53eaa9f --- /dev/null +++ b/src/netlink.h @@ -0,0 +1,13 @@ +#ifdef __cplusplus +extern "C" { +#endif + +typedef void (*msg_callback)(int /* ifindex */, int /* rta_type */, void * /* data */); + +bool read_netlink_msgs(int s, msg_callback callback); + +int open_netlink_socket(); + +#ifdef __cplusplus +} +#endif