src/canping_rtl.c

   1 #include <posix/unistd.h>
   2 #include <linux/module.h>
   3 #include <asm/atomic.h>
   4 #include <pthread.h>
   5 #include <ul_list.h>
   6 #include <semaphore.h>
   7 #include <linux/time.h>
   8
   9 #include <can.h>
  10
  11 /* TODO osetrit vicero pongu */
  12
  13 /* Defines for better compatibility with user-space version */
  14 #define option_device device
  15 #define option_masters masters
  16 #define option_first_id first_id
  17 #define option_slaves slaves
  18 #define option_verbose verbose
  19 #define option_length length
  20 #define option_count count
  21 #define option_wait_ms wait_ms
  22 #define option_timeout timeout
  23 #define option_open_once open_once
  24 #define option_synch_start synch_start
  25
  26 #define printf rtl_printf
  27 #define fprintf(file, fmt, arg...) printf(fmt, ## arg)
  28 #define perror(s) printf("%s: error %d\n", s, errno);
  29 #define exit(retval) pthread_exit((void *)retval)
  30 #define select(NFDS, READFDS, WRITEFDS, EXCEPTFDS, TIMEOUT) (1)
  31 #define malloc(size) kmalloc(size, GFP_KERNEL)
  32 #define free(what) kfree(what)
  33 #define sqrt(val) (0.0)
  34 #define fflush(fd)
  35 #define signal(a,b)
  36 #define siginterrupt(a, b)
  37 #define gettimeofday(a, b) do {;} while (0) //do_gettimeofday(a)
  38
  39 #ifndef DEBUG
  40 #define dbg(level, fmt, arg...) do {} while (0)
  41 #else
  42 #define dbg(level, fmt, arg...) do { if (level <= DEBUG) { printf("candping: " fmt, ## arg); } } while (0)
  43 #endif
  44
  45 #define NOT_INTERRUPTED_SYSCALL (errno != EINTR && errno != ERESTART)
  46 #define IS_FINISH_FLAG() (atomic_read(&finish_flag) != 0)
  47
  48 /* Exit codes */
  49 #define EXIT_OK 0
  50 #define EXIT_BAD_PARAM 1
  51 #define EXIT_CANNOT_OPEN 2
  52 #define EXIT_FILTER_ERROR 3
  53 #define EXIT_NO_MEM 4
  54 #define EXIT_READ_ERROR 5
  55 #define EXIT_WRITE_ERROR 6
  56 #define EXIT_SELECT_ERROR 7
  57 #define EXIT_FLUSH_ERROR 8
  58
  59 /* Global variables */
  60 atomic_t finish_flag = ATOMIC_INIT(0); /* Threads should terminate. */
  61 sem_t finish_sem;               /* Thread signals a termination */
  62 int global_canfd;
  63
  64 int total_count = 0;
  65 int total_timeout = 0;
  66
  67 pthread_mutex_t mut_start = PTHREAD_MUTEX_INITIALIZER;
  68 pthread_cond_t cond_start = PTHREAD_COND_INITIALIZER;
  69 int start_flag = 0;
  70 sem_t ready_sem;                /* Thread is ready for execution */
  71
  72
  73 /* Command line options */
  74 char *option_device = "/dev/can0";
  75 int option_masters = 0;
  76 long int option_first_id = 1000;
  77 int option_slaves = 0;
  78 int option_verbose = 0;         /* 0 - nothing, 1 - only global
  79                                  * statistics, 2 - simple times, 3 -
  80                                  * verbose times */
  81 int option_length = 8;
  82 int option_count = 0;
  83 int option_wait_ms = 1000;
  84 int option_timeout = 4;
  85 int option_open_once = 0;
  86 int option_synch_start = 0;
  87
  88 MODULE_PARM_DESC(option_device,"name of CAN device [/dev/can0]");
  89 MODULE_PARM(option_device,"1s");
  90
  91 MODULE_PARM_DESC(option_first_id,"lowest ID of generated messages [1000]");
  92 MODULE_PARM(option_first_id,"1i");
  93
  94 MODULE_PARM_DESC(option_masters,"how many master threads to start");
  95 MODULE_PARM(option_masters,"1i");
  96
  97 MODULE_PARM_DESC(option_slaves,"how many slave threads to start");
  98 MODULE_PARM(option_slaves,"1i");
  99
 100 MODULE_PARM_DESC(option_verbose,"verbosity level");
 101 MODULE_PARM(option_verbose,"1i");
 102
 103 MODULE_PARM_DESC(option_count,"number of messages to send");
 104 MODULE_PARM(option_count,"1i");
 105
 106 MODULE_PARM_DESC(option_wait_ms,"miliseconds to wait between sent messages");
 107 MODULE_PARM(option_wait_ms,"1i");
 108
 109 MODULE_PARM_DESC(option_synch_start,"synchronize threads before start");
 110 MODULE_PARM(option_synch_start,"1i");
 111
 112
 113 MODULE_SUPPORTED_DEVICE("canping");
 114 MODULE_AUTHOR("Michal Sojka <sojkam1@fel.cvut.cz>");
 115 MODULE_DESCRIPTION("canping: RT-Linux LinCAN test application");
 116 MODULE_LICENSE("GPL");
 117
 118 /* Lists */
 119 typedef struct threads {
 120         ul_list_head_t head;
 121 } threads_t;
 122
 123 typedef struct thread_data {
 124         pthread_t tid;
 125         long int canid;
 126
 127         int count;
 128         double mean;            /* mean value of responses */
 129         double moment2nd;       /* used to compute variance of
 130                                  * responses */
 131         int min, max;           /* min/max response times */
 132         int timeout;            /* number of timeouts */
 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 /* Subtract the `struct timeval' values X and Y, storing the result in
 143    RESULT.  Return 1 if the difference is negative, otherwise 0.  */
 144
 145 int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y)
 146 {
 147         /* Perform the carry for the later subtraction by updating Y. */
 148         if (x->tv_usec < y->tv_usec) {
 149                 int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
 150                 y->tv_usec -= 1000000 * nsec;
 151                 y->tv_sec += nsec;
 152         }
 153         if (x->tv_usec - y->tv_usec > 1000000) {
 154                 int nsec = (x->tv_usec - y->tv_usec) / 1000000;
 155                 y->tv_usec += 1000000 * nsec;
 156                 y->tv_sec -= nsec;
 157         }
 158
 159         /* Compute the time remaining to wait.
 160            `tv_usec' is certainly positive. */
 161         result->tv_sec = x->tv_sec - y->tv_sec;
 162         result->tv_usec = x->tv_usec - y->tv_usec;
 163
 164         /* Return 1 if result is negative. */
 165         return x->tv_sec < y->tv_sec;
 166 }
 167
 168 void dbg_print_timeval(char *msg, struct timeval *tv)
 169 {
 170
 171         printf("%s sec=%ld usec=%ld\n", msg, tv->tv_sec, tv->tv_usec);
 172 }
 173 void kill_all_threads(int signum)
 174 {
 175         thread_data_t *td;
 176
 177         ul_list_for_each(thread_list, &master_threads, td) {
 178                 pthread_kill(td->tid, signum);
 179         }
 180         ul_list_for_each(thread_list, &slave_threads, td) {
 181                 pthread_kill(td->tid, signum);
 182         }
 183 }
 184 void term_handler(int signum)
 185 {
 186         dbg(1, "Thread %p got a signal %d\n", (void *)pthread_self(), signum);
 187         if (!IS_FINISH_FLAG()) {
 188                 dbg(1, "Terminating threads\n");
 189                 atomic_set(&finish_flag, 1);
 190
 191                 kill_all_threads(signum);
 192         }
 193 }
 194
 195 void *master_thread(void *arg)
 196 {
 197         thread_data_t *td = (thread_data_t *)arg;
 198         int ping_id = td->canid;
 199         int pong_id = ping_id + 1;
 200         struct canmsg_t pingmsg, pongmsg;
 201         int ping_count = 0;
 202         int ret = 0;
 203         int canfd;              /* File descriptor of CAN driver. */
 204         int i;
 205         fd_set rdset;           /* read set for select syscall */
 206         struct timeval to, pingtime, pongtime;
 207         struct canfilt_t canfilt; /* filter for received messages */
 208
 209         if (!option_open_once) {
 210                 /* Open can driver */
 211                 if ((canfd = open(option_device, O_RDWR)) < 0) {
 212                         perror("open");
 213                         fprintf(stderr, "Error opening %s (for id %d)\n", option_device, ping_id);
 214                         exit(EXIT_CANNOT_OPEN);
 215                 }
 216         } else {
 217                 canfd = global_canfd;
 218         }
 219
 220         /* setup filtering of received messages */
 221         memset(&canfilt, 0, sizeof(canfilt));
 222         canfilt.mask = 0xfffffff;
 223         canfilt.id = pong_id;   /* pong responces with increased id */
 224         ret = ioctl(canfd, CANQUE_FILTER, &canfilt);
 225         if(ret<0) {
 226                 perror("ioctl CANQUE_FILTER");
 227                 exit(EXIT_FILTER_ERROR);
 228         }
 229         ret = ioctl(canfd, CANQUE_FLUSH, NULL);
 230         if(ret<0) {
 231                 perror("ioctl CANQUE_QUEUE_FLUSH");
 232                 exit(EXIT_FLUSH_ERROR);
 233         }
 234
 235         /* Prepare data structures for the select syscall. */
 236         FD_ZERO (&rdset);
 237
 238         /* Signal that I'm ready */
 239         sem_post(&ready_sem);
 240
 241         if (option_synch_start) {
 242                 /* Wait for other threads to initialize */
 243                 pthread_mutex_lock(&mut_start);
 244                 while (!start_flag) pthread_cond_wait(&cond_start, &mut_start);
 245                 pthread_mutex_unlock(&mut_start);
 246         }
 247
 248         while (!IS_FINISH_FLAG() && (option_count == 0 || ping_count++ < option_count)) {
 249                 /* Send a ping message */
 250                 pingmsg.flags=0;
 251                 pingmsg.id=ping_id;
 252                 pingmsg.length = option_length;
 253                 for (i=0; i < option_length; i++) pingmsg.data[i] = i;
 254                 gettimeofday(&pingtime, NULL);
 255
 256                 ret = write(canfd, &pingmsg, sizeof(pingmsg));
 257                 if (ret < 0) {
 258                         if (NOT_INTERRUPTED_SYSCALL) {
 259                                 perror("write");
 260                                 exit(EXIT_WRITE_ERROR);
 261                         }
 262                 }
 263
 264                 /* Wait for a pong responce */
 265                 FD_SET (canfd, &rdset);
 266
 267                 to.tv_sec = option_timeout;
 268                 to.tv_usec = 0;
 269
 270                 ret = select(FD_SETSIZE, &rdset, NULL, NULL, &to);
 271                 if (ret > 0) {
 272                         /* Read the message */
 273                         pongmsg.flags=0;
 274                         ret = read(canfd, &pongmsg, sizeof(pongmsg));
 275                         if (ret < 0) {
 276                                 if (NOT_INTERRUPTED_SYSCALL) {
 277                                         perror("read");
 278                                         exit(EXIT_READ_ERROR);
 279                                 }
 280                         }
 281                         else {  /* A pong message received */
 282                                 long int time;
 283                                 gettimeofday(&pongtime, NULL);
 284                                 if (ret == 0) {
 285                                         fprintf(stderr, "read returned zero\n");
 286                                         exit(EXIT_READ_ERROR);
 287                                 }
 288                                 if (pongmsg.id != pong_id) {
 289                                         fprintf(stderr, "Filter error (expected: %d, received %ld)\n", pong_id, pongmsg.id);
 290                                         exit(EXIT_FILTER_ERROR);
 291                                 }
 292                                 timeval_subtract(&pongtime, &pongtime, &pingtime);
 293                                 time = pongtime.tv_sec*1000000 + pongtime.tv_usec;
 294                                 switch (option_verbose) {
 295                                 case 2:
 296                                         printf("%d:%ld\n", ping_id, time);
 297                                         break;
 298                                 case 3:
 299                                         printf("Pong response for id %d received in %ld us\n", ping_id, time);
 300                                         break;
 301                                 }
 302                                 /* Update statistics */
 303                                 td->count++;
 304                                 td->mean =
 305                                         td->mean * ((double)(td->count - 1) / td->count) +
 306                                         (double)time / td->count;
 307                                 td->moment2nd =
 308                                         td->moment2nd * ((double)(td->count - 1) / td->count) +
 309                                         (double)time*time  / td->count;
 310                                 if (time > td->max) td->max = time;
 311                                 if (time < td->min) td->min = time;
 312                                 total_count++;
 313                         } /* read */
 314                 } else if (ret == 0) { /* select */
 315                         if (option_verbose >= 2)
 316                                 printf("Timeout encountered (id %d)\n", ping_id);
 317                         td->timeout++;
 318                         total_timeout++;
 319                 } else {
 320                         if (NOT_INTERRUPTED_SYSCALL) {
 321                                 perror("select");
 322                                 exit(EXIT_SELECT_ERROR);
 323                         }
 324                 }
 325                 usleep(option_wait_ms * 1000);
 326         }
 327
 328         if (!option_open_once) {
 329                 /* Close the can driver */
 330                 close(canfd);
 331         }
 332
 333         sem_post(&finish_sem);
 334         return (void *)ret;
 335 }
 336
 337 void start_masters(int masters, int first_id)
 338 {
 339         int id = first_id;
 340         int i;
 341
 342         dbg(1, "Starting %d master threads\n", masters);
 343
 344         for (i = 0; i < masters; i++, id += 2) {
 345                 thread_data_t *td;
 346
 347                 td = malloc(sizeof(*td));
 348                 if (!td) {
 349                         printf("Can't allocate memory");
 350                         exit(EXIT_NO_MEM);
 351                 }
 352                 memset(td, 0, sizeof(*td));
 353                 td->canid = id;
 354                 td->min = 0x7fffffff;
 355                 /* TODO use mutexes and signal blocking */
 356                 thread_list_ins_tail(&master_threads, td);
 357                 pthread_create(&td->tid, NULL, master_thread, (void *)td);
 358                 dbg(2, "Master thread: %p\n", (void *)td->tid);
 359         }
 360
 361
 362         /* Wait for all threads beeing ready */
 363         for (i = 0; i < masters; i++) sem_wait(&ready_sem);
 364
 365         /* Start threads */
 366         pthread_mutex_lock(&mut_start);
 367         start_flag = 1;
 368         pthread_cond_broadcast(&cond_start);
 369         pthread_mutex_unlock(&mut_start);
 370 }
 371
 372 void *slave_thread(void *arg)
 373 {
 374         thread_data_t *td = (thread_data_t *)arg;
 375         int ping_id = td->canid;
 376         int pong_id = ping_id + 1;
 377         struct canmsg_t pingmsg, pongmsg;
 378         int ret = 0;
 379         int canfd;              /* File descriptor of CAN driver. */
 380         int i;
 381         struct canfilt_t canfilt; /* filter for received messages */
 382
 383         if (!option_open_once) {
 384                 /* Open the CAN driver */
 385                 if ((canfd = open(option_device, O_RDWR)) < 0) {
 386                         perror("open");
 387                         printf("Error opening %s (for id %d)\n", option_device, ping_id);
 388                         exit(EXIT_CANNOT_OPEN);
 389                 }
 390         } else {
 391                 canfd = global_canfd;
 392         }
 393
 394         /* setup filtering of received messages */
 395         memset(&canfilt, 0, sizeof(canfilt));
 396         canfilt.mask = 0xfffffff;
 397         canfilt.id = ping_id;   /* receive only our ping messages */
 398         ret = ioctl(canfd, CANQUE_FILTER, &canfilt);
 399         if (ret < 0) {
 400                 perror("ioctl CANQUE_FILTER");
 401                 exit(EXIT_FILTER_ERROR);
 402         }
 403         /* If there are some messages already, delete them. These may
 404          * not processed by our filter and thus may have a wrong
 405          * ID. */
 406         ret = ioctl(canfd, CANQUE_FLUSH, NULL);
 407         if (ret < 0) {
 408                 perror("ioctl CANQUE_QUEUE_FLUSH");
 409                 exit(EXIT_FLUSH_ERROR);
 410         }
 411
 412         /* Prepare a pong message */
 413         pongmsg.flags = 0;
 414         pongmsg.id = pong_id;
 415         pongmsg.length = option_length;
 416         for (i=0; i < option_length; i++) pongmsg.data[i] = i;
 417
 418         while (!IS_FINISH_FLAG()) {
 419                 /* Receive a ping message */
 420                 pingmsg.flags=0;
 421                 ret = read(canfd, &pingmsg, sizeof(pingmsg));
 422                 if (ret < 0) {
 423                         if (NOT_INTERRUPTED_SYSCALL) {
 424                                 printf("%d\n", errno);
 425                                 perror("read");
 426                                 exit(EXIT_READ_ERROR);
 427                         }
 428                 }
 429                 if (NOT_INTERRUPTED_SYSCALL && pingmsg.id != ping_id) {
 430                         fprintf(stderr, "Filter error (expected: %d, received %ld)\n", ping_id, pingmsg.id);
 431                         exit(EXIT_FILTER_ERROR);
 432                 }
 433                 /* Answer immendiately with a pong message */
 434                 if (NOT_INTERRUPTED_SYSCALL) {
 435                         ret = write(canfd, &pongmsg, sizeof(pongmsg));
 436                         if (ret < 0) {
 437                                 if (NOT_INTERRUPTED_SYSCALL) {
 438                                         perror("write");
 439                                         exit(EXIT_WRITE_ERROR);
 440                                 }
 441                         }
 442                 }
 443
 444                 if (ret >= 0) total_count++;
 445
 446                 if (option_verbose >= 2)
 447                         /* This drasticly slows down the pong
 448                          * response. Why??? */
 449                         printf("Replying to ping id %lu\n", pingmsg.id);
 450
 451         }
 452
 453         if (!option_open_once) {
 454                 /* Close can driver */
 455                 close(canfd);
 456         }
 457
 458         sem_post(&finish_sem);
 459         return (void *)ret;
 460 }
 461
 462 void start_slaves(int slaves, int first_id)
 463 {
 464         int id = first_id;
 465         int i;
 466
 467         dbg(1, "Starting %d slave threads\n", slaves);
 468
 469         for (i = 0; i < slaves; i++, id += 2) {
 470                 thread_data_t *td;
 471
 472                 td = malloc(sizeof(*td));
 473                 if (!td) {
 474                         printf("Can't allocate memory");
 475                         exit(EXIT_NO_MEM);
 476                 }
 477                 memset(td, 0, sizeof(*td));
 478                 td->canid = id;
 479                 /* TODO use mutexes and signal blocking */
 480                 thread_list_ins_tail(&slave_threads, td);
 481                 pthread_create(&td->tid, NULL, slave_thread, (void *)td);
 482                 dbg(2, "Slave thread: %p\n", (void *)td->tid);
 483         }
 484 }
 485
 486 void print_help(void)
 487 {
 488         printf("Usage: canping -m <master threads> [other options]\n"
 489                "       canping -s <slave threads> [other options]\n\n"
 490                "Other options:\n"
 491                "  -c count      how many messages each master sends\n"
 492                "  -d dev        device (e.g. /dev/can1)\n"
 493                "  -h            print this help\n"
 494                "  -i id         id of first master message\n"
 495                "  -l length     length of the messages (0..8)\n"
 496                "  -o            open a device only once for all threads (doesn't work)\n" /* due to filters */
 497                "  -t timeout    timeout in seconds (default 4 s)\n"
 498                "  -v            be verbose (use more than once to increase verbosity)\n"
 499                "  -w ms         wait ms miliseconds between sending pings\n"
 500                "  -y            synchronize threads before start (doesn't test race conditions\n"
 501                "                between open and read/write)\n"
 502                "\n"
 503                "Example: canping -m 10 -vv\n"
 504                 );
 505 }
 506
 507 void print_stats(thread_data_t *td)
 508 {
 509         char std[20];
 510         int count = td->count + td->timeout;
 511
 512         if (td->count >= 2) {
 513                 snprintf(std, 19, "%7.2f", sqrt((td->count/(td->count - 1)) *
 514                                                 (td->moment2nd - td->mean*td->mean)));
 515         } else {
 516                 strncpy(std, "N/A", 19);
 517         }
 518         printf("Id %4ld: count = %5d"
 519                " mean = %7.2f stddev = %s"
 520                " min = %5d max = %5d [us]"
 521                " loss = %d%% (%d)\n",
 522                td->canid, count, td->mean,
 523                std,
 524                td->min, td->max,
 525                (count > 0) ? 100 * td->timeout / count : 0, td->timeout
 526                 );
 527 }
 528
 529 void wait_for_threads(void)
 530 {
 531         thread_data_t *td;
 532         void *thread_ret;
 533         int thread_count = option_slaves + option_masters;
 534         while (thread_count > 0) {
 535                 if (option_verbose != 1) {
 536                         sem_wait(&finish_sem);
 537                         thread_count--;
 538                 } else {
 539                         if (!IS_FINISH_FLAG()) {
 540                                 printf("Total count: %6d, Timeouts: %6d\r", total_count, total_timeout);
 541                                 fflush(stdout);
 542                                 usleep(1000000);
 543                         }
 544                         while (sem_trywait(&finish_sem) == 0) thread_count--;
 545                 }
 546         }
 547         if (option_verbose == 1) {
 548                 printf("\n");
 549         }
 550
 551         ul_list_for_each(thread_list, &master_threads, td) {
 552                 pthread_join(td->tid, &thread_ret);
 553                 dbg(2, "Master thread for id %ld finished; exit code %d\n", td->canid, (int)thread_ret);
 554         }
 555
 556         ul_list_for_each(thread_list, &slave_threads, td) {
 557                 pthread_join(td->tid, &thread_ret);
 558                 dbg(2, "Slave thread for id %ld finished; exit code %d\n", td->canid, (int)thread_ret);
 559         }
 560
 561         if (option_masters) printf("Summary statistics:\n");
 562
 563         ul_list_for_each_cut(thread_list, &master_threads, td) {
 564                 print_stats(td);
 565                 free(td);
 566         }
 567
 568
 569         ul_list_for_each_cut(thread_list, &slave_threads, td)
 570                 free(td);
 571 }
 572
 573 int main(int argc, char *argv[])
 574 {
 575         thread_list_init_head(&master_threads);
 576         thread_list_init_head(&slave_threads);
 577
 578         sem_init(&finish_sem, 0, 0);
 579         sem_init(&ready_sem, 0, 0);
 580
 581         siginterrupt(SIGINT, 1);
 582         signal(SIGINT, term_handler);
 583         siginterrupt(SIGTERM, 1);
 584         signal(SIGTERM, term_handler);
 585
 586         dbg(2, "Main thread: %p\n", (void *)pthread_self());
 587
 588         if (option_open_once) {
 589                 /* Open the CAN driver */
 590                 if ((global_canfd = open(option_device, O_RDWR)) < 0) {
 591                         perror("open");
 592                         printf("Error opening %s\n", option_device);
 593                         exit(EXIT_CANNOT_OPEN);
 594                 }
 595         }
 596
 597         if (option_masters) start_masters(option_masters, option_first_id);
 598         if (option_slaves) start_slaves(option_slaves, option_first_id);
 599
 600 /*      wait_for_threads(); */
 601
 602 /*      if (option_open_once) { */
 603 /*              close(global_canfd); */
 604 /*      } */
 605
 606         return 0;
 607 }
 608
 609 int init_module(void)
 610 {
 611         rtl_printf("Loading canping_rtl module...\n");
 612         return main(0, NULL);
 613 }
 614
 615 void cleanup_module(void)
 616 {
 617         term_handler(0);
 618         wait_for_threads();
 619         rtl_printf("Module canping_rtl finished.\n");
 620 }