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