rtcan_rtt.c

00001 /*
00002  * Round-Trip-Time Test - sends and receives messages and measures the
00003  *                        time in between.
00004  *
00005  * Copyright (C) 2006 Wolfgang Grandegger <wg@grandegger.com>
00006  *
00007  * Based on RTnet's examples/xenomai/posix/rtt-sender.c.
00008  *
00009  * Copyright (C) 2002 Ulrich Marx <marx@kammer.uni-hannover.de>
00010  *               2002 Marc Kleine-Budde <kleine-budde@gmx.de>
00011  *               2006 Jan Kiszka <jan.kiszka@web.de>
00012  *
00013  * This program is free software; you can redistribute it and/or modify
00014  * it under the terms of the GNU General Public License as published by
00015  * the Free Software Foundation; either version 2 of the License, or
00016  * (at your option) any later version.
00017  *
00018  * This program is distributed in the hope that it will be useful,
00019  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00020  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00021  * GNU General Public License for more details.
00022  *
00023  * You should have received a copy of the GNU General Public License
00024  * along with this program; if not, write to the Free Software
00025  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00026  *
00027  *
00028  * The program sends out CAN messages periodically and copies the current
00029  * time-stamp to the payload. At reception, that time-stamp is compared
00030  * with the current time to determine the round-trip time. The jitter
00031  * values are printer out regularly. Concurrent tests can be carried out
00032  * by starting the program with different message identifiers. It is also
00033  * possible to use this program on a remote system as simple repeater to
00034  * loopback messages.
00035  */
00036 
00037 #include <errno.h>
00038 #include <mqueue.h>
00039 #include <signal.h>
00040 #include <pthread.h>
00041 #include <stdio.h>
00042 #include <stdlib.h>
00043 #include <string.h>
00044 #include <unistd.h>
00045 #include <limits.h>
00046 #include <getopt.h>
00047 #include <netinet/in.h>
00048 #include <sys/mman.h>
00049 
00050 #include <rtdm/rtcan.h>
00051 
00052 #define NSEC_PER_SEC 1000000000
00053 
00054 static unsigned int cycle = 10000; /* 10 ms */
00055 static can_id_t can_id = 0x1;
00056 
00057 static pthread_t txthread, rxthread;
00058 static int txsock, rxsock;
00059 static mqd_t mq;
00060 static int txcount, rxcount;
00061 static int overruns;
00062 static int repeater;
00063 
00064 struct rtt_stat {
00065     long long rtt;
00066     long long rtt_min;
00067     long long rtt_max;
00068     long long rtt_sum;
00069     long long rtt_sum_last;
00070     int counts_per_sec;
00071 };
00072 
00073 static void print_usage(char *prg)
00074 {
00075     fprintf(stderr,
00076             "Usage: %s  [Options] <tx-can-interface> <rx-can-interface>\n"
00077             "Options:\n"
00078             " -h, --help     This help\n"
00079             " -r, --repeater Repeater, send back received messages\n"
00080             " -i, --id=ID    CAN Identifier (default = 0x1)\n"
00081             " -c, --cycle    Cycle time in us (default = 10000us)\n",
00082             prg);
00083 }
00084 
00085 void *transmitter(void *arg)
00086 {
00087     struct sched_param  param = { .sched_priority = 80 };
00088     struct timespec next_period;
00089     struct timespec time;
00090     struct can_frame frame;
00091     long long *rtt_time = (long long *)&frame.data;
00092 
00093     /* Pre-fill CAN frame */
00094     frame.can_id = can_id;
00095     frame.can_dlc = sizeof(*rtt_time);
00096 
00097     pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
00098 
00099     clock_gettime(CLOCK_MONOTONIC, &next_period);
00100 
00101     while(1) {
00102         next_period.tv_nsec += cycle * 1000;
00103         while (next_period.tv_nsec >= NSEC_PER_SEC) {
00104                 next_period.tv_nsec -= NSEC_PER_SEC;
00105                 next_period.tv_sec++;
00106         }
00107 
00108         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL);
00109 
00110         if (rxcount != txcount) {
00111             overruns++;
00112             continue;
00113         }
00114 
00115         clock_gettime(CLOCK_MONOTONIC, &time);
00116         *rtt_time = time.tv_sec * NSEC_PER_SEC + time.tv_nsec;
00117 
00118         /* Transmit the message containing the local time */
00119         if (send(txsock, (void *)&frame, sizeof(can_frame_t), 0) < 0) {
00120             if (errno == EBADF)
00121                 printf("terminating transmitter thread\n");
00122             else
00123                 perror("send failed");
00124             return NULL;
00125         }
00126         txcount++;
00127     }
00128 }
00129 
00130 
00131 void *receiver(void *arg)
00132 {
00133     struct sched_param param = { .sched_priority = 82 };
00134     struct timespec time;
00135     struct can_frame frame;
00136     long long *rtt_time = (long long *)frame.data;
00137     struct rtt_stat rtt_stat = {0, 1000000000000000000LL, -1000000000000000000LL,
00138                                 0, 0, 0};
00139     pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
00140 
00141     rtt_stat.counts_per_sec = 1000000 / cycle;
00142 
00143     while (1) {
00144         if (recv(rxsock, (void *)&frame, sizeof(can_frame_t), 0) < 0) {
00145             if (errno == EBADF)
00146                 printf("terminating receiver thread\n");
00147             else
00148                 perror("recv failed");
00149             return NULL;
00150         }
00151         if (repeater) {
00152             /* Transmit the message back as is */
00153             if (send(txsock, (void *)&frame, sizeof(can_frame_t), 0) < 0) {
00154                 if (errno == EBADF)
00155                     printf("terminating transmitter thread\n");
00156                 else
00157                     perror("send failed");
00158                 return NULL;
00159             }
00160             txcount++;
00161         } else {
00162             clock_gettime(CLOCK_MONOTONIC, &time);
00163             if (rxcount > 0) {
00164                 rtt_stat.rtt = (time.tv_sec * 1000000000LL +
00165                                 time.tv_nsec - *rtt_time);
00166                 rtt_stat.rtt_sum += rtt_stat.rtt;
00167                 if (rtt_stat.rtt <  rtt_stat.rtt_min)
00168                     rtt_stat.rtt_min = rtt_stat.rtt;
00169                 if (rtt_stat.rtt > rtt_stat.rtt_max)
00170                     rtt_stat.rtt_max = rtt_stat.rtt;
00171             }
00172         }
00173         rxcount++;
00174 
00175         if ((rxcount % rtt_stat.counts_per_sec) == 0) {
00176             mq_send(mq, (char *)&rtt_stat, sizeof(rtt_stat), 0);
00177             rtt_stat.rtt_sum_last = rtt_stat.rtt_sum;
00178         }
00179     }
00180 }
00181 
00182 void catch_signal(int sig)
00183 {
00184     mq_close(mq);
00185 }
00186 
00187 
00188 int main(int argc, char *argv[])
00189 {
00190     struct sched_param param = { .sched_priority = 1 };
00191     pthread_attr_t thattr;
00192     struct mq_attr mqattr;
00193     struct sockaddr_can rxaddr, txaddr;
00194     struct can_filter rxfilter[1];
00195     struct rtt_stat rtt_stat;
00196     char mqname[32];
00197     char *txdev, *rxdev;
00198     struct ifreq ifr;
00199     int ret, opt;
00200 
00201     struct option long_options[] = {
00202         { "id", required_argument, 0, 'i'},
00203         { "cycle", required_argument, 0, 'c'},
00204         { "repeater", required_argument, 0, 'r'},
00205         { "help", no_argument, 0, 'h'},
00206         { 0, 0, 0, 0},
00207     };
00208 
00209     while ((opt = getopt_long(argc, argv, "hri:c:",
00210                               long_options, NULL)) != -1) {
00211         switch (opt) {
00212         case 'c':
00213             cycle = atoi(optarg);
00214             break;
00215 
00216         case 'i':
00217             can_id = strtoul(optarg, NULL, 0);
00218             break;
00219 
00220         case 'r':
00221             repeater = 1;
00222             break;
00223 
00224         default:
00225             fprintf(stderr, "Unknown option %c\n", opt);
00226         case 'h':
00227             print_usage(argv[0]);
00228             exit(-1);
00229         }
00230     }
00231 
00232     printf("%d %d\n", optind, argc);
00233     if (optind + 2 != argc) {
00234         print_usage(argv[0]);
00235         exit(0);
00236     }
00237 
00238     txdev = argv[optind];
00239     rxdev = argv[optind + 1];
00240 
00241     /* Create and configure RX socket */
00242     if ((rxsock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
00243         perror("RX socket failed");
00244         return -1;
00245     }
00246 
00247     strncpy(ifr.ifr_name, rxdev, IFNAMSIZ);
00248     printf("RX rxsock=%d, ifr_name=%s\n", rxsock, ifr.ifr_name);
00249 
00250     if (ioctl(rxsock, SIOCGIFINDEX, &ifr) < 0) {
00251         perror("RX ioctl SIOCGIFINDEX failed");
00252         goto failure1;
00253     }
00254 
00255     /* We only want to receive our own messages */
00256     rxfilter[0].can_id = can_id;
00257     rxfilter[0].can_mask = 0x3ff;
00258     if (setsockopt(rxsock, SOL_CAN_RAW, CAN_RAW_FILTER,
00259                    &rxfilter, sizeof(struct can_filter)) < 0) {
00260         perror("RX setsockopt CAN_RAW_FILTER failed");
00261         goto failure1;
00262     }
00263     memset(&rxaddr, 0, sizeof(rxaddr));
00264     rxaddr.can_ifindex = ifr.ifr_ifindex;
00265     rxaddr.can_family = AF_CAN;
00266     if (bind(rxsock, (struct sockaddr *)&rxaddr, sizeof(rxaddr)) < 0) {
00267         perror("RX bind failed\n");
00268         goto failure1;
00269     }
00270 
00271     /* Create and configure TX socket */
00272 
00273     if (strcmp(rxdev, txdev) == 0) {
00274         txsock = rxsock;
00275     } else {
00276         if ((txsock = socket(PF_CAN, SOCK_RAW, 0)) < 0) {
00277             perror("TX socket failed");
00278             goto failure1;
00279         }
00280 
00281         strncpy(ifr.ifr_name, txdev, IFNAMSIZ);
00282         printf("TX txsock=%d, ifr_name=%s\n", txsock, ifr.ifr_name);
00283 
00284         if (ioctl(txsock, SIOCGIFINDEX, &ifr) < 0) {
00285             perror("TX ioctl SIOCGIFINDEX failed");
00286             goto failure2;
00287         }
00288 
00289         /* Suppress definiton of a default receive filter list */
00290         if (setsockopt(txsock, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0) < 0) {
00291             perror("TX setsockopt CAN_RAW_FILTER failed");
00292             goto failure2;
00293         }
00294 
00295         memset(&txaddr, 0, sizeof(txaddr));
00296         txaddr.can_ifindex = ifr.ifr_ifindex;
00297         txaddr.can_family = AF_CAN;
00298 
00299         if (bind(txsock, (struct sockaddr *)&txaddr, sizeof(txaddr)) < 0) {
00300                 perror("TX bind failed\n");
00301                 goto failure2;
00302         }
00303     }
00304 
00305     signal(SIGTERM, catch_signal);
00306     signal(SIGINT, catch_signal);
00307     signal(SIGHUP, catch_signal);
00308     mlockall(MCL_CURRENT|MCL_FUTURE);
00309 
00310     printf("Round-Trip-Time test %s -> %s with CAN ID 0x%x\n",
00311            argv[optind], argv[optind + 1], can_id);
00312     printf("Cycle time: %d us\n", cycle);
00313     printf("All RTT timing figures are in us.\n");
00314 
00315     /* Create statistics message queue */
00316     snprintf(mqname, sizeof(mqname), "/rtcan_rtt-%d", getpid());
00317     mqattr.mq_flags   = 0;
00318     mqattr.mq_maxmsg  = 100;
00319     mqattr.mq_msgsize = sizeof(struct rtt_stat);
00320     mq = mq_open(mqname, O_RDWR | O_CREAT | O_EXCL, 0600, &mqattr);
00321     if (mq == (mqd_t)-1) {
00322         perror("opening mqueue failed");
00323         goto failure2;
00324     }
00325 
00326     /* Create receiver RT-thread */
00327     pthread_attr_init(&thattr);
00328     pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE);
00329     pthread_attr_setstacksize(&thattr, PTHREAD_STACK_MIN);
00330     ret = pthread_create(&rxthread, &thattr, &receiver, NULL);
00331     if (ret) {
00332         fprintf(stderr, "%s: pthread_create(receiver) failed\n",
00333                 strerror(-ret));
00334         goto failure3;
00335     }
00336 
00337     if (!repeater) {
00338         /* Create transitter RT-thread */
00339         ret = pthread_create(&txthread, &thattr, &transmitter, NULL);
00340         if (ret) {
00341             fprintf(stderr, "%s: pthread_create(transmitter) failed\n",
00342                     strerror(-ret));
00343             goto failure4;
00344         }
00345     }
00346 
00347     pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
00348 
00349     if (repeater)
00350         printf("Messages\n");
00351     else
00352         printf("Messages RTTlast RTT_avg RTT_min RTT_max Overruns\n");
00353 
00354     while (1) {
00355         long long rtt_avg;
00356 
00357         ret = mq_receive(mq, (char *)&rtt_stat, sizeof(rtt_stat), NULL);
00358         if (ret != sizeof(rtt_stat)) {
00359             if (ret < 0) {
00360                 if (errno == EBADF)
00361                     printf("terminating mq_receive\n");
00362                 else
00363                     perror("mq_receive failed");
00364             } else
00365                 fprintf(stderr,
00366                         "mq_receive returned invalid length %d\n", ret);
00367             break;
00368         }
00369 
00370         if (repeater) {
00371             printf("%8d\n", rxcount);
00372         } else {
00373             rtt_avg = ((rtt_stat.rtt_sum - rtt_stat.rtt_sum_last) /
00374                        rtt_stat.counts_per_sec);
00375             printf("%8d %7ld %7ld %7ld %7ld %8d\n", rxcount,
00376                    (long)(rtt_stat.rtt / 1000), (long)(rtt_avg / 1000),
00377                    (long)(rtt_stat.rtt_min / 1000),
00378                    (long)(rtt_stat.rtt_max / 1000),
00379                    overruns);
00380         }
00381     }
00382 
00383     /* This call also leaves primary mode, required for socket cleanup. */
00384     printf("shutting down\n");
00385 
00386     /* Important: First close the sockets! */
00387     while ((close(rxsock) < 0) && (errno == EAGAIN)) {
00388         printf("RX socket busy - waiting...\n");
00389         sleep(1);
00390     }
00391     while ((close(txsock) < 0) && (errno == EAGAIN)) {
00392         printf("TX socket busy - waiting...\n");
00393         sleep(1);
00394     }
00395 
00396     pthread_join(txthread, NULL);
00397     pthread_kill(rxthread, SIGHUP);
00398     pthread_join(rxthread, NULL);
00399 
00400     return 0;
00401 
00402  failure4:
00403     pthread_kill(rxthread, SIGHUP);
00404     pthread_join(rxthread, NULL);
00405  failure3:
00406     mq_close(mq);
00407  failure2:
00408     close(txsock);
00409  failure1:
00410     close(rxsock);
00411 
00412     return 1;
00413 }

Generated on Mon Mar 24 18:02:40 2008 for Xenomai API by  doxygen 1.5.3