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

Initial commit of Version 2.0.0, with CAN FD support

pull/2/head
Paul Hollinsky 2019-05-02 19:23:12 -04:00
parent 9fde92fa40
commit f2719ce16f
10 changed files with 587 additions and 760 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.gitignore vendored
View File

@ -1,6 +1,6 @@
*.a
*.o
icsneo_sample
*.log
icsscand
.vscode/
build/
build*/
.DS_Store
Thumbs.db
.vscode
*.bak

3
.gitmodules vendored 100644
View File

@ -0,0 +1,3 @@
[submodule "third-party/libicsneo"]
path = third-party/libicsneo
url = https://github.com/intrepidcs/libicsneo.git

31
CMakeLists.txt 100644
View File

@ -0,0 +1,31 @@
cmake_minimum_required(VERSION 3.2)
project(libicsneo-socketcan-daemon VERSION 2.0.0)
set(CMAKE_CXX_STANDARD 11)
include(GNUInstallDirs)
# Enable Warnings
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-switch -Wno-nested-anon-types -Wno-gnu-anonymous-struct -Wno-unknown-pragmas -Wno-zero-length-array")
# Generate build info header
execute_process(
COMMAND git rev-parse --abbrev-ref HEAD
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
OUTPUT_VARIABLE GIT_BRANCH
OUTPUT_STRIP_TRAILING_WHITESPACE
)
execute_process(
COMMAND git describe --abbrev=6 --dirty --always --tags
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
OUTPUT_VARIABLE GIT_DESCRIBE
ERROR_VARIABLE GIT_DESCRIBE
OUTPUT_STRIP_TRAILING_WHITESPACE
)
configure_file(src/buildinfo.h.template ${CMAKE_CURRENT_BINARY_DIR}/generated/buildinfo.h)
include_directories(BEFORE ${CMAKE_CURRENT_BINARY_DIR})
add_subdirectory("third-party/libicsneo")
add_executable(libicsneo-socketcan-daemon src/main.cpp)
target_link_libraries(libicsneo-socketcan-daemon icsneocpp)

2
LICENSE
View File

@ -1,4 +1,4 @@
Copyright (c) 2016 Intrepid Control Systems, Inc.
Copyright (c) 2016-2019 Intrepid Control Systems, Inc.
All rights reserved.
Redistribution and use in source and binary forms, with or without

16
Makefile
View File

@ -1,16 +0,0 @@
CXX=g++
CC=gcc
CFLAGS=-g -c -O2
LDFLAGS=
AR=ar
all: icsscand
icsscand: icsscand.o
$(CC) $(LDFLAGS) icsscand.o -o icsscand -lpthread -licsneoapi
icsscand.o: icsscand.c
$(CC) $(CFLAGS) icsscand.c
clean:
rm -rf *.o icsscand

36
README.md
View File

