#include <stdio.h>
|
#include <stdlib.h>
|
#include <signal.h>
|
#include <unistd.h>
|
#include <time.h>
|
#include <errno.h>
|
#include <getopt.h>
|
|
#include <boilerplate/ancillaries.h>
|
#include <alchemy/task.h>
|
#include <alchemy/timer.h>
|
|
#include <rtdm/can.h>
|
|
extern int optind, opterr, optopt;
|
|
static void print_usage(char *prg)
|
{
|
fprintf(stderr,
|
"Usage: %s <can-interface> [Options] <can-msg>\n"
|
"<can-msg> can consist of up to 8 bytes given as a space separated list\n"
|
"Options:\n"
|
" -i, --identifier=ID CAN Identifier (default = 1)\n"
|
" -r --rtr send remote request\n"
|
" -e --extended send extended frame\n"
|
" -l --loop=COUNT send message COUNT times\n"
|
" -c, --count message count in data[0-3]\n"
|
" -d, --delay=MS delay in ms (default = 1ms)\n"
|
" -s, --send use send instead of sendto\n"
|
" -t, --timeout=MS timeout in ms\n"
|
" -L, --loopback=0|1 switch local loopback off or on\n"
|
" -v, --verbose be verbose\n"
|
" -p, --print=MODULO print every MODULO message\n"
|
" -h, --help this help\n",
|
prg);
|
}
|
|
|
RT_TASK rt_task_desc;
|
|
static int s=-1, dlc=0, rtr=0, extended=0, verbose=0, loops=1;
|
static SRTIME delay=1000000;
|
static int count=0, print=1, use_send=0, loopback=-1;
|
static nanosecs_rel_t timeout = 0;
|
static struct can_frame frame;
|
static struct sockaddr_can to_addr;
|
|
|
static void cleanup(void)
|
{
|
int ret;
|
|
if (verbose)
|
printf("Cleaning up...\n");
|
|
usleep(100000);
|
|
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, j, ret;
|
|
for (i = 0; i < loops; i++) {
|
rt_task_sleep(rt_timer_ns2ticks(delay));
|
if (count)
|
memcpy(&frame.data[0], &i, sizeof(i));
|
/* Note: sendto avoids the definiton of a receive filter list */
|
if (use_send)
|
ret = send(s, (void *)&frame, sizeof(can_frame_t), 0);
|
else
|
ret = sendto(s, (void *)&frame, sizeof(can_frame_t), 0,
|
(struct sockaddr *)&to_addr, sizeof(to_addr));
|
if (ret < 0) {
|
switch (errno) {
|
case ETIMEDOUT:
|
if (verbose)
|
printf("send(to): timed out\n");
|
break;
|
case EBADF:
|
if (verbose)
|
printf("send(to): aborted because socket was closed\n");
|
break;
|
default:
|
fprintf(stderr, "send: %s\n", strerror(errno));
|
break;
|
}
|
i = loops; /* abort */
|
break;
|
}
|
if (verbose && (i % print) == 0) {
|
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);
|
for (j = 0; j < frame.can_dlc; j++) {
|
printf(" %02x", frame.data[j]);
|
}
|
printf("\n");
|
}
|
}
|
}
|
|
int main(int argc, char **argv)
|
{
|
int i, opt, ret;
|
struct can_ifreq ifr;
|
char name[32];
|
|
struct option long_options[] = {
|
{ "help", no_argument, 0, 'h' },
|
{ "identifier", required_argument, 0, 'i'},
|
{ "rtr", no_argument, 0, 'r'},
|
{ "extended", no_argument, 0, 'e'},
|
{ "verbose", no_argument, 0, 'v'},
|
{ "count", no_argument, 0, 'c'},
|
{ "print", required_argument, 0, 'p'},
|
{ "loop", required_argument, 0, 'l'},
|
{ "delay", required_argument, 0, 'd'},
|
{ "send", no_argument, 0, 's'},
|
{ "timeout", required_argument, 0, 't'},
|
{ "loopback", required_argument, 0, 'L'},
|
{ 0, 0, 0, 0},
|
};
|
|
signal(SIGTERM, cleanup_and_exit);
|
signal(SIGINT, cleanup_and_exit);
|
|
frame.can_id = 1;
|
|
while ((opt = getopt_long(argc, argv, "hvi:l:red:t:cp:sL:",
|
long_options, NULL)) != -1) {
|
switch (opt) {
|
case 'h':
|
print_usage(argv[0]);
|
exit(0);
|
|
case 'p':
|
print = strtoul(optarg, NULL, 0);
|
|
case 'v':
|
verbose = 1;
|
break;
|
|
case 'c':
|
count = 1;
|
break;
|
|
case 'l':
|
loops = strtoul(optarg, NULL, 0);
|
break;
|
|
case 'i':
|
frame.can_id = strtoul(optarg, NULL, 0);
|
break;
|
|
case 'r':
|
rtr = 1;
|
break;
|
|
case 'e':
|
extended = 1;
|
break;
|
|
case 'd':
|
delay = strtoul(optarg, NULL, 0) * 1000000LL;
|
break;
|
|
case 's':
|
use_send = 1;
|
break;
|
|
case 't':
|
timeout = strtoul(optarg, NULL, 0) * 1000000LL;
|
break;
|
|
case 'L':
|
loopback = strtoul(optarg, NULL, 0);
|
break;
|
|
default:
|
fprintf(stderr, "Unknown option %c\n", opt);
|
break;
|
}
|
}
|
|
if (optind == argc) {
|
print_usage(argv[0]);
|
exit(0);
|
}
|
|
if (argv[optind] == NULL) {
|
fprintf(stderr, "No Interface supplied\n");
|
exit(-1);
|
}
|
|
if (verbose)
|
printf("interface %s\n", argv[optind]);
|
|
ret = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
if (ret < 0) {
|
fprintf(stderr, "socket: %s\n", strerror(errno));
|
return -1;
|
}
|
s = ret;
|
|
if (loopback >= 0) {
|
ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK,
|
&loopback, sizeof(loopback));
|
if (ret < 0) {
|
fprintf(stderr, "setsockopt: %s\n", strerror(errno));
|
goto failure;
|
}
|
if (verbose)
|
printf("Using loopback=%d\n", loopback);
|
}
|
|
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: %s\n", strerror(errno));
|
goto failure;
|
}
|
|
memset(&to_addr, 0, sizeof(to_addr));
|
to_addr.can_ifindex = ifr.ifr_ifindex;
|
to_addr.can_family = AF_CAN;
|
if (use_send) {
|
/* Suppress definiton of a default receive filter list */
|
ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
|
if (ret < 0) {
|
fprintf(stderr, "setsockopt: %s\n", strerror(errno));
|
goto failure;
|
}
|
|
ret = bind(s, (struct sockaddr *)&to_addr, sizeof(to_addr));
|
if (ret < 0) {
|
fprintf(stderr, "bind: %s\n", strerror(errno));
|
goto failure;
|
}
|
}
|
|
if (count)
|
frame.can_dlc = sizeof(int);
|
else {
|
for (i = optind + 1; i < argc; i++) {
|
frame.data[dlc] = strtoul(argv[i], NULL, 0);
|
dlc++;
|
if( dlc == 8 )
|
break;
|
}
|
frame.can_dlc = dlc;
|
}
|
|
if (rtr)
|
frame.can_id |= CAN_RTR_FLAG;
|
|
if (extended)
|
frame.can_id |= CAN_EFF_FLAG;
|
|
if (timeout) {
|
if (verbose)
|
printf("Timeout: %lld ns\n", (long long)timeout);
|
ret = ioctl(s, RTCAN_RTIOC_SND_TIMEOUT, &timeout);
|
if (ret) {
|
fprintf(stderr, "ioctl SND_TIMEOUT: %s\n", strerror(errno));
|
goto failure;
|
}
|
}
|
|
snprintf(name, sizeof(name), "rtcansend-%d", getpid());
|
ret = rt_task_shadow(&rt_task_desc, name, 1, 0);
|
if (ret) {
|
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
|
goto failure;
|
}
|
|
rt_task();
|
|
cleanup();
|
return 0;
|
|
failure:
|
cleanup();
|
return -1;
|
}
|
