sven
/
icsscand
mirror of https://github.com/intrepidcs/icsscand.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Allow setting individual bit timing parameters 

Rework of the bitrate setting to allow finer control about the
bit timing settings.

The current settings are propagated via existing netlink
interfaces, reporting the current settings uses custom
ioctls.

Also report bit errors and bus off state if sent from the
device.

Signed-off-by: Christian Gabriel <ch_gabriel@web.de>
pull/17/head
Christian Gabriel 2024-09-25 10:32:18 +02:00
parent be51cce4f1
commit fe777df9ee
3 changed files with 522 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

445
src/main.cpp
View File

@ -17,6 +17,7 @@
#include <fcntl.h>
#include <signal.h>
#include <linux/if.h>
#include <linux/can/netlink.h>
#include <icsneo/icsneocpp.h>
#include <icsneo/communication/message/neomessage.h>
@ -25,6 +26,8 @@
#include <icsneo/communication/message/callback/canmessagecallback.h>
#include <generated/buildinfo.h>
#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<icsneo::CANErrorCountMessage>& 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<typename T>
void addReceivedMessageToQueue(const std::shared_ptr<icsneo::Frame>& 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<icsneo::Device> 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<icsneo::Device> 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<icsneo::Device> 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<icsneo::Device> 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<NetworkInterface>(interfaceName, net.getType());
newDevice.interfaces[net.getNetID()] = std::make_shared<NetworkInterface>(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<icsneo::MessageCallback>([serial](std::shared_ptr<icsneo::Message> message) {
@ -350,6 +679,11 @@ void searchForDevices() {
break;
}
}
if(frame->type == icsneo::Message::Type::CANErrorCount) {
const auto errmsg = std::static_pointer_cast<icsneo::CANErrorCountMessage>(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());

99
src/netlink.c 100644
View File

@ -0,0 +1,99 @@
#include <stdio.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <sys/uio.h>
#include <sys/socket.h>
#include <linux/if_link.h>
#include <linux/if_arp.h>
#include <linux/netlink.h>
#include <linux/can/netlink.h>
#include <linux/rtnetlink.h>
#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;
}

13
src/netlink.h 100644
View File

@ -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