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

Merge c13a0039db into be51cce4f1

chgabriel79 2024-09-25 10:43:34 +02:00 committed by GitHub
parent be51cce4f1 c13a0039db
commit cf1ec3017a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 524 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

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

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