src/vca_canping.c

   1 /**************************************************************************/
   2 /* File: vca_canping.c - utility to test CAN functionality and throughput */
   3 /*                                                                        */
   4 /* LibVCA - Versatile CAN/CANopen API library                             */
   5 /* Copyright (C) 2005-2006 Michal Sojka, DCE FEE CTU Prague               */
   6 /* Copyright (C) 2006-2009 Pavel Pisa <pisa@cmp.felk.cvut.cz>             */
   7 /*                                                                        */
   8 /* LibVCA is free software; you can redistribute it and/or modify it      */
   9 /* under terms of the GNU General Public License as published by the      */
  10 /* Free Software Foundation; either version 2, or (at your option) any    */
  11 /* later version.  LinCAN is distributed in the hope that it will be      */
  12 /* useful, but WITHOUT ANY WARRANTY; without even the implied warranty    */
  13 /* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU    */
  14 /* General Public License for more details. You should have received a    */
  15 /* copy of the GNU General Public License along with LinCAN; see file     */
  16 /* COPYING. If not, write to the Free Software Foundation, 675 Mass Ave,  */
  17 /* Cambridge, MA 02139, USA.                                              */
  18 /**************************************************************************/
  19
  20 #include <string.h>
  21 #include <math.h>
  22 #include <stdio.h>
  23 #include <unistd.h>
  24 #include <fcntl.h>
  25 #include <stdlib.h>
  26 #include <ctype.h>
  27 #include <pthread.h>
  28 #include <signal.h>
  29 #include <sys/types.h>
  30 #include <sys/time.h>
  31 #include <ul_list.h>
  32 #include <errno.h>
  33 #include <semaphore.h>
  34 #include <stdbool.h>
  35 #include <error.h>
  36
  37 /* TODO: Handle the case where there are more canping slaves running
  38  * on one CAN bus. */
  39
  40 #include <can_vca.h>
  41
  42 //#define DEBUG 1
  43 //#define DEBUG 2
  44 #define WITH_RTPRIO
  45
  46 #ifdef WITH_RTPRIO
  47 #include <sched.h>
  48
  49 int sched_policy = SCHED_OTHER;
  50 int sched_rtprio;
  51
  52 #endif
  53
  54
  55 #ifndef DEBUG
  56 #define dbg(level, fmt, arg...) do {} while (0)
  57 #else
  58 #define dbg(level, fmt, arg...) do { if (level <= DEBUG) { printf("candping: " fmt, ## arg); } } while (0)
  59 #endif
  60
  61 #define NOT_INTERRUPTED_SYSCALL (errno != EINTR && errno != ERESTART)
  62 #define IS_FINISH_FLAG() (finish_flag)
  63
  64 /* Exit codes */
  65 #define EXIT_OK 0
  66 #define EXIT_BAD_PARAM 1
  67 #define EXIT_CANNOT_OPEN 2
  68 #define EXIT_FILTER_ERROR 3
  69 #define EXIT_NO_MEM 4
  70 #define EXIT_READ_ERROR 5
  71 #define EXIT_WRITE_ERROR 6
  72 #define EXIT_SELECT_ERROR 7
  73 #define EXIT_FLUSH_ERROR 8
  74
  75 /* Global variables */
  76 sig_atomic_t finish_flag = 0;   /* Threads should terminate. */
  77 sem_t finish_sem;               /* Thread signals a termination */
  78 vca_handle_t global_vcah = VCA_HANDLE_INVALID;
  79
  80 int total_count = 0;
  81 int total_timeout = 0;
  82
  83 pthread_mutex_t mut_start = PTHREAD_MUTEX_INITIALIZER;
  84 pthread_cond_t cond_start = PTHREAD_COND_INITIALIZER;
  85 int start_flag = 0;
  86 sem_t ready_sem;                /* Thread is ready for execution */
  87
  88
  89 /* Command line options */
  90 char *option_device = "/dev/can0";
  91 int option_masters = 0;
  92 long int option_first_id = 1000;
  93 int option_slaves = 0;
  94 int option_verbose = 0;         /* 0 - nothing, 1 - only global
  95                                  * statistics, 2 - simple times, 3 -
  96                                  * verbose times */
  97 int option_length = 8;
  98 int option_count = 0;
  99 int option_wait_ms = 1000;
 100 int option_timeout = 4;
 101 int option_open_once = 0;
 102 int option_synch_start = 0;
 103 bool option_background = false;
 104 char *option_histogram = NULL;
 105
 106 /* Lists */
 107 typedef struct threads {
 108         ul_list_head_t head;
 109 } threads_t;
 110
 111 struct histogram {
 112         unsigned *data;
 113         unsigned allocated;
 114         unsigned resolution;
 115 };
 116
 117 typedef struct thread_data {
 118         pthread_t tid;
 119         long int canid;
 120
 121         int count;
 122         double mean;            /* mean value of responses */
 123         double moment2nd;       /* used to compute variance of
 124                                  * responses */
 125         int min, max;           /* min/max response times */
 126         int timeout;            /* number of timeouts */
 127
 128         struct histogram hist;
 129
 130         ul_list_node_t node;
 131 } thread_data_t;
 132
 133 UL_LIST_CUST_DEC(thread_list, threads_t, thread_data_t, head, node);
 134
 135 threads_t master_threads;
 136 threads_t slave_threads;
 137
 138 int histogram_init(struct histogram *h,
 139                    unsigned max_value,
 140                    unsigned resolution)
 141 {
 142         size_t mem;
 143         h->allocated = max_value/resolution + 1;
 144         h->resolution = resolution;
 145         mem = h->allocated*sizeof(*h->data);
 146         h->data = malloc(mem);
 147         if (h->data) {
 148                 memset(h->data, 0, mem);
 149                 return 0;
 150         } else
 151                 return -1;
 152 }
 153
 154 void histogram_add(struct histogram *h, unsigned value)
 155 {
 156         unsigned index = value / h->resolution;
 157         if (index >= h->allocated)
 158                 index = h->allocated - 1;
 159         h->data[index]++;
 160 }
 161
 162 void histogram_fprint(struct histogram *h, FILE *f)
 163 {
 164         unsigned long long sum = 0, cum;
 165         unsigned i;
 166         for (i = 0; i < h->allocated; i++)
 167                 sum += h->data[i];
 168         cum = sum;
 169         for (i = 0; i < h->allocated; i++) {
 170                 if (h->data[i] != 0)
 171                         fprintf(f, "%d %lld\n", i*h->resolution, cum);
 172                 cum -= h->data[i];
 173         }
 174 }
 175
 176 /* Subtract the `struct timeval' values X and Y, storing the result in
 177    RESULT.  Return 1 if the difference is negative, otherwise 0.  */
 178
 179 int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y)
 180 {
 181         /* Perform the carry for the later subtraction by updating Y. */
 182         if (x->tv_usec < y->tv_usec) {
 183                 int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
 184                 y->tv_usec -= 1000000 * nsec;
 185                 y->tv_sec += nsec;
 186         }
 187         if (x->tv_usec - y->tv_usec > 1000000) {
 188                 int nsec = (x->tv_usec - y->tv_usec) / 1000000;
 189                 y->tv_usec += 1000000 * nsec;
 190                 y->tv_sec -= nsec;
 191         }
 192
 193         /* Compute the time remaining to wait.
 194            `tv_usec' is certainly positive. */
 195         result->tv_sec = x->tv_sec - y->tv_sec;
 196         result->tv_usec = x->tv_usec - y->tv_usec;
 197
 198         /* Return 1 if result is negative. */
 199         return x->tv_sec < y->tv_sec;
 200 }
 201
 202 void dbg_print_timeval(char *msg, struct timeval *tv)
 203 {
 204
 205         printf("%s sec=%ld usec=%ld\n", msg, tv->tv_sec, tv->tv_usec);
 206 }
 207 void kill_all_threads(int signum)
 208 {
 209         thread_data_t *td;
 210
 211         ul_list_for_each(thread_list, &master_threads, td) {
 212                 pthread_kill(td->tid, signum);
 213         }
 214         ul_list_for_each(thread_list, &slave_threads, td) {
 215                 pthread_kill(td->tid, signum);
 216         }
 217 }
 218 void term_handler(int signum)
 219 {
 220         dbg(1, "Thread %p got a signal %d\n", (void *)pthread_self(), signum);
 221         if (!IS_FINISH_FLAG()) {
 222                 dbg(1, "Terminating threads\n");
 223                 finish_flag = 1;
 224
 225                 kill_all_threads(signum);
 226         }
 227 }
 228
 229 void *master_thread(void *arg)
 230 {
 231         thread_data_t *td = (thread_data_t *)arg;
 232         int ping_id = td->canid;
 233         int pong_id = ping_id + 1;
 234         struct canmsg_t pingmsg, pongmsg;
 235         int ping_count = 0;
 236         int ret = 0;
 237         vca_handle_t vcah = VCA_HANDLE_INVALID; /* File descriptor of CAN driver. */
 238         int i;
 239         fd_set rdset;           /* read set for select syscall */
 240         struct timeval timeout, pingtime, pongtime;
 241         struct canfilt_t canfilt; /* filter for received messages */
 242
 243         if (!option_open_once) {
 244                 /* Open can driver */
 245                 if(vca_open_handle(&vcah, option_device, NULL, 0) < 0) {
 246                         perror("open");
 247                         fprintf(stderr, "Error opening %s (for id %d)\n", option_device, ping_id);
 248                         exit(EXIT_CANNOT_OPEN);
 249                 }
 250         } else {
 251                 vcah = global_vcah;
 252         }
 253
 254         /* setup filtering of received messages */
 255         memset(&canfilt, 0, sizeof(canfilt));
 256         canfilt.mask = 0xfffffff;
 257         canfilt.id = pong_id;   /* pong responces with increased id */
 258         ret = vca_set_filt(vcah, &canfilt);
 259         if(ret<0) {
 260                 perror("ioctl CANQUE_FILTER");
 261                 exit(EXIT_FILTER_ERROR);
 262         }
 263         ret = vca_queue_flush(vcah, 0);
 264         if(ret<0) {
 265                 perror("ioctl CANQUE_QUEUE_FLUSH");
 266                 exit(EXIT_FLUSH_ERROR);
 267         }
 268
 269         if (option_histogram)
 270                 histogram_init(&td->hist, 1000*1000/*max [us]*/, 5/*resolution [us]*/);
 271
 272         /* Prepare data structures for the select syscall. */
 273         FD_ZERO (&rdset);
 274
 275         /* Signal that I'm ready */
 276         sem_post(&ready_sem);
 277
 278         if (option_synch_start) {
 279                 /* Wait for other threads to initialize */
 280                 pthread_mutex_lock(&mut_start);
 281                 while (!start_flag) pthread_cond_wait(&cond_start, &mut_start);
 282                 pthread_mutex_unlock(&mut_start);
 283         }
 284
 285         while (!IS_FINISH_FLAG() && (option_count == 0 || ping_count++ < option_count)) {
 286                 /* Send a ping message */
 287                 pingmsg.flags=0;
 288                 pingmsg.id=ping_id;
 289                 pingmsg.length = option_length;
 290                 for (i=0; i < option_length; i++) pingmsg.data[i] = i;
 291                 gettimeofday(&pingtime, NULL);
 292
 293                 ret = vca_send_msg_seq(vcah, &pingmsg, 1);
 294                 if (ret < 0) {
 295                         if (NOT_INTERRUPTED_SYSCALL) {
 296                                 perror("write");
 297                                 exit(EXIT_WRITE_ERROR);
 298                         }
 299                         else continue;
 300                 }
 301
 302                 /* Wait for a pong responce */
 303                 FD_SET (vca_h2fd(vcah), &rdset);
 304
 305                 timeout.tv_sec = option_timeout;
 306                 timeout.tv_usec = 0;
 307
 308                 ret = select(FD_SETSIZE, &rdset, NULL, NULL, &timeout);
 309                 if (ret > 0) {
 310                         /* Read the message */
 311                         pongmsg.flags=0;
 312                         ret = vca_rec_msg_seq(vcah, &pongmsg, 1);
 313                         if (ret < 0) {
 314                                 if (NOT_INTERRUPTED_SYSCALL) {
 315                                         perror("read");
 316                                         exit(EXIT_READ_ERROR);
 317                                 }
 318                                 else continue;
 319                         }
 320                         else {  /* A pong message received */
 321                                 long int time;
 322                                 gettimeofday(&pongtime, NULL);
 323                                 if (ret == 0) {
 324                                         fprintf(stderr, "read returned zero\n");
 325                                         exit(EXIT_READ_ERROR);
 326                                 }
 327                                 if (pongmsg.id != pong_id) {
 328                                         fprintf(stderr, "Filter error (expected: %d, received %ld)\n", pong_id, pongmsg.id);
 329                                         exit(EXIT_FILTER_ERROR);
 330                                 }
 331                                 timeval_subtract(&pongtime, &pongtime, &pingtime);
 332                                 time = pongtime.tv_sec*1000000 + pongtime.tv_usec;
 333                                 switch (option_verbose) {
 334                                 case 2:
 335                                         printf("%d:%ld\n", ping_id, time);
 336                                         break;
 337                                 case 3:
 338                                         printf("Pong response for id %d received in %ld us\n", ping_id, time);
 339                                         break;
 340                                 }
 341                                 /* Update statistics */
 342                                 td->count++;
 343                                 td->mean =
 344                                         td->mean * ((double)(td->count - 1) / td->count) +
 345                                         (double)time / td->count;
 346                                 td->moment2nd =
 347                                         td->moment2nd * ((double)(td->count - 1) / td->count) +
 348                                         (double)time*time  / td->count;
 349                                 if (time > td->max) td->max = time;
 350                                 if (time < td->min) td->min = time;
 351                                 if (option_histogram)
 352                                         histogram_add(&td->hist, time);
 353                                 total_count++;
 354                         } /* read */
 355                 }
 356                 else if (ret == 0) { /* select */
 357                         if (option_verbose >= 2)
 358                                 printf("Timeout encountered (id %d)\n", ping_id);
 359                         td->timeout++;
 360                         total_timeout++;
 361                 }
 362                 else {
 363                         if (NOT_INTERRUPTED_SYSCALL) {
 364                                 perror("select");
 365                                 exit(EXIT_SELECT_ERROR);
 366                         }
 367                         else continue;
 368                 }
 369                 usleep(option_wait_ms * 1000);
 370         }
 371
 372         if (!option_open_once) {
 373                 /* Close the can driver */
 374                 vca_close_handle(vcah);
 375         }
 376
 377         if (option_histogram) {
 378                 FILE *f;
 379                 char buf[100];
 380                 snprintf(buf, sizeof(buf), "%s-%d.dat", option_histogram, ping_id);
 381                 f = fopen(buf, "w");
 382                 histogram_fprint(&td->hist, f);
 383                 fclose(f);
 384         }
 385
 386         sem_post(&finish_sem);
 387         return (void *)ret;
 388 }
 389
 390 void start_masters(int masters, int first_id)
 391 {
 392         int id = first_id;
 393         int i;
 394
 395         dbg(1, "Starting %d master threads\n", masters);
 396
 397         for (i = 0; i < masters; i++, id += 2) {
 398                 thread_data_t *td;
 399
 400                 td = malloc(sizeof(*td));
 401                 if (!td) {
 402                         printf("Can't allocate memory");
 403                         exit(EXIT_NO_MEM);
 404                 }
 405                 memset(td, 0, sizeof(*td));
 406                 td->canid = id;
 407                 td->min = 0x7fffffff;
 408                 /* TODO use mutexes and signal blocking */
 409                 thread_list_ins_tail(&master_threads, td);
 410                 pthread_create(&td->tid, NULL, master_thread, (void *)td);
 411                 dbg(2, "Master thread: %p\n", (void *)td->tid);
 412         }
 413
 414
 415         /* Wait for all threads beeing ready */
 416         for (i = 0; i < masters; i++) sem_wait(&ready_sem);
 417
 418         /* Start threads */
 419         pthread_mutex_lock(&mut_start);
 420         start_flag = 1;
 421         pthread_cond_broadcast(&cond_start);
 422         pthread_mutex_unlock(&mut_start);
 423 }
 424
 425 void *slave_thread(void *arg)
 426 {
 427         thread_data_t *td = (thread_data_t *)arg;
 428         int ping_id = td->canid;
 429         int pong_id = ping_id + 1;
 430         struct canmsg_t pingmsg, pongmsg;
 431         int ret = 0;
 432         vca_handle_t vcah = VCA_HANDLE_INVALID; /* File descriptor of CAN driver. */
 433         int i;
 434         struct canfilt_t canfilt; /* filter for received messages */
 435
 436         if (!option_open_once) {
 437                 /* Open the CAN driver */
 438                 if(vca_open_handle(&vcah, option_device, NULL, 0) < 0) {
 439                         perror("open");
 440                         printf("Error opening %s (for id %d)\n", option_device, ping_id);
 441                         exit(EXIT_CANNOT_OPEN);
 442                 }
 443         } else {
 444                 vcah = global_vcah;
 445         }
 446
 447         /* setup filtering of received messages */
 448         memset(&canfilt, 0, sizeof(canfilt));
 449         canfilt.mask = 0xfffffff;
 450         canfilt.id = ping_id;   /* receive only our ping messages */
 451         ret = vca_set_filt(vcah, &canfilt);
 452         if (ret < 0) {
 453                 perror("ioctl CANQUE_FILTER");
 454                 exit(EXIT_FILTER_ERROR);
 455         }
 456         /* If there are some messages already, delete them. These may
 457          * not processed by our filter and thus may have a wrong
 458          * ID. */
 459         ret = vca_queue_flush(vcah, 0);
 460         if (ret < 0) {
 461                 perror("ioctl CANQUE_QUEUE_FLUSH");
 462                 exit(EXIT_FLUSH_ERROR);
 463         }
 464
 465         /* Prepare a pong message */
 466         pongmsg.flags = 0;
 467         pongmsg.id = pong_id;
 468         pongmsg.length = option_length;
 469         for (i=0; i < option_length; i++) pongmsg.data[i] = i;
 470
 471         /* Signal that I'm ready */
 472         sem_post(&ready_sem);
 473
 474         while (!IS_FINISH_FLAG()) {
 475                 /* Receive a ping message */
 476                 pingmsg.flags=0;
 477                 ret = vca_rec_msg_seq(vcah, &pingmsg, 1);
 478                 if (ret < 0) {
 479                         if (NOT_INTERRUPTED_SYSCALL) {
 480                                 printf("%d\n", errno);
 481                                 perror("read");
 482                                 exit(EXIT_READ_ERROR);
 483                         }
 484                 }
 485                 if (NOT_INTERRUPTED_SYSCALL && pingmsg.id != ping_id) {
 486                         fprintf(stderr, "Filter error (expected: %d, received %ld)\n", ping_id, pingmsg.id);
 487                         exit(EXIT_FILTER_ERROR);
 488                 }
 489                 /* Answer immendiately with a pong message */
 490                 if (NOT_INTERRUPTED_SYSCALL) {
 491                         ret = vca_send_msg_seq(vcah, &pongmsg, 1);
 492                         if (ret < 0) {
 493                                 if (NOT_INTERRUPTED_SYSCALL) {
 494                                         perror("write");
 495                                         exit(EXIT_WRITE_ERROR);
 496                                 }
 497                         }
 498                 }
 499
 500                 if (ret >= 0) total_count++;
 501
 502                 if (option_verbose >= 2)
 503                         /* This drasticly slows down the pong
 504                          * response. Why??? */
 505                         printf("Replying to ping id %lu\n", pingmsg.id);
 506
 507         }
 508
 509         if (!option_open_once) {
 510                 /* Close can driver */
 511                 vca_close_handle(vcah);
 512         }
 513
 514         dbg(2, "Slave thread for id %d is going to finish\n", ping_id);
 515
 516         sem_post(&finish_sem);
 517         return (void *)ret;
 518 }
 519
 520 void start_slaves(int slaves, int first_id)
 521 {
 522         int id = first_id;
 523         int i;
 524
 525         dbg(1, "Starting %d slave threads\n", slaves);
 526
 527         for (i = 0; i < slaves; i++, id += 2) {
 528                 thread_data_t *td;
 529
 530                 td = malloc(sizeof(*td));
 531                 if (!td) {
 532                         printf("Can't allocate memory");
 533                         exit(EXIT_NO_MEM);
 534                 }
 535                 memset(td, 0, sizeof(*td));
 536                 td->canid = id;
 537                 /* TODO use mutexes and signal blocking */
 538                 thread_list_ins_tail(&slave_threads, td);
 539                 pthread_create(&td->tid, NULL, slave_thread, (void *)td);
 540                 dbg(2, "Slave thread: %p\n", (void *)td->tid);
 541         }
 542
 543         /* Wait for all threads beeing ready */
 544         for (i = 0; i < slaves; i++) sem_wait(&ready_sem);
 545 }
 546
 547 #ifdef WITH_RTPRIO
 548 int set_sched_policy_and_prio(int policy, int rtprio)
 549 {
 550         struct sched_param scheduling_parameters;
 551         int maxprio=sched_get_priority_max(policy);
 552         int minprio=sched_get_priority_min(policy);
 553
 554         if((rtprio < minprio) || (rtprio > maxprio)) {
 555                 fprintf(stderr, "The priority for requested policy is out of <%d, %d> range\n",
 556                                 minprio, maxprio);
 557                 return -1;
 558         }
 559
 560         scheduling_parameters.sched_priority = rtprio;
 561
 562         if (0 != pthread_setschedparam(pthread_self(), policy, &scheduling_parameters)) {
 563                 perror("pthread_setschedparam error");
 564         }
 565         return 0;
 566 }
 567 #endif
 568
 569 void print_help(void)
 570 {
 571         printf("Usage: canping -m <master threads> [other options]\n"
 572                "       canping -s <slave threads> [other options]\n\n"
 573                "Other options:\n"
 574                "  -b            go to background (fork) after initialization, prints child PID\n"
 575                "  -c count      how many messages each master sends\n"
 576                "  -d dev        device (e.g. /dev/can1)\n"
 577                "  -g prefix     store cumulative histogram in file <prefix>-<id>.dat\n"
 578                "  -h            print this help\n"
 579                "  -i id         id of first master message\n"
 580                "  -l length     length of the messages (0..8)\n"
 581                "  -o            open a device only once for all threads (doesn't work)\n" /* due to filters */
 582                "  -t timeout    timeout in seconds (default 4 s)\n"
 583                "  -v            be verbose (use more than once to increase verbosity)\n"
 584                "  -w ms         wait ms miliseconds between sending pings\n"
 585                "  -y            synchronize threads before start (doesn't test race conditions\n"
 586                "                between open and read/write)\n"
 587 #ifdef WITH_RTPRIO
 588                "  -r            run at realtime priority\n"
 589                "  -R P:prio     run at realtime priority prio, policy RR or FF\n"
 590 #endif
 591                "\n"
 592                "Example: canping -m 10 -vv\n"
 593                 );
 594 }
 595
 596 int parse_options(int argc, char *argv[])
 597 {
 598         int c;
 599
 600         opterr = 0;
 601         while ((c = getopt (argc, argv, "bc:d:g:h:i:l:m:os:t:vw:yrR:")) != -1)
 602                 switch (c)
 603                 {
 604                 case 'b':
 605                         option_background = true;
 606                         break;
 607                 case 'c':
 608                         option_count = atoi(optarg);
 609                         break;
 610                 case 'd':
 611                         option_device = optarg;
 612                         break;
 613                 case 'g':
 614                         option_histogram = optarg;
 615                         break;
 616                 case 'h':
 617                         print_help();
 618                         exit(EXIT_OK);
 619                         break;
 620                 case 'i':
 621                         option_first_id = atoi(optarg);
 622                         break;
 623                 case 'l':
 624                         option_length = atoi(optarg);
 625                         if (option_length > 8) option_length = 8;
 626                         if (option_length < 0) option_length = 0;
 627                         break;
 628                 case 'm':
 629                         option_masters = atoi(optarg);
 630                         break;
 631                 case 'o':
 632                         option_open_once = 1;
 633                         break;
 634                 case 's':
 635                         option_slaves = atoi(optarg);
 636                         break;
 637                 case 't':
 638                         option_timeout = atoi(optarg);
 639                         break;
 640                 case 'v':
 641                         option_verbose++;
 642                         break;
 643                 case 'w':
 644                         option_wait_ms = atoi(optarg);
 645                         break;
 646                 case 'y':
 647                         option_synch_start = 1;
 648                         break;
 649 #ifdef WITH_RTPRIO
 650                 case 'r':
 651                         sched_policy = SCHED_FIFO;
 652                         sched_rtprio = (sched_get_priority_min(SCHED_FIFO) +
 653                                         sched_get_priority_max(SCHED_FIFO)) / 2;
 654                         break;
 655                 case 'R':
 656                         if(!isalpha(*optarg)) {
 657                                 sched_policy = SCHED_FIFO;
 658                         } else if(!strncmp(optarg,"FF:",3)) {
 659                                 sched_policy = SCHED_FIFO;
 660                                 optarg += 3;
 661                         } else if(!strncmp(optarg,"RR:",3)) {
 662                                 sched_policy = SCHED_RR;
 663                                 optarg += 3;
 664                         } else {
 665                                 fprintf (stderr, "Unknown policy %s\n", optarg);
 666                                 exit(EXIT_BAD_PARAM);
 667                         }
 668                         sched_rtprio = atoi(optarg);
 669                         break;
 670 #endif
 671                 case '?':
 672                         if (isprint (optopt))
 673                                 fprintf (stderr, "Unknown option `-%c'.\n", optopt);
 674                         else
 675                                 fprintf (stderr,
 676                                          "Unknown option character `\\x%x'.\n",
 677                                          optopt);
 678                         return 1;
 679                 default:
 680                         exit(EXIT_BAD_PARAM);
 681                 }
 682         if (!(option_masters || option_slaves) || (option_masters && option_slaves))
 683                 exit(EXIT_BAD_PARAM);
 684         return 0;
 685 }
 686
 687 void print_stats(thread_data_t *td)
 688 {
 689         char std[20];
 690         int count = td->count + td->timeout;
 691
 692         if (td->count >= 2) {
 693                 snprintf(std, 19, "%7.2f", sqrt((td->count/(td->count - 1)) *
 694                                                 (td->moment2nd - td->mean*td->mean)));
 695         } else {
 696                 strncpy(std, "N/A", 19);
 697         }
 698         printf("Id %4ld: count = %5d"
 699                " mean = %7.2f stddev = %s"
 700                " min = %5d max = %5d [us]"
 701                " loss = %d%% (%d)\n",
 702                td->canid, count, td->mean,
 703                std,
 704                td->min, td->max,
 705                (count > 0) ? 100 * td->timeout / count : 0, td->timeout
 706                 );
 707 }
 708
 709 void wait_for_threads(void)
 710 {
 711         thread_data_t *td;
 712         void *thread_ret;
 713         int thread_count = option_slaves + option_masters;
 714         int ret;
 715
 716         while (thread_count > 0) {
 717                 if (option_verbose != 1) {
 718                         ret = sem_wait(&finish_sem);
 719                         dbg(2, "Main thread sem_wait() exited with ret=%d.\n", ret);
 720
 721                         /* If the sem_wait is successful i.e. not
 722                          * interrupted by a signal, decrease the
 723                          * thread_count*/
 724                         if (ret == 0) thread_count--;
 725                 } else {
 726                         if (!IS_FINISH_FLAG()) {
 727                                 printf("Total count: %6d, Timeouts: %6d\r", total_count, total_timeout);
 728                                 fflush(stdout);
 729                                 usleep(1000000);
 730                         }
 731                         while (sem_trywait(&finish_sem) == 0) thread_count--;
 732                 }
 733         }
 734         if (option_verbose == 1) {
 735                 printf("\n");
 736         }
 737
 738         ul_list_for_each(thread_list, &master_threads, td) {
 739                 pthread_join(td->tid, &thread_ret);
 740                 dbg(2, "Master thread for id %ld finished; exit code %d\n", td->canid, (int)thread_ret);
 741         }
 742
 743         ul_list_for_each(thread_list, &slave_threads, td) {
 744                 pthread_join(td->tid, &thread_ret);
 745                 dbg(2, "Slave thread for id %ld finished; exit code %d\n", td->canid, (int)thread_ret);
 746         }
 747
 748         if (option_masters) printf("Summary statistics:\n");
 749
 750         ul_list_for_each_cut(thread_list, &master_threads, td) {
 751                 print_stats(td);
 752                 free(td);
 753         }
 754
 755
 756         ul_list_for_each_cut(thread_list, &slave_threads, td)
 757                 free(td);
 758 }
 759
 760 int main(int argc, char *argv[])
 761 {
 762         parse_options(argc, argv);
 763
 764 #ifdef WITH_RTPRIO
 765         if(sched_policy != SCHED_OTHER)
 766                 if(set_sched_policy_and_prio(sched_policy, sched_rtprio) <0)
 767                         exit(EXIT_BAD_PARAM);
 768 #endif
 769
 770
 771         thread_list_init_head(&master_threads);
 772         thread_list_init_head(&slave_threads);
 773
 774         sem_init(&finish_sem, 0, 0);
 775         sem_init(&ready_sem, 0, 0);
 776
 777         siginterrupt(SIGINT, 1);
 778         signal(SIGINT, term_handler);
 779         siginterrupt(SIGTERM, 1);
 780         signal(SIGTERM, term_handler);
 781
 782         dbg(2, "Main thread: %p\n", (void *)pthread_self());
 783
 784         if (option_open_once) {
 785                 /* Open the CAN driver */
 786                 if(vca_open_handle(&global_vcah, option_device, NULL, 0) < 0) {
 787                         perror("open");
 788                         printf("Error opening %s\n", option_device);
 789                         exit(EXIT_CANNOT_OPEN);
 790                 }
 791         }
 792
 793         sem_t *child_ready;
 794         int fork_ret = 0;
 795         if ((child_ready = sem_open("canping", O_CREAT)) == NULL)
 796                 error(1, errno, "sem_open");
 797         if (option_background) {
 798                 /* Go to background when everything is ready */
 799                 fork_ret = fork();
 800                 if (fork_ret < 0)
 801                         error(1, errno, "Cannot go to background");
 802                 if (fork_ret == 0) {
 803                         int devnull = open("/dev/null", O_WRONLY);
 804                         dup2(devnull, 1);
 805                 }
 806         }
 807
 808         if (fork_ret == 0) {
 809                 /* Child */
 810                 if (option_masters) start_masters(option_masters, option_first_id);
 811                 if (option_slaves) start_slaves(option_slaves, option_first_id);
 812                 sem_post(child_ready);
 813         } else {
 814                 /* Parent */
 815                 sem_wait(child_ready);
 816                 printf("%d\n", fork_ret);
 817                 exit(0);
 818         }
 819
 820         wait_for_threads();
 821
 822         if (option_open_once) {
 823                 vca_close_handle(global_vcah);
 824         }
 825
 826         return 0;
 827 }