@ -1,21 +1,29 @@
This is the user mode daemon for Intrepid Control Systems' SocketCAN support. This daemon requires that ```intrepid.ko``` is loaded on your system. For instructions on building and loading the kernel object, see [intrepid-socketcan-kernel-module](https://github.com/intrepidcs/intrepid-socketcan-kernel-module). In addition, you will need a version of ```libicsneoapi```. For the purposes of SocketCAN support the open source version will suffice; see [libicsneoapi](https://github.com/intrepidcs/icsneoapi).
Version 2.0.0
To build, simply run ```make```.
This is the usermode daemon for the Intrepid Control Systems SocketCAN support. This daemon requires that ```intrepid.ko``` is loaded on your system.
```
$ make
```
1. Build and load the kernel module follwowing the instructions in [intrepid-socketcan-kernel-module](https://github.com/intrepidcs/intrepid-socketcan-kernel-module).
To enable SocketCAN for your attached Intrepid devices, run the daemon. Use ```-D``` to run as a daemon, or pass no arguments to run in the foreground.
2. Install the dependencies needed. These are CMake 3.2+, GCC 4.8+, git, and libusb-1.0-0-dev.
```
$ sudo ./icsscand -D
```
On Ubuntu or other Debian-based systems, run `sudo apt install git cmake gcc libusb-1.0-0-dev build-essential`.
Most Intrepid devices support more than one CAN channel. Your interfaces will be named ```icsXcan_typeY``` where ```X``` is an incrementing identifier for different devices, ```can_type``` identifies the type of CAN channel, and ```Y``` is an incrementing identifier for each device. For example, the first neoVI FIRE you plug into your system will create the interfaces ```ics0can0```, ```ics0can1```, ```ics0can2```, ```ics0can3```, ```ics0lsftcan0```, ```ics0swcan0```.
3. Clone this repository recursively by running `git clone --recursive https://github.com/intrepidcs/icsscand.git`
Before you can read or write messages on an interface you'll need to bring it up
4. Switch into the cloned directory, `cd icsscand`
```
$ sudo ifconfig ics0can0 up
```
5. Make a build directory, `mkdir build`
6. Invoke CMake, `cmake .. -DCMAKE_BUILD_TYPE=Release`
7. Build the daemon, `make`
8. The daemon is now available as `libicsneo-socketcan-daemon`
9. Start the daemon up to begin connecting to devices. Use `sudo ./libicsneo-socketcan-daemon`. If you're happy with the results and would like to run in the background, run `sudo ./libicsneo-socketcan-daemon -d` to run in daemon mode.
10. CAN interfaces will have been created, but are "down" or, in other words, not enabled for transmit and receive yet. You can see them with `ip link`. They will be labelled `can0`, `can1`, and etc. They will have an alias listed which corresponds to the serial number of the device and network on that device.
11. Enable the CAN interface with `sudo ip link set up can0`, replacing `can0` with whichever interface you'd like to enable
12. You can now use any SocketCAN application with this interface. A good package for testing is the `can-utils` package. You can get this package with `sudo apt install can-utils`. A good testing tool which comes with this package is `candump`. Running `candump can0` will print a line for every incoming frame.

723
icsscand.c
View File

@ -1,723 +0,0 @@
/*
* icsscd.c - Userspace daemon for Intrepid SocketCAN support
*
* Copyright (c) 2016 Intrepid Control Systems, Inc.
* All rights reserved.
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation; either version 2 of the License, or (at your
* option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, see http://www.gnu.org/licenses/gpl.html
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
* DAMAGE.
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <syslog.h>
#include <errno.h>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <linux/if.h>
#include <fcntl.h>
#include <pthread.h>
#include <ics/icsneo40API.h>
#define DAEMON_NAME "icsscand"
#define RUN_AS_USER "root"
#define NETDEVICE_PATH "/dev/intrepid_netdevice"
#define SIOCSADDIF 0x3001
#define SIOCSREMOVEIF 0x3002
#define SIOCGSHAREDMEMSIZE 0x3003
#define SIOCSMSGSWRITTEN 0x3004
#define SIOCGMAXIFACES 0x3005
#define MSG_BUFFER_NUM_MSGS 20000
struct interface_t {
int kernel_handle;
unsigned char *to_kernel;
char name[IFNAMSIZ];
};
struct connected_device_t {
/* handle returned by icsneoOpenNeoDevice */
void *handle;
/* info on interfaces created by the kernel driver */
struct interface_t *interfaces;
/* the number of interfaces for this device */
int num_interfaces;
/* translation from the icsSpyMessage NetworkID to interface handle */
int netid_to_interface[NETID_MAX];
/* while this is true the rx thread will keep running */
int keep_running;
pthread_t rx_thread;
NeoDevice device;
};
struct intrepid_pending_tx_info {
int tx_box_index;
int count;
};
struct netid_lookup_t {
int device_index;
int netid;
};
static int run_as_daemon = 0; /* if running in daemon mode */
static int icsscand_running = 0; /* if we're running */
static int netdevice = 0; /* fd of /dev/intrepid_netdevice */
static int shared_mem_size = 0; /* size of shared kernel mem */
static int max_num_ifaces = 0; /* max ifaces the kernel supports */
static unsigned char *shared_mem = NULL; /* shared kernel mem */
static int exit_code;
static struct connected_device_t *connected = NULL; /* connected devices */
static struct netid_lookup_t *netid_lookup = NULL; /* iface index -> device/netid */
static pthread_mutex_t devices_mutex ; /* mutex for adding/removing devices */
#define RX_BOX_SIZE (shared_mem_size / (max_num_ifaces * 2))
#define TX_BOX_SIZE (shared_mem_size / 4)
#define GET_RX_BOX(DEVICE_INDEX) (shared_mem + (RX_BOX_SIZE * DEVICE_INDEX))
#define GET_TX_BOX(INDEX) (shared_mem + (shared_mem_size / 2) + (INDEX * TX_BOX_SIZE))
#define MAX_NUM_RX_MSGS (RX_BOX_SIZE / sizeof(icsSpyMessage))
#define MAX_NUM_TX_MSGS (TX_BOX_SIZE / sizeof(icsSpyMessage))
#define PROBE_INTERVAL_MS 1000UL
#define TX_TIMEOUT_MS 100UL
#define LOG(LVL, MSG, ...) do{if(run_as_daemon) syslog(LVL, MSG, __VA_ARGS__); \
else fprintf(stderr, MSG, __VA_ARGS__);}while(0)
#define LOG_NOARGS(LVL, MSG) do{if(run_as_daemon) syslog(LVL, MSG); \
else fprintf(stderr, MSG);}while(0)
static void print_usage(char *prg)
{
fprintf(stderr, "\nUsage: %s\n\n", prg);
fprintf(stderr, " -D (run as a daemon)\n");
fprintf(stderr, " -h (show this help page)\n");
fprintf(stderr, "\n");
exit(EXIT_FAILURE);
}
static void signal_handler(int signum)
{
switch (signum) {
case SIGUSR1:
exit(EXIT_SUCCESS);
break;
case SIGALRM:
case SIGCHLD:
LOG(LOG_NOTICE, "received signal %i\n", signum);
exit_code = EXIT_FAILURE;
icsscand_running = 0;
break;
case SIGINT:
case SIGTERM:
LOG(LOG_NOTICE, "received signal %i\n", signum);
exit_code = EXIT_SUCCESS;
icsscand_running = 0;
break;
}
}
static void free_connected_device(struct connected_device_t* device, int join_thread)
{
if (join_thread)
{
device->keep_running = 0;
pthread_join(device->rx_thread, NULL);
}
if (device->handle)
{
int num_errors;
icsneoClosePort(device->handle, &num_errors);
}
if (device->interfaces)
{
int i;
for (i = 0 ; i < device->num_interfaces ; ++i)
{
ioctl(netdevice, SIOCSREMOVEIF, device->interfaces[i].kernel_handle);
netid_lookup[device->interfaces[i].kernel_handle].device_index = -1;
}
free(device->interfaces);
}
memset(device, 0, sizeof(*device));
}
static void setup_interface_info(int device_index, int net_id,
int iface_index, const char* name)
{
struct connected_device_t *device = &connected[device_index];
struct interface_t *iface = &device->interfaces[iface_index];
device->netid_to_interface[net_id] = iface_index;
netid_lookup[iface->kernel_handle].device_index = device_index;
netid_lookup[iface->kernel_handle].netid = net_id;
/* this is apparently how you change a netdevice's name */
if (name)
{
char new_name[IFNAMSIZ];
struct ifreq ifr;
int s = socket(PF_INET, SOCK_DGRAM, 0);
sprintf(new_name, "ics%d%s", device_index, name);
if (s >= 0)
{
strncpy(ifr.ifr_name, iface->name, IFNAMSIZ);
strncpy(ifr.ifr_newname, new_name, IFNAMSIZ);
if (ioctl(s, SIOCSIFNAME, &ifr) < 0)
{
LOG(LOG_NOTICE, "could not rename %s to %s\n",
iface->name, new_name);
}
else
strncpy(iface->name, new_name, IFNAMSIZ);
close(s);
}
}
}
static void* rx(void* arg)
{
int num_errors, num_msgs, ret, i;
struct connected_device_t* dev = (struct connected_device_t*)arg;
icsSpyMessage* msgs = (icsSpyMessage*)malloc(
sizeof(icsSpyMessage) * MSG_BUFFER_NUM_MSGS);
int *box_count = (int*)malloc(sizeof(int) * dev->num_interfaces);
int max_num_msgs = MAX_NUM_RX_MSGS;
/* we pass the num of written messages as a short, so cap at this amount */
if (max_num_msgs >= 1 << 16)
max_num_msgs = (1 << 16) - 1;
if (!msgs || !box_count)
{
LOG(LOG_ERR, "could not allocate a %d message buffer\n",
MSG_BUFFER_NUM_MSGS);
goto exit;
}
while (dev->keep_running)
{
/* block the thread waiting for more messages.
* this is a pthread_cond_timedwait underneath, so our thread
* will sleep if there's nothing there */
int ret = icsneoWaitForRxMessagesWithTimeOut(dev->handle, 100);
if (ret < 0)
{
LOG(LOG_ERR, "error waiting for messages on device %d\n",
dev->device.SerialNumber);
goto error;
}
else if(ret == 0)
continue; /* timeout */
/* read out the pending messages */
ret = icsneoGetMessages(dev->handle, msgs, &num_msgs, &num_errors);
if (ret == 0)
{
LOG(LOG_ERR, "error reading messages on device %d\n",
dev->device.SerialNumber);
goto error;
}
/* reset the counts of all boxes */
memset(box_count, 0, sizeof(int) * dev->num_interfaces);
for(i = 0 ; i < num_msgs ; ++i)
{
icsSpyMessage* box;
struct interface_t *iface;
/* get the message to copy */
const icsSpyMessage* msg = &msgs[i];
/* lookup which interface this network maps to */
int iface_index = dev->netid_to_interface[msg->NetworkID];
if (iface_index == -1)
continue; /* unknown network */
iface = &dev->interfaces[iface_index];
/* find the appropriate box for writing to this iface */
box = (icsSpyMessage*)iface->to_kernel;
/* copy to kernel memory */
memcpy(box + box_count[iface_index], msg, sizeof(icsSpyMessage));
/* check to see if the box is full -- if so, notify the kernel */
if (++box_count[iface_index] == max_num_msgs)
{
unsigned int ioctl_arg =
(iface->kernel_handle << 16) | max_num_msgs;
ret = ioctl(netdevice, SIOCSMSGSWRITTEN, ioctl_arg);
if (ret < 0)
{
LOG(LOG_ERR, "error transferring to kernel: %s\n",
strerror(ret));
goto error;
}
box_count[iface_index] = 0;
}
}
/* notify the kernel of any boxes that didn't fill up */
for(i = 0 ; i < dev->num_interfaces ; ++i)
{
struct interface_t *iface = &dev->interfaces[i];
if(box_count[i] > 0)
{
unsigned int ioctl_arg =
(iface->kernel_handle << 16) | box_count[i];
ret = ioctl(netdevice, SIOCSMSGSWRITTEN, ioctl_arg);
if (ret < 0)
{
LOG(LOG_ERR, "error transferring to kernel: %s\n",
strerror(ret));
goto error;
}
}
}
}
goto exit;
error:
pthread_mutex_lock(&devices_mutex);
free_connected_device(dev, 0); /* don't cancel our thread quite yet */
pthread_mutex_unlock(&devices_mutex);
exit:
if (msgs)
free(msgs);
if (box_count)
free(box_count);
return 0;
}
static void probe_new_devices()
{
NeoDevice *detected = (NeoDevice*)malloc(sizeof(NeoDevice) * max_num_ifaces);
int num_devices = max_num_ifaces, i;
const unsigned long device_types = NEODEVICE_VCAN41 | NEODEVICE_VCAN42 | NEODEVICE_VCAN3
| NEODEVICE_FIRE | NEODEVICE_ANY_ION | NEODEVICE_ANY_PLASMA | NEODEVICE_FIRE2;
/* find all the connected devices */
int ret = icsneoFindNeoDevices(device_types, detected, &num_devices);
if (ret == 0 || num_devices == 0)
{
free(detected);
return;
}
pthread_mutex_lock(&devices_mutex);
for (i = 0 ; i < num_devices ; ++i)
{
int j, num_nets = 0, already_connected = 0, device_index;
struct connected_device_t *device = NULL;
/* see if we're already connected to this device, and if we aren't,
* find a spot for this device in our array */
for(j = 0 ; j < max_num_ifaces ; ++j)
{
if (connected[j].handle != NULL &&
connected[j].device.DeviceType == detected[i].DeviceType &&
connected[j].device.SerialNumber == detected[i].SerialNumber )
{
already_connected = 1;
break;
}
if (device == NULL && connected[j].handle == NULL)
device = &connected[device_index = j];
}
if (already_connected)
continue;
if (device == NULL)
break; /* no more room at the inn */
/* actually open the device */
ret = icsneoOpenNeoDevice(&detected[i], &device->handle, NULL, 1, 0);
if (ret != 1)
{
LOG(LOG_ERR, "Unable to open device with serial %i\n",
device->device.SerialNumber);
continue;
}
device->device = detected[i];
/* figure out how many interfaces we need to make */
switch(device->device.DeviceType)
{
case NEODEVICE_VCAN41:
case NEODEVICE_VCAN42:
case NEODEVICE_VCAN3:
num_nets = 2;
break;
case NEODEVICE_FIRE:
case NEODEVICE_ANY_ION:
case NEODEVICE_ANY_PLASMA:
/* some ions and plasmas actually have 8,
* but we can't tell from the PID */
num_nets = 6;
break;
case NEODEVICE_FIRE2:
/* todo: software selectable networks */
num_nets = 6;
break;
}
if(num_nets <= 0)
{
LOG(LOG_ERR, "Unknown device with serial %d\n",
device->device.SerialNumber);
free_connected_device(device, 0);
continue;
}
device->interfaces = (struct interface_t*)malloc(
sizeof(struct interface_t) * num_nets
);
for(j = 0 ; j < num_nets ; ++j)
{
struct interface_t *iface = &device->interfaces[device->num_interfaces++];
ret = ioctl(netdevice, SIOCSADDIF, iface->name);
if (ret >= 0)
{
iface->kernel_handle = ret;
iface->to_kernel = GET_RX_BOX(ret);
}
}
/* make sure we got all the interfaces we wanted */
if (device->num_interfaces != num_nets)
{
LOG(LOG_ERR, "Could not create all interfaces for %d\n",
device->device.SerialNumber);
free_connected_device(device, 0);
continue;
}
/* build a mapping from network id -> interface id */
for(j = 0 ; j < NETID_MAX ; ++j)
device->netid_to_interface[j] = -1;
switch(device->device.DeviceType)
{
case NEODEVICE_VCAN41:
case NEODEVICE_VCAN42:
case NEODEVICE_VCAN3:
setup_interface_info(device_index, NETID_HSCAN, 0, "can0");
setup_interface_info(device_index, NETID_MSCAN, 1, "can1");
break;
case NEODEVICE_FIRE:
case NEODEVICE_ANY_PLASMA:
case NEODEVICE_ANY_ION:
setup_interface_info(device_index, NETID_HSCAN, 0, "can0");
setup_interface_info(device_index, NETID_MSCAN, 1, "can1");
setup_interface_info(device_index, NETID_HSCAN2, 2, "can2");
setup_interface_info(device_index, NETID_HSCAN3, 3, "can3");
setup_interface_info(device_index, NETID_SWCAN, 4, "swcan0");
setup_interface_info(device_index, NETID_LSFTCAN, 5, "lsftcan0");
/* TODO: IONs w/ 8 CAN */
break;
case NEODEVICE_FIRE2:
setup_interface_info(device_index, NETID_HSCAN, 0, "can0");
setup_interface_info(device_index, NETID_MSCAN, 1, "can1");
setup_interface_info(device_index, NETID_HSCAN2, 2, "can2");
setup_interface_info(device_index, NETID_HSCAN3, 3, "can3");
setup_interface_info(device_index, NETID_HSCAN4, 4, "can4");
setup_interface_info(device_index, NETID_HSCAN5, 5, "can5");
/* TODO: software selectable networks */
break;
}
device->keep_running = 1;
ret = pthread_create(&device->rx_thread, NULL, rx, device);
if (ret)
{
LOG(LOG_ERR, "Error creating thread for %d\n",
device->device.SerialNumber);
free_connected_device(device, 0);
continue;
}
}
pthread_mutex_unlock(&devices_mutex);
free(detected);
}
static unsigned long get_ms_tick_count()
{
struct timeval tv;
gettimeofday(&tv, NULL);
return (tv.tv_sec * 1000UL) + (tv.tv_usec / 1000UL);
}
/* kernel wants us to transmit some messages */
static void tx(int index, int count)
{
struct netid_lookup_t *lookup;
struct connected_device_t *dev;
icsSpyMessage *msg = (icsSpyMessage*)GET_TX_BOX(index);
int i, ret;
for(i = 0 ; i < count ; ++i, ++msg)
{
/* NetworkID is actually the device index. When we went online we stored
* the real NetworkID for this device index in netid. Do the reverse
* lookup and set the correct NetworkID */
if (msg->NetworkID < 0 || msg->NetworkID >= max_num_ifaces)
continue;
lookup = &netid_lookup[msg->NetworkID];
if (lookup->device_index < 0 || lookup->device_index > max_num_ifaces)
continue;
msg->NetworkID = lookup->netid;
dev = &connected[lookup->device_index];
ret = icsneoTxMessages(dev->handle, msg, lookup->netid, 1);
if (ret == 0)
{
LOG(LOG_ERR, "error transmitting on device %d\n",
dev->device.SerialNumber);
pthread_mutex_lock(&devices_mutex);
free_connected_device(dev, 1);
pthread_mutex_unlock(&devices_mutex);
break;
}
}
}
static void* probe_device_thread(void* arg)
{
while (icsscand_running)
{
usleep(PROBE_INTERVAL_MS * 1000);
if (!icsscand_running)
return NULL;
probe_new_devices(netdevice);
}
}
int main(int argc, char **argv)
{
int opt, ret, i, opened_devices = 0;
unsigned long last_probe = 0;
fd_set fds;
struct timeval timeout;
struct timespec clock;
pthread_t probe_thread;
/* process command line switches */
while ((opt = getopt(argc, argv, "hD")) != -1)
{
switch(opt)
{
case 'D':
run_as_daemon = 1;
break;
case 'h':
case '?':
default:
print_usage(argv[0]);
break;
}
}
/* setup logging, signals, and daemonize if requested */
openlog(DAEMON_NAME, LOG_PID, LOG_LOCAL5);
if (run_as_daemon)
{
if (daemon(0, 0))
{
LOG_NOARGS(LOG_ERR, "failed to daemonize");
exit(EXIT_FAILURE);
}
}
else
{
signal(SIGINT, signal_handler);
signal(SIGTERM, signal_handler);
}
/* we're now "running" -- incoming signals tell us to stop */
icsscand_running = 1;
/* this is the lock for adding/removing devices from the connected array */
pthread_mutex_init(&devices_mutex, NULL);
icsneoInitializeAPI();
/* open /dev/intrepid_netdevice -- this has ioctls for adding can interfaces */
netdevice = open(NETDEVICE_PATH, O_RDWR | O_NONBLOCK);
if (netdevice < 0)
{
LOG(LOG_ERR, "failed to open %s: %s\n", NETDEVICE_PATH, strerror(errno));
exit(EXIT_FAILURE);
}
/* read out some constants from the driver (these are #defines that can change) */
max_num_ifaces = ioctl(netdevice, SIOCGMAXIFACES);
if (max_num_ifaces <= 0)
{
LOG_NOARGS(LOG_ERR, "could not get maximum number of interfaces\n");
exit(EXIT_FAILURE);
}
shared_mem_size = ioctl(netdevice, SIOCGSHAREDMEMSIZE);
if (shared_mem_size <= 0)
{
LOG_NOARGS(LOG_ERR, "could not read shared memory size\n");
exit(EXIT_FAILURE);
}
/* the shared memory is what we use to transfer messages between the kernel and
* user mode. the only blocking kernel switch is the ioctl once we're done copying
* (and the inital page faults) */
shared_mem = mmap(NULL, shared_mem_size, PROT_READ | PROT_WRITE, MAP_SHARED,
netdevice, 0);
if (shared_mem == MAP_FAILED)
{
LOG_NOARGS(LOG_ERR, "could not create shared memory mapping\n");
exit(EXIT_FAILURE);
}
/* allocate the global structure of the devices we're conntected to */
connected = (struct connected_device_t*)
malloc(sizeof(struct connected_device_t) * max_num_ifaces);
memset(connected, 0, sizeof(struct connected_device_t) * max_num_ifaces);
/* the NetworkID of transmit requests is the interface index, we need to
* convert it to the device/netid pair to transmit on -- allocate this map */
netid_lookup = (struct netid_lookup_t*)
malloc(sizeof(struct netid_lookup_t) * max_num_ifaces);
for(i = 0 ; i < max_num_ifaces ; ++i)
netid_lookup[i].device_index = -1;
/* probe once for new devices, then start a thread to do it in the background
* periodically */
probe_new_devices(netdevice);
pthread_create(&probe_thread, NULL, probe_device_thread, NULL);
/* main loop. icsscand_running can be set to 0 from SIGINT, SIGTERM, etc. */
while (icsscand_running)
{
/* wait for some new transmit messages */
timeout.tv_sec = 0;
timeout.tv_usec = TX_TIMEOUT_MS * 1000;
FD_ZERO(&fds);
FD_SET(netdevice, &fds);
ret = select(netdevice + 1, &fds, NULL, NULL, &timeout);
if (ret == -1)
{
LOG(LOG_ERR, "error waiting for tx messages: %s\n", strerror(errno));
icsscand_running = 0;
break;
}
else if(ret)
{
/* kernel says there're 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;
ssize_t r;
r = read(netdevice, &info, sizeof(info));
if (r == -1)
{
LOG(LOG_ERR, "error reading tx messages: %s\n",
strerror(errno));
icsscand_running = 0;
break;
}
else if(r != sizeof(info))
{
LOG(LOG_ERR,
"unexpected number of bytes read, expected %d got %d\n",
(int)sizeof(info), (int)r);
icsscand_running = 0;
break;
}
else
{
/* send the messages! */
tx(info.tx_box_index, info.count);
}
}
else
{
/* timeout */
}
}
/* cleanup */
pthread_join(probe_thread, NULL);
pthread_mutex_lock(&devices_mutex);
for(i = 0 ; i < max_num_ifaces ; ++i)
free_connected_device(&connected[i], 1);
pthread_mutex_unlock(&devices_mutex);
free(connected);
free(netid_lookup);
munmap(shared_mem, shared_mem_size);
close(netdevice);
pthread_exit(NULL);
return exit_code;
}

