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