Xenomai  3.0.5
can-rtt.c
/*
* Round-Trip-Time Test - sends and receives messages and measures the
* time in between.
*
* Copyright (C) 2006 Wolfgang Grandegger <wg@grandegger.com>
*
* Based on RTnet's examples/xenomai/posix/rtt-sender.c.
*
* Copyright (C) 2002 Ulrich Marx <marx@kammer.uni-hannover.de>
* 2002 Marc Kleine-Budde <kleine-budde@gmx.de>
* 2006 Jan Kiszka <jan.kiszka@web.de>
*
* 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, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*
*
* The program sends out CAN messages periodically and copies the current
* time-stamp to the payload. At reception, that time-stamp is compared
* with the current time to determine the round-trip time. The jitter
* values are printer out regularly. Concurrent tests can be carried out
* by starting the program with different message identifiers. It is also
* possible to use this program on a remote system as simple repeater to
* loopback messages.
*/
#include <errno.h>
#include <mqueue.h>
#include <signal.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <limits.h>
#include <getopt.h>
#include <memory.h>
#include <netinet/in.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <rtdm/can.h>
#include <xenomai/init.h>
#define NSEC_PER_SEC 1000000000
static unsigned int cycle = 10000; /* 10 ms */
static canid_t can_id = 0x1;
static pthread_t txthread, rxthread;
static int txsock, rxsock;
static mqd_t mq;
static int txcount, rxcount;
static int overruns;
static int repeater;
struct rtt_stat {
long long rtt;
long long rtt_min;
long long rtt_max;
long long rtt_sum;
long long rtt_sum_last;
int counts_per_sec;
};
void application_usage(void)
{
fprintf(stderr, "usage: %s [options] <tx-can-interface> <rx-can-interface>:\n",
get_program_name());
fprintf(stderr,
" -r, --repeater Repeater, send back received messages\n"
" -i, --id=ID CAN Identifier (default = 0x1)\n"
" -c, --cycle Cycle time in us (default = 10000us)\n");
}
static void *transmitter(void *arg)
{
struct sched_param param = { .sched_priority = 80 };
struct timespec next_period;
struct timespec time;
struct can_frame frame;
long long *rtt_time = (long long *)&frame.data, t;
/* Pre-fill CAN frame */
frame.can_id = can_id;
frame.can_dlc = sizeof(*rtt_time);
pthread_setname_np(pthread_self(), "rtcan_rtt_transmitter");
pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
clock_gettime(CLOCK_MONOTONIC, &next_period);
while(1) {
next_period.tv_nsec += cycle * 1000;
while (next_period.tv_nsec >= NSEC_PER_SEC) {
next_period.tv_nsec -= NSEC_PER_SEC;
next_period.tv_sec++;
}
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL);
if (rxcount != txcount) {
overruns++;
continue;
}
clock_gettime(CLOCK_MONOTONIC, &time);
t = (long long)time.tv_sec * NSEC_PER_SEC + time.tv_nsec;
memcpy(rtt_time, &t, sizeof(t));
/* Transmit the message containing the local time */
if (send(txsock, (void *)&frame, sizeof(struct can_frame), 0) < 0) {
if (errno == EBADF)
printf("terminating transmitter thread\n");
else
perror("send failed");
return NULL;
}
txcount++;
}
}
static void *receiver(void *arg)
{
struct sched_param param = { .sched_priority = 82 };
struct timespec time;
struct can_frame frame;
long long *rtt_time = (long long *)frame.data, t;
struct rtt_stat rtt_stat = {0, 1000000000000000000LL, -1000000000000000000LL,
0, 0, 0};
pthread_setname_np(pthread_self(), "rtcan_rtt_receiver");
pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
rtt_stat.counts_per_sec = 1000000 / cycle;
while (1) {
if (recv(rxsock, (void *)&frame, sizeof(struct can_frame), 0) < 0) {
if (errno == EBADF)
printf("terminating receiver thread\n");
else
perror("recv failed");
return NULL;
}
if (repeater) {
/* Transmit the message back as is */
if (send(txsock, (void *)&frame, sizeof(struct can_frame), 0) < 0) {
if (errno == EBADF)
printf("terminating transmitter thread\n");
else
perror("send failed");
return NULL;
}
txcount++;
} else {
memcpy(&t, rtt_time, sizeof(t));
clock_gettime(CLOCK_MONOTONIC, &time);
if (rxcount > 0) {
rtt_stat.rtt = ((long long)time.tv_sec * 1000000000LL +
time.tv_nsec - t);
rtt_stat.rtt_sum += rtt_stat.rtt;
if (rtt_stat.rtt < rtt_stat.rtt_min)
rtt_stat.rtt_min = rtt_stat.rtt;
if (rtt_stat.rtt > rtt_stat.rtt_max)
rtt_stat.rtt_max = rtt_stat.rtt;
}
}
rxcount++;
if ((rxcount % rtt_stat.counts_per_sec) == 0) {
mq_send(mq, (char *)&rtt_stat, sizeof(rtt_stat), 0);
rtt_stat.rtt_sum_last = rtt_stat.rtt_sum;
}
}
}
static void catch_signal(int sig)
{
mq_close(mq);
close(rxsock);
close(txsock);
}
int main(int argc, char *argv[])
{
struct sched_param param = { .sched_priority = 1 };
pthread_attr_t thattr;
struct mq_attr mqattr;
struct sockaddr_can rxaddr, txaddr;
struct can_filter rxfilter[1];
struct rtt_stat rtt_stat;
char mqname[32];
char *txdev, *rxdev;
struct can_ifreq ifr;
int ret, opt;
struct option long_options[] = {
{ "id", required_argument, 0, 'i'},
{ "cycle", required_argument, 0, 'c'},
{ "repeater", no_argument, 0, 'r'},
{ 0, 0, 0, 0},
};
while ((opt = getopt_long(argc, argv, "ri:c:",
long_options, NULL)) != -1) {
switch (opt) {
case 'c':
cycle = atoi(optarg);
break;
case 'i':
can_id = strtoul(optarg, NULL, 0);
break;
case 'r':
repeater = 1;
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
exit(-1);
}
}
printf("%d %d\n", optind, argc);
if (optind + 2 != argc) {
xenomai_usage();
exit(0);
}
txdev = argv[optind];
rxdev = argv[optind + 1];
/* Create and configure RX socket */
if ((rxsock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
perror("RX socket failed");
return -1;
}
strncpy(ifr.ifr_name, rxdev, IFNAMSIZ);
printf("RX rxsock=%d, ifr_name=%s\n", rxsock, ifr.ifr_name);
if (ioctl(rxsock, SIOCGIFINDEX, &ifr) < 0) {
perror("RX ioctl SIOCGIFINDEX failed");
goto failure1;
}
/* We only want to receive our own messages */
rxfilter[0].can_id = can_id;
rxfilter[0].can_mask = 0x3ff;
if (setsockopt(rxsock, SOL_CAN_RAW, CAN_RAW_FILTER,
&rxfilter, sizeof(struct can_filter)) < 0) {
perror("RX setsockopt CAN_RAW_FILTER failed");
goto failure1;
}
memset(&rxaddr, 0, sizeof(rxaddr));
rxaddr.can_ifindex = ifr.ifr_ifindex;
rxaddr.can_family = AF_CAN;
if (bind(rxsock, (struct sockaddr *)&rxaddr, sizeof(rxaddr)) < 0) {
perror("RX bind failed\n");
goto failure1;
}
/* Create and configure TX socket */
if (strcmp(rxdev, txdev) == 0) {
txsock = rxsock;
} else {
if ((txsock = socket(PF_CAN, SOCK_RAW, 0)) < 0) {
perror("TX socket failed");
goto failure1;
}
strncpy(ifr.ifr_name, txdev, IFNAMSIZ);
printf("TX txsock=%d, ifr_name=%s\n", txsock, ifr.ifr_name);
if (ioctl(txsock, SIOCGIFINDEX, &ifr) < 0) {
perror("TX ioctl SIOCGIFINDEX failed");
goto failure2;
}
/* Suppress definiton of a default receive filter list */
if (setsockopt(txsock, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0) < 0) {
perror("TX setsockopt CAN_RAW_FILTER failed");
goto failure2;
}
memset(&txaddr, 0, sizeof(txaddr));
txaddr.can_ifindex = ifr.ifr_ifindex;
txaddr.can_family = AF_CAN;
if (bind(txsock, (struct sockaddr *)&txaddr, sizeof(txaddr)) < 0) {
perror("TX bind failed\n");
goto failure2;
}
}
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
signal(SIGHUP, catch_signal);
printf("Round-Trip-Time test %s -> %s with CAN ID 0x%x\n",
argv[optind], argv[optind + 1], can_id);
printf("Cycle time: %d us\n", cycle);
printf("All RTT timing figures are in us.\n");
/* Create statistics message queue */
snprintf(mqname, sizeof(mqname), "/rtcan_rtt-%d", getpid());
mqattr.mq_flags = 0;
mqattr.mq_maxmsg = 100;
mqattr.mq_msgsize = sizeof(struct rtt_stat);
mq = mq_open(mqname, O_RDWR | O_CREAT | O_EXCL, 0600, &mqattr);
if (mq == (mqd_t)-1) {
perror("opening mqueue failed");
goto failure2;
}
/* Create receiver RT-thread */
pthread_attr_init(&thattr);
pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE);
ret = pthread_create(&rxthread, &thattr, &receiver, NULL);
if (ret) {
fprintf(stderr, "%s: pthread_create(receiver) failed\n",
strerror(-ret));
goto failure3;
}
if (!repeater) {
/* Create transitter RT-thread */
ret = pthread_create(&txthread, &thattr, &transmitter, NULL);
if (ret) {
fprintf(stderr, "%s: pthread_create(transmitter) failed\n",
strerror(-ret));
goto failure4;
}
}
pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
if (repeater)
printf("Messages\n");
else
printf("Messages RTTlast RTT_avg RTT_min RTT_max Overruns\n");
while (1) {
long long rtt_avg;
ret = mq_receive(mq, (char *)&rtt_stat, sizeof(rtt_stat), NULL);
if (ret != sizeof(rtt_stat)) {
if (ret < 0) {
if (errno == EBADF)
printf("terminating mq_receive\n");
else
perror("mq_receive failed");
} else
fprintf(stderr,
"mq_receive returned invalid length %d\n", ret);
break;
}
if (repeater) {
printf("%8d\n", rxcount);
} else {
rtt_avg = ((rtt_stat.rtt_sum - rtt_stat.rtt_sum_last) /
rtt_stat.counts_per_sec);
printf("%8d %7ld %7ld %7ld %7ld %8d\n", rxcount,
(long)(rtt_stat.rtt / 1000), (long)(rtt_avg / 1000),
(long)(rtt_stat.rtt_min / 1000),
(long)(rtt_stat.rtt_max / 1000),
overruns);
}
}
/* This call also leaves primary mode, required for socket cleanup. */
printf("shutting down\n");
/* Important: First close the sockets! */
close(rxsock);
close(txsock);
pthread_join(txthread, NULL);
pthread_cancel(rxthread);
pthread_join(rxthread, NULL);
return 0;
failure4:
pthread_cancel(rxthread);
pthread_join(rxthread, NULL);
failure3:
mq_close(mq);
failure2:
close(txsock);
failure1:
close(rxsock);
return 1;
}