12
src/buildinfo.h.template 100644
View File

@ -0,0 +1,12 @@
#ifndef __ICSNEO_SOCKETCAN_USERMODE_BUILDINFO_H_
#define __ICSNEO_SOCKETCAN_USERMODE_BUILDINFO_H_
const char* ICSNEO_SOCKETCAN_GIT_BRANCH = "@GIT_BRANCH@";
const char* ICSNEO_SOCKETCAN_GIT_DESCRIBE = "@GIT_DESCRIBE@";
const uint8_t ICSNEO_SOCKETCAN_BUILD_MAJOR = (@PROJECT_VERSION_MAJOR@);
const uint8_t ICSNEO_SOCKETCAN_BUILD_MINOR = (@PROJECT_VERSION_MINOR@);
const uint8_t ICSNEO_SOCKETCAN_BUILD_PATCH = (@PROJECT_VERSION_PATCH@);
const uint32_t ICSNEO_SOCKETCAN_BUILD_VERINT = (((@PROJECT_VERSION_MAJOR@) << 16) | ((@PROJECT_VERSION_MINOR@) << 8) | (@PROJECT_VERSION_PATCH@));
const char* ICSNEO_SOCKETCAN_BUILD_METADATA = "@BUILD_METADATA@";
#endif

511
src/main.cpp 100644
View File

