00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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;
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
00094 frame.can_id = can_id;
00095 frame.can_dlc = sizeof(*rtt_time);
00096
00097 pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m);
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
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, ¶m);
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
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
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
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
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
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
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
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
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, ¶m);
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
00384 printf("shutting down\n");
00385
00386
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 }