#include <stdio.h>
|
#include <stdlib.h>
|
#include <signal.h>
|
#include <unistd.h>
|
#include <time.h>
|
#include <errno.h>
|
#include <getopt.h>
|
|
#include <alchemy/task.h>
|
#include <boilerplate/ancillaries.h>
|
|
#include <rtdm/can.h>
|
|
static void print_usage(char *prg)
|
{
|
fprintf(stderr,
|
"Usage: %s [<can-interface>] [Options]\n"
|
"Options:\n"
|
" -f --filter=id:mask[:id:mask]... apply filter\n"
|
" -e --error=mask receive error messages\n"
|
" -t, --timeout=MS timeout in ms\n"
|
" -T, --timestamp with absolute timestamp\n"
|
" -R, --timestamp-rel with relative timestamp\n"
|
" -v, --verbose be verbose\n"
|
" -p, --print=MODULO print every MODULO message\n"
|
" -h, --help this help\n",
|
prg);
|
}
|
|
|
extern int optind, opterr, optopt;
|
|
static int s = -1, verbose = 0, print = 1;
|
static nanosecs_rel_t timeout = 0, with_timestamp = 0, timestamp_rel = 0;
|
|
RT_TASK rt_task_desc;
|
|
#define BUF_SIZ 255
|
#define MAX_FILTER 16
|
|
struct sockaddr_can recv_addr;
|
struct can_filter recv_filter[MAX_FILTER];
|
static int filter_count = 0;
|
|
static int add_filter(u_int32_t id, u_int32_t mask)
|
{
|
if (filter_count >= MAX_FILTER)
|
return -1;
|
recv_filter[filter_count].can_id = id;
|
recv_filter[filter_count].can_mask = mask;
|
printf("Filter #%d: id=0x%08x mask=0x%08x\n", filter_count, id, mask);
|
filter_count++;
|
return 0;
|
}
|
|
static void cleanup(void)
|
{
|
int ret;
|
|
if (verbose)
|
printf("Cleaning up...\n");
|
|
if (s >= 0) {
|
ret = close(s);
|
s = -1;
|
if (ret) {
|
fprintf(stderr, "close: %s\n", strerror(errno));
|
}
|
exit(EXIT_SUCCESS);
|
}
|
}
|
|
static void cleanup_and_exit(int sig)
|
{
|
if (verbose)
|
printf("Signal %d received\n", sig);
|
cleanup();
|
exit(0);
|
}
|
|
static void rt_task(void)
|
{
|
int i, ret, count = 0;
|
struct can_frame frame;
|
struct sockaddr_can addr;
|
socklen_t addrlen = sizeof(addr);
|
struct msghdr msg;
|
struct iovec iov;
|
nanosecs_abs_t timestamp, timestamp_prev = 0;
|
|
if (with_timestamp) {
|
msg.msg_iov = &iov;
|
msg.msg_iovlen = 1;
|
msg.msg_name = (void *)&addr;
|
msg.msg_namelen = sizeof(struct sockaddr_can);
|
msg.msg_control = (void *)×tamp;
|
msg.msg_controllen = sizeof(nanosecs_abs_t);
|
}
|
|
while (1) {
|
if (with_timestamp) {
|
iov.iov_base = (void *)&frame;
|
iov.iov_len = sizeof(can_frame_t);
|
ret = recvmsg(s, &msg, 0);
|
} else
|
ret = recvfrom(s, (void *)&frame, sizeof(can_frame_t), 0,
|
(struct sockaddr *)&addr, &addrlen);
|
if (ret < 0) {
|
switch (errno) {
|
case ETIMEDOUT:
|
if (verbose)
|
printf("recv: timed out\n");
|
continue;
|
case EBADF:
|
if (verbose)
|
printf("recv: aborted because socket was closed\n");
|
break;
|
default:
|
fprintf(stderr, "recv: %s\n", strerror(errno));
|
}
|
break;
|
}
|
|
if (print && (count % print) == 0) {
|
printf("#%d: (%d) ", count, addr.can_ifindex);
|
if (with_timestamp && msg.msg_controllen) {
|
if (timestamp_rel) {
|
printf("%lldns ", (long long)(timestamp - timestamp_prev));
|
timestamp_prev = timestamp;
|
} else
|
printf("%lldns ", (long long)timestamp);
|
}
|
if (frame.can_id & CAN_ERR_FLAG)
|
printf("!0x%08x!", frame.can_id & CAN_ERR_MASK);
|
else if (frame.can_id & CAN_EFF_FLAG)
|
printf("<0x%08x>", frame.can_id & CAN_EFF_MASK);
|
else
|
printf("<0x%03x>", frame.can_id & CAN_SFF_MASK);
|
|
printf(" [%d]", frame.can_dlc);
|
if (!(frame.can_id & CAN_RTR_FLAG))
|
for (i = 0; i < frame.can_dlc; i++) {
|
printf(" %02x", frame.data[i]);
|
}
|
if (frame.can_id & CAN_ERR_FLAG) {
|
printf(" ERROR ");
|
if (frame.can_id & CAN_ERR_BUSOFF)
|
printf("bus-off");
|
if (frame.can_id & CAN_ERR_CRTL)
|
printf("controller problem");
|
} else if (frame.can_id & CAN_RTR_FLAG)
|
printf(" remote request");
|
printf("\n");
|
}
|
count++;
|
}
|
}
|
|
int main(int argc, char **argv)
|
{
|
int opt, ret;
|
u_int32_t id, mask;
|
u_int32_t err_mask = 0;
|
struct can_ifreq ifr;
|
char *ptr;
|
char name[32];
|
|
struct option long_options[] = {
|
{ "help", no_argument, 0, 'h' },
|
{ "verbose", no_argument, 0, 'v'},
|
{ "filter", required_argument, 0, 'f'},
|
{ "error", required_argument, 0, 'e'},
|
{ "timeout", required_argument, 0, 't'},
|
{ "timestamp", no_argument, 0, 'T'},
|
{ "timestamp-rel", no_argument, 0, 'R'},
|
{ 0, 0, 0, 0},
|
};
|
|
signal(SIGTERM, cleanup_and_exit);
|
signal(SIGINT, cleanup_and_exit);
|
|
while ((opt = getopt_long(argc, argv, "hve:f:t:p:RT",
|
long_options, NULL)) != -1) {
|
switch (opt) {
|
case 'h':
|
print_usage(argv[0]);
|
exit(0);
|
|
case 'p':
|
print = strtoul(optarg, NULL, 0);
|
break;
|
|
case 'v':
|
verbose = 1;
|
break;
|
|
case 'e':
|
err_mask = strtoul(optarg, NULL, 0);
|
break;
|
|
case 'f':
|
ptr = optarg;
|
while (1) {
|
id = strtoul(ptr, NULL, 0);
|
ptr = strchr(ptr, ':');
|
if (!ptr) {
|
fprintf(stderr, "filter must be applied in the form id:mask[:id:mask]...\n");
|
exit(1);
|
}
|
ptr++;
|
mask = strtoul(ptr, NULL, 0);
|
ptr = strchr(ptr, ':');
|
add_filter(id, mask);
|
if (!ptr)
|
break;
|
ptr++;
|
}
|
break;
|
|
case 't':
|
timeout = (nanosecs_rel_t)strtoul(optarg, NULL, 0) * 1000000;
|
break;
|
|
case 'R':
|
timestamp_rel = 1;
|
case 'T':
|
with_timestamp = 1;
|
break;
|
|
default:
|
fprintf(stderr, "Unknown option %c\n", opt);
|
break;
|
}
|
}
|
|
ret = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
if (ret < 0) {
|
fprintf(stderr, "socket: %s\n", strerror(errno));
|
return -1;
|
}
|
s = ret;
|
|
if (argv[optind] == NULL) {
|
if (verbose)
|
printf("interface all\n");
|
|
ifr.ifr_ifindex = 0;
|
} else {
|
if (verbose)
|
printf("interface %s\n", argv[optind]);
|
|
namecpy(ifr.ifr_name, argv[optind]);
|
if (verbose)
|
printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name);
|
|
ret = ioctl(s, SIOCGIFINDEX, &ifr);
|
if (ret < 0) {
|
fprintf(stderr, "ioctl GET_IFINDEX: %s\n", strerror(errno));
|
goto failure;
|
}
|
}
|
|
if (err_mask) {
|
ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
|
&err_mask, sizeof(err_mask));
|
if (ret < 0) {
|
fprintf(stderr, "setsockopt: %s\n", strerror(errno));
|
goto failure;
|
}
|
if (verbose)
|
printf("Using err_mask=%#x\n", err_mask);
|
}
|
|
if (filter_count) {
|
ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER,
|
&recv_filter, filter_count *
|
sizeof(struct can_filter));
|
if (ret < 0) {
|
fprintf(stderr, "setsockopt: %s\n", strerror(errno));
|
goto failure;
|
}
|
}
|
|
recv_addr.can_family = AF_CAN;
|
recv_addr.can_ifindex = ifr.ifr_ifindex;
|
ret = bind(s, (struct sockaddr *)&recv_addr,
|
sizeof(struct sockaddr_can));
|
if (ret < 0) {
|
fprintf(stderr, "bind: %s\n", strerror(errno));
|
goto failure;
|
}
|
|
if (timeout) {
|
if (verbose)
|
printf("Timeout: %lld ns\n", (long long)timeout);
|
ret = ioctl(s, RTCAN_RTIOC_RCV_TIMEOUT, &timeout);
|
if (ret) {
|
fprintf(stderr, "ioctl RCV_TIMEOUT: %s\n", strerror(errno));
|
goto failure;
|
}
|
}
|
|
if (with_timestamp) {
|
ret = ioctl(s, RTCAN_RTIOC_TAKE_TIMESTAMP, RTCAN_TAKE_TIMESTAMPS);
|
if (ret) {
|
fprintf(stderr, "ioctl TAKE_TIMESTAMP: %s\n", strerror(errno));
|
goto failure;
|
}
|
}
|
|
snprintf(name, sizeof(name), "rtcanrecv-%d", getpid());
|
ret = rt_task_shadow(&rt_task_desc, name, 0, 0);
|
if (ret) {
|
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
|
goto failure;
|
}
|
|
rt_task();
|
/* never returns */
|
|
failure:
|
cleanup();
|
return -1;
|
}
|