@ -0,0 +1,511 @@
#include <iostream>
#include <atomic>
#include <thread>
#include <chrono>
#include <mutex>
#include <unordered_map>
#include <map>
#include <sysexits.h>
#include <unistd.h>
#include <syslog.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <signal.h>
#include <linux/if.h>
#include <icsneo/icsneocpp.h>
#include <icsneo/communication/message/neomessage.h>
#include <icsneo/communication/message/callback/canmessagecallback.h>
#include <generated/buildinfo.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__); \
else fprintf(stderr, MSG, __VA_ARGS__);}while(0)
#define SIOCSADDIF 0x3001
#define SIOCSREMOVEIF 0x3002
#define SIOCGSHAREDMEMSIZE 0x3003
#define SIOCSMSGSWRITTEN 0x3004
#define SIOCGMAXIFACES 0x3005
#define SIOCGVERSION 0x3006
#define SIOCGCLIENTVEROK 0x3007
#define RX_BOX_SIZE (sharedMemSize / (maxInterfaces * 2))
#define TX_BOX_SIZE (sharedMemSize / 4)
#define GET_RX_BOX(DEVICE_INDEX) (reinterpret_cast<uint8_t*>(sharedMemory) + (RX_BOX_SIZE * DEVICE_INDEX))
#define GET_TX_BOX(INDEX) (reinterpret_cast<uint8_t*>(sharedMemory) + (sharedMemSize / 2) + (INDEX * TX_BOX_SIZE))
bool runningAsDaemon = false;
int driver = 0; // /dev/intrepid_netdevice
int driverMajor = 0;
int driverMinor = 0;
int driverPatch = 0;
int maxInterfaces = 0; // From driver
int sharedMemSize = 0; // From driver
void* sharedMemory = nullptr;
std::atomic<bool> stopRunning(false);
struct intrepid_pending_tx_info {
int tx_box_index;
int count;
size_t bytes;
};
class NetworkInterface {
public:
NetworkInterface(const std::string& desiredName) : name(desiredName) {
char ifname[IFALIASZ + 1] = {0};
strncpy(ifname, name.c_str(), IFALIASZ);
kernelHandle = ioctl(driver, SIOCSADDIF, ifname);
if(openedSuccessfully()) {
rxBox = GET_RX_BOX(kernelHandle);
rxBoxCurrentPosition = rxBox;
}
}
~NetworkInterface() {
if(openedSuccessfully()) {
LOGF(LOG_DEBUG, "Removing device %s with handle %d\n", name.c_str(), kernelHandle);
int res = ioctl(driver, SIOCSREMOVEIF, kernelHandle);
LOGF(LOG_DEBUG, "Removed device %s with handle %d, result %d\n", name.c_str(), kernelHandle, res);
} else
LOG(LOG_DEBUG, "Removing interface which was not opened successfully\n");
}
NetworkInterface(const NetworkInterface&) = delete;
NetworkInterface& operator =(const NetworkInterface&) = delete;
bool openedSuccessfully() const { return kernelHandle >= 0; }
int getKernelHandle() const { return kernelHandle; }
const std::string& getName() const { return name; }
uint8_t* getRxBox() { return rxBox; }
const uint8_t* getRxBox() const { return rxBox; }
void addReceivedMessageToQueue(const std::shared_ptr<icsneo::Message>& msg) {
auto neomessage = icsneo::CreateNeoMessage(msg);
size_t bytesNeeded = sizeof(neomessage) + neomessage.length;
std::lock_guard<std::mutex> lg(rxBoxLock);
if(ssize_t((rxBoxCurrentPosition - rxBox) + bytesNeeded) > RX_BOX_SIZE) {
// fail, too big!
LOG(LOG_DEBUG, "box too small\n");
return;
}
memcpy(rxBoxCurrentPosition, &neomessage, sizeof(neomessage));
rxBoxCurrentPosition += sizeof(neomessage);
memcpy(rxBoxCurrentPosition, neomessage.data, neomessage.length);
rxBoxCurrentPosition += neomessage.length;
rxBoxMessageCount++;
if(ioctl(driver, SIOCSMSGSWRITTEN, (kernelHandle << 16) | rxBoxMessageCount) < 0) {
LOGF(LOG_DEBUG, "send ioctl failed %d %zu\n", kernelHandle, rxBoxMessageCount);
return;
}
rxBoxCurrentPosition = rxBox;
rxBoxMessageCount = 0;
}
private:
std::string name;
int kernelHandle = -1;
std::mutex rxBoxLock;
uint8_t* rxBox = nullptr;
uint8_t* rxBoxCurrentPosition = nullptr;
size_t rxBoxMessageCount = 0;
};
class OpenDevice {
public:
OpenDevice(const std::shared_ptr<icsneo::Device>& openDevice) : device(openDevice) {}
std::shared_ptr<icsneo::Device> device;
std::map<icsneo::Network::NetID, std::shared_ptr<NetworkInterface>> interfaces;
bool operator ==(const std::shared_ptr<icsneo::Device>& other) const {
return device->getSerial() == other->getSerial();
}
};
template <typename T>
class Lazy {
public:
Lazy(std::function<T()> f) : fn(f) {}
operator T() {
if(!valid)
evaluate();
return result;
}
void invalidate() { valid = false; }
void evaluate() {
result = fn();
valid = true;
}
private:
T result;
bool valid = false;
std::function<T()> fn;
};
std::vector<OpenDevice> openDevices;
std::vector<std::string /* serial */> failedToOpen;
std::mutex openDevicesMutex;
std::string& replaceInPlace(std::string& str, char o, const std::string& n) {
size_t start_pos = 0;
const size_t new_len = n.length();
while((start_pos = str.find(o, start_pos)) != std::string::npos) {
str.replace(start_pos, 1, n);
start_pos += new_len;
}
return str;
}
std::string sanitizeInterfaceName(std::string str) {
static const std::string nullString = "";
replaceInPlace(str, ' ', nullString);
std::transform(str.begin(), str.end(), str.begin(), ::tolower);
return str;
}
void header() {
std::cout << "The libicsneo SocketCAN Usermode Daemon\n";
std::cout << "Copyright Intrepid Control Systems, Inc. 2019\n\n";
std::cout << "Daemon v";
std::cout << (int)ICSNEO_SOCKETCAN_BUILD_MAJOR << '.' << (int)ICSNEO_SOCKETCAN_BUILD_MINOR << '.' << (int)ICSNEO_SOCKETCAN_BUILD_PATCH;
if(ICSNEO_SOCKETCAN_BUILD_METADATA[0] != '\0')
std::cout << '+' << ICSNEO_SOCKETCAN_BUILD_METADATA;
std::string describe(ICSNEO_SOCKETCAN_GIT_DESCRIBE);
if(describe.find("fatal") != 0) {
if(std::string(ICSNEO_SOCKETCAN_GIT_BRANCH) != "master")
std::cout << ' ' << ICSNEO_SOCKETCAN_GIT_BRANCH;
if(describe[0] != 'v')
std::cout << " @ " << describe;
}
std::cout << "\nlibicsneo " << icsneo::GetVersion() << "\n";
}
void usage(std::string executableName) {
std::cerr << "The libicsneo SocketCAN Usermode Daemon\n";
std::cerr << "Copyright Intrepid Control Systems, Inc. 2019\n\n";
std::cerr << "Usage: " << executableName << " [option]\n\n";
std::cerr << "Options:\n";
std::cerr << "\t-d, --daemon\t\tRun as a daemon in the background\n";
std::cerr << "\t-h, -?, --help, --usage\t\tShow this help page\n";
std::cerr << "\t --devices\t\tList supported devices\n";
}
void terminateSignal(int signal) {
stopRunning = true;
}
void searchForDevices() {
auto found = icsneo::FindAllDevices();
std::lock_guard<std::mutex> lg(openDevicesMutex);
// Open devices we have not seen before
for(auto& dev : found) {
bool alreadyOpen = false;
for(const auto& openDev : openDevices) {
if(openDev == dev) {
alreadyOpen = true;
break;
}
}
if(alreadyOpen)
continue;
// Now open the device
OpenDevice newDevice(dev);
const std::string serial = newDevice.device->getSerial();
Lazy<bool> firstTimeFailedToOpen([&serial]() {
return std::find(failedToOpen.begin(), failedToOpen.end(), serial) == failedToOpen.end();
});
if(!newDevice.device->open() || !newDevice.device->goOnline()) {
if(firstTimeFailedToOpen) {
icsneo::APIError err;
icsneo::GetLastError(err);
LOGF(LOG_INFO, "%s failed to connect. Will keep trying...\n%s\n", newDevice.device->describe().c_str(), err.describe().c_str());
failedToOpen.push_back(serial);
}
continue;
}
// Get the supported CAN networks
auto supportedNetworks = newDevice.device->getSupportedRXNetworks();
supportedNetworks.erase(std::remove_if(supportedNetworks.begin(), supportedNetworks.end(), [](const icsneo::Network& net) -> bool {
return net.getType() != icsneo::Network::Type::CAN;// Only want CAN networks
}), supportedNetworks.end());
if(supportedNetworks.empty()) {
if(firstTimeFailedToOpen) {
LOGF(LOG_INFO, "%s has no supported CAN networks\n", newDevice.device->describe().c_str());
failedToOpen.push_back(serial);
}
continue;
}
// Create a network interface for each CAN network
for(const auto& net : supportedNetworks) {
std::stringstream ss;
ss << sanitizeInterfaceName(icsneo::Network::GetNetIDString(net.getNetID())) << "_" << serial;
std::string interfaceName(ss.str());
if(firstTimeFailedToOpen)
LOGF(LOG_INFO, "Creating network interface %s\n", interfaceName.c_str());
newDevice.interfaces[net.getNetID()] = std::make_shared<NetworkInterface>(interfaceName);
LOGF(LOG_INFO, "Created network interface %s\n", interfaceName.c_str());
}
bool failedToCreateNetworkInterfaces = false;
for(const auto& iface : newDevice.interfaces) {
if(!iface.second->openedSuccessfully()) {
failedToCreateNetworkInterfaces = true;
break;
}
}
if(failedToCreateNetworkInterfaces) {
if(firstTimeFailedToOpen) {
LOGF(LOG_INFO, "%s failed to create network interfaces. Will keep trying...\n", newDevice.device->describe().c_str());
failedToOpen.push_back(serial);
}
continue;
}
// Create rx listener
newDevice.device->addMessageCallback(icsneo::CANMessageCallback([serial](std::shared_ptr<icsneo::Message> message) {
if(message->transmitted)
return;
auto canMessage = std::static_pointer_cast<icsneo::CANMessage>(message);
const OpenDevice* openDevice = nullptr;
for(const auto& dev : openDevices) {
if(dev.device->getSerial() == serial) {
openDevice = &dev;
break;
}
}
if(!openDevice) {
LOG(LOG_ERR, "Dropping message, no open device\n");
return;
}
// todo might throw
openDevice->interfaces.at(canMessage->network.getNetID())->addReceivedMessageToQueue(canMessage);
}));
LOGF(LOG_INFO, "%s connected\n", newDevice.device->describe().c_str());
failedToOpen.erase(std::remove_if(failedToOpen.begin(), failedToOpen.end(), [&serial](const std::string& s) -> bool {
return serial == s;
}), failedToOpen.end());
openDevices.push_back(std::move(newDevice));
}
// Close devices we don't see anymore
openDevices.erase(
std::remove_if(
openDevices.begin(),
openDevices.end(),
[&found](OpenDevice& openDev) -> bool {
bool stillHere = false;
for(const auto& dev : found) {
if(openDev == dev) {
stillHere = true;
break;
}
}
if(stillHere)
return false;
// The device is closed and the networks are removed by virtue of removing it from the array
LOGF(LOG_INFO, "%s disconnected\n", openDev.device->describe().c_str());
return true;
}
),
openDevices.end()
);
for(const auto& err : icsneo::GetErrors()) {
bool forErrorDevice = false;
for(const auto& dev : failedToOpen) {
if(err.isForDevice(dev)) {
forErrorDevice = true;
break;
}
}
if(forErrorDevice)
continue;
std::string description = err.describe();
description += "\n";
LOGF(LOG_INFO, "%s", description.c_str());
}
}
void deviceSearchThread() {
while(!stopRunning) {
searchForDevices();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
}
int main(int argc, char** argv) {
if(argc > 2) {
usage(argv[0]);
return EX_USAGE;
}
if(argc == 2) {
const std::string arg = argv[1];
if(arg == "-d" || arg == "--daemon") {
runningAsDaemon = true;
} else if(arg == "-h" || arg == "--help" || arg == "-?" || arg == "--usage") {
usage(argv[0]);
return EXIT_SUCCESS;
} else if(arg == "--devices") {
header();
std::cout<< "\nSupported devices:" << std::endl;
for(auto& dev : icsneo::GetSupportedDevices())
std::cout << '\t' << dev << std::endl;
return EXIT_SUCCESS;
} else {
usage(argv[0]);
return EX_USAGE;
}
}
header();
// Open the /dev/intrepid_netdevice kernel driver
if((driver = open("/dev/intrepid_netdevice", O_RDWR | O_NONBLOCK)) <= 0) {
std::cout << '\n'; // Still printing versions
LOGF(LOG_ERR, "Could not open the kernel driver\nError %d: %s\n", errno, strerror(errno));
switch(errno) {
case 2: // No such file or directory
LOG(LOG_ERR, "\nThis usually happens if the driver has not been loaded with insmod\n");
break;
case 5: // Input/output error
LOG(LOG_ERR, "\nThis usually happens if there is already a daemon running\n");
break;
case 13: // Permission denied
LOG(LOG_ERR, "\nThis usually happens if the daemon is not being run as root (use sudo)\n");
break;
}
return EXIT_FAILURE;
}
// Read out version from the driver
int version = 0;
if((version = ioctl(driver, SIOCGVERSION)) <= 0) {
std::cout << '\n'; // Still printing versions
LOGF(LOG_ERR, "Error reading the version from the kernel driver\nError %d: %s\n\nThis can happen if the driver is too old",
errno, strerror(errno));
return EXIT_FAILURE;
}
driverMajor = (version >> 16) & 0xFF;
driverMinor = (version >> 8) & 0xFF;
driverPatch = version & 0xFF;
if(driverMajor < 2) {
std::cout << '\n'; // Still printing versions
LOGF(LOG_ERR, "Error reading the version from the kernel driver\nError version %d.%d.%d\n", driverMajor, driverMinor, driverPatch);
return EXIT_FAILURE;
}
std::cout << "Driver v" << driverMajor << '.' << driverMinor << '.' << driverPatch << "\n\n";
if(driverMajor > 2) {
LOG(LOG_ERR, "This version of the usermode daemon is too old to work with this driver\nPlease ensure that both the usermode daemon and kernel driver are up to date\n");
return EXIT_FAILURE;
}
if(ioctl(driver, SIOCGCLIENTVEROK, ICSNEO_SOCKETCAN_BUILD_VERINT) != 0) {
LOG(LOG_ERR, "The kernel driver reports an incompatibility with this version of the usermode daemon\nPlease ensure that both the usermode daemon and kernel driver are up to date\n");
return EXIT_FAILURE;
}
// Read out other constants from the driver
if((maxInterfaces = ioctl(driver, SIOCGMAXIFACES)) <= 0) {
LOGF(LOG_ERR, "Error reading the maximum number of interfaces from the kernel driver\nError %d: %s\n", errno, strerror(errno));
return EXIT_FAILURE;
}
if((sharedMemSize = ioctl(driver, SIOCGSHAREDMEMSIZE)) <= 0) {
LOGF(LOG_ERR, "Error reading the shared memory size from the kernel driver\nError %d: %s\n", errno, strerror(errno));
return EXIT_FAILURE;
}
// Set up shared memory
if((sharedMemory = mmap(nullptr, sharedMemSize, PROT_READ | PROT_WRITE, MAP_SHARED, driver, 0)) == MAP_FAILED || sharedMemory == nullptr) {
LOG(LOG_ERR, "Error setting up shared memory with the kernel driver\n");
return EXIT_FAILURE;
}
// Daemonize if necessary
if(runningAsDaemon) {
LOG(LOG_INFO, "The daemon will now continue to run in the background\n");
openlog("icsneo-socketcan", LOG_PID, LOG_LOCAL5);
if(daemon(0 /* change pwd to root */, 0 /* no stdout or stderr anymore */)) {
std::cerr << "Failed to spawn the daemon. Exiting...\n";
return EXIT_FAILURE;
}
} else {
signal(SIGINT, terminateSignal);
LOG(LOG_INFO, "Waiting for connections...\n");
}
std::thread searchThread(deviceSearchThread);
while(!stopRunning) {
fd_set fds;
FD_ZERO(&fds);
FD_SET(driver, &fds);
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 1000 * 1000; // 1s
auto ret = select(driver + 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) {
// 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;
ssize_t r = read(driver, &info, sizeof(info));
if (r == -1) {
LOGF(LOG_ERR, "Error waiting for tx messages: %s\n", strerror(errno));
stopRunning = true;
break;
} else if(r != sizeof(info)) {
LOGF(LOG_ERR, "Unexpected number of bytes read, expected %d got %d\n", (int)sizeof(info), (int)r);
stopRunning = true;
break;
} else {
// Send!
uint8_t* currentPosition = GET_TX_BOX(info.tx_box_index);
while(info.count--) {
neomessage_t* msg = reinterpret_cast<neomessage_t*>(currentPosition);
currentPosition += sizeof(neomessage_t);
msg->data = currentPosition;
currentPosition += msg->length;
bool sent = false;
for(auto& dev : openDevices) {
for(auto& netifPair : dev.interfaces) {
if(netifPair.second->getKernelHandle() != msg->netid)
continue;
msg->netid = static_cast<uint16_t>(netifPair.first);
auto tx = icsneo::CreateMessageFromNeoMessage(msg);
if(!dev.device->transmit(tx))
break;
sent = true;
break;
}
if(sent)
break;
}
if(!sent)
LOG(LOG_ERR, "Message dropped, could not find the device the kernel referenced\n");
}
}
}
}
searchThread.join();
LOG(LOG_INFO, "\nExiting...\n");
return EXIT_SUCCESS;
}

1
third-party/libicsneo vendored 160000

@ -0,0 +1 @@
Subproject commit 0b4ffdbaad66904926623a6ed3800effd1d9ff42