canfdtest.c

   1 /*
   2  * canfdtest.c - Full-duplex test program (DUT and host part)
   3  *
   4  * (C) 2009 by Vladislav Gribov, IXXAT Automation GmbH, <gribov@ixxat.de>
   5  * (C) 2009 Wolfgang Grandegger <wg@grandegger.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful, but
  13  * WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  15  * General Public License for more details.
  16  *
  17  * Send feedback to <linux-can@vger.kernel.org>
  18  */
  19
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <string.h>
  24 #include <signal.h>
  25 #include <libgen.h>
  26 #include <getopt.h>
  27 #include <time.h>
  28 #include <sched.h>
  29 #include <limits.h>
  30 #include <errno.h>
  31
  32 #include <sys/types.h>
  33 #include <sys/time.h>
  34 #include <sys/socket.h>
  35 #include <sys/ioctl.h>
  36 #include <net/if.h>
  37
  38 #include <linux/can.h>
  39 #include <linux/can/raw.h>
  40
  41 #define CAN_MSG_ID      0x77
  42 #define CAN_MSG_LEN     8
  43 #define CAN_MSG_COUNT   50
  44 #define CAN_MSG_WAIT    27
  45
  46 static int running = 1;
  47 static int verbose;
  48 static int sockfd;
  49 static int test_loops;
  50
  51 static void print_usage(char *prg)
  52 {
  53         fprintf(stderr,
  54                 "Usage: %s [options] <can-interface>\n"
  55                 "\n"
  56                 "Options: -v       (low verbosity)\n"
  57                 "         -vv      (high verbosity)\n"
  58                 "         -g       (generate messages)\n"
  59                 "         -l COUNT (test loop count)\n"
  60                 "\n"
  61                 "With the option '-g' CAN messages are generated and checked\n"
  62                 "on <can-interface>, otherwise all messages received on the\n"
  63                 "<can-interface> are sent back incrementing the CAN id and\n"
  64                 "all data bytes. The program can be aborted with ^C.\n"
  65                 "\n"
  66                 "Example:\n"
  67                 "\ton DUT : %s -v can0\n"
  68                 "\ton Host: %s -g -v can2\n",
  69                 prg, prg, prg);
  70
  71         exit(1);
  72 }
  73
  74 static void print_frame(struct can_frame *frame)
  75 {
  76         int i;
  77
  78         printf("%04x: ", frame->can_id);
  79         if (frame->can_id & CAN_RTR_FLAG) {
  80                 printf("remote request");
  81         } else {
  82                 printf("[%d]", frame->can_dlc);
  83                 for (i = 0; i < frame->can_dlc; i++)
  84                         printf(" %02x", frame->data[i]);
  85         }
  86         printf("\n");
  87 }
  88
  89 static void print_compare(struct can_frame *exp, struct can_frame *rec)
  90 {
  91         printf("expected: ");
  92         print_frame(exp);
  93         printf("received: ");
  94         print_frame(rec);
  95 }
  96
  97 static void compare_frame(struct can_frame *exp, struct can_frame *rec)
  98 {
  99         int i;
 100
 101         if (rec->can_id != exp->can_id) {
 102                 printf("Message ID mismatch!\n");
 103                 print_compare(exp, rec);
 104                 running = 0;
 105         } else if (rec->can_dlc != exp->can_dlc) {
 106                 printf("Message length mismatch!\n");
 107                 print_compare(exp, rec);
 108                 running = 0;
 109         } else {
 110                 for (i = 0; i < rec->can_dlc; i++) {
 111                         if (rec->data[i] != exp->data[i]) {
 112                                 printf("Databyte %x mismatch !\n", i);
 113                                 print_compare(exp,
 114                                               rec);
 115                                 running = 0;
 116                         }
 117                 }
 118         }
 119 }
 120
 121 static void millisleep(int msecs)
 122 {
 123         if (msecs <= 0) {
 124                 sched_yield();
 125         } else {
 126                 struct timespec rqtp, rmtp;
 127
 128                 /* sleep in ms */
 129                 rqtp.tv_sec = msecs / 1000;
 130                 rqtp.tv_nsec = (msecs % 1000) * 1000000;
 131                 while (nanosleep(&rqtp, &rmtp)) {
 132                         if (errno != EINTR) {
 133                                 printf("t\n");
 134                                 break;
 135                         }
 136                         rqtp = rmtp;
 137                 }
 138         }
 139 }
 140
 141 static void echo_progress(unsigned char data)
 142 {
 143         if (data == 0xff) {
 144                 printf(".");
 145                 fflush(stdout);
 146         }
 147 }
 148
 149 static void signal_handler(int signo)
 150 {
 151         close(sockfd);
 152         running = 0;
 153 }
 154
 155 static int recv_frame(struct can_frame *frame)
 156 {
 157         int ret;
 158
 159         ret = recv(sockfd, frame, sizeof(*frame), 0);
 160         if (ret != sizeof(*frame)) {
 161                 if (ret < 0)
 162                         perror("recv failed");
 163                 else
 164                         fprintf(stderr, "recv returned %d", ret);
 165                 return -1;
 166         }
 167         return 0;
 168 }
 169
 170 static int send_frame(struct can_frame *frame)
 171 {
 172         int ret;
 173
 174         while ((ret = send(sockfd, frame, sizeof(*frame), 0))
 175                != sizeof(*frame)) {
 176                 if (ret < 0) {
 177                         if (errno != ENOBUFS) {
 178                                 perror("send failed");
 179                                 return -1;
 180                         } else {
 181                                 if (verbose) {
 182                                         printf("N");
 183                                         fflush(stdout);
 184                                 }
 185                         }
 186                 } else {
 187                         fprintf(stderr, "send returned %d", ret);
 188                         return -1;
 189                 }
 190         }
 191         return 0;
 192 }
 193
 194 static int can_echo_dut(void)
 195 {
 196         unsigned int frame_count = 0;
 197         struct timeval tvn, tv_stop;
 198         struct can_frame frame;
 199         int i;
 200
 201         while (running) {
 202                 if (recv_frame(&frame))
 203                         return -1;
 204                 frame_count++;
 205                 if (verbose == 1) {
 206                         echo_progress(frame.data[0]);
 207                 } else if (verbose > 1) {
 208                         printf("%04x: ", frame.can_id);
 209                         if (frame.can_id & CAN_RTR_FLAG) {
 210                                 printf("remote request");
 211                         } else {
 212                                 printf("[%d]", frame.can_dlc);
 213                                 for (i = 0; i < frame.can_dlc; i++)
 214                                         printf(" %02x", frame.data[i]);
 215                         }
 216                         printf("\n");
 217                 }
 218                 frame.can_id++;
 219                 for (i = 0; i < frame.can_dlc; i++)
 220                         frame.data[i]++;
 221                 if (send_frame(&frame))
 222                         return -1;
 223
 224                 /*
 225                  * to force a interlacing of the frames send by DUT and PC
 226                  * test tool a waiting time is injected
 227                  */
 228                 if (frame_count == CAN_MSG_WAIT) {
 229                         frame_count = 0;
 230                         if (gettimeofday(&tv_stop, NULL)) {
 231                                 perror("gettimeofday failed\n");
 232                                 return -1;
 233                         } else {
 234                                 tv_stop.tv_usec += 3000;
 235                                 if (tv_stop.tv_usec > 999999) {
 236                                         tv_stop.tv_sec++;
 237                                         tv_stop.tv_usec =
 238                                                 tv_stop.tv_usec % 1000000;
 239                                 }
 240                                 gettimeofday(&tvn, NULL);
 241                                 while ((tv_stop.tv_sec > tvn.tv_sec) ||
 242                                        ((tv_stop.tv_sec = tvn.tv_sec) &&
 243                                         (tv_stop.tv_usec >= tvn.tv_usec)))
 244                                         gettimeofday(&tvn, NULL);
 245                         }
 246                 }
 247         }
 248
 249         return 0;
 250 }
 251
 252 static int can_echo_gen(void)
 253 {
 254         struct can_frame tx_frames[CAN_MSG_COUNT];
 255         struct can_frame rx_frame;
 256         unsigned char counter = 0;
 257         int send_pos = 0, recv_pos = 0, unprocessed = 0, loops = 0;
 258         int i;
 259
 260         while (running) {
 261                 if (unprocessed < CAN_MSG_COUNT) {
 262                         /* still send messages */
 263                         tx_frames[send_pos].can_dlc = CAN_MSG_LEN;
 264                         tx_frames[send_pos].can_id = CAN_MSG_ID;
 265                         for (i = 0; i < CAN_MSG_LEN; i++)
 266                                 tx_frames[send_pos].data[i] = counter + i;
 267                         if (send_frame(&tx_frames[send_pos]))
 268                                 return -1;
 269
 270                         /* increment to be equal to expected */
 271                         tx_frames[send_pos].can_id++;
 272                         for (i = 0; i < CAN_MSG_LEN; i++)
 273                                 tx_frames[send_pos].data[i]++;
 274
 275                         send_pos++;
 276                         if (send_pos == CAN_MSG_COUNT)
 277                                 send_pos = 0;
 278                         unprocessed++;
 279                         if (verbose == 1)
 280                                 echo_progress(counter);
 281                         counter++;
 282
 283                         if ((counter % 33) == 0)
 284                                 millisleep(3);
 285                         else
 286                                 millisleep(1);
 287                 } else {
 288                         if (recv_frame(&rx_frame))
 289                                 return -1;
 290
 291                         if (verbose > 1)
 292                                 print_frame(&rx_frame);
 293
 294                         /* compare with expected */
 295                         compare_frame(&tx_frames[recv_pos], &rx_frame);
 296
 297                         loops++;
 298                         if (test_loops && loops >= test_loops)
 299                                 break;
 300
 301                         recv_pos++;
 302                         if (recv_pos == CAN_MSG_COUNT)
 303                                 recv_pos = 0;
 304                         unprocessed--;
 305                 }
 306         }
 307
 308         printf("\nTest messages sent and received: %d\n", loops);
 309
 310         return 0;
 311 }
 312
 313 int main(int argc, char *argv[])
 314 {
 315         struct sockaddr_can addr;
 316         char *intf_name;
 317         int family = PF_CAN, type = SOCK_RAW, proto = CAN_RAW;
 318         int echo_gen = 0;
 319         int opt, err;
 320
 321         signal(SIGTERM, signal_handler);
 322         signal(SIGHUP, signal_handler);
 323         signal(SIGINT, signal_handler);
 324
 325         while ((opt = getopt(argc, argv, "gl:v")) != -1) {
 326                 switch (opt) {
 327                 case 'v':
 328                         verbose++;
 329                         break;
 330
 331                 case 'l':
 332                         test_loops = atoi(optarg);;
 333                         break;
 334
 335                 case 'g':
 336                         echo_gen = 1;
 337                         break;
 338
 339                 default:
 340                         print_usage(basename(argv[0]));
 341                         break;
 342                 }
 343         }
 344
 345         if ((argc - optind) != 1)
 346                 print_usage(basename(argv[0]));
 347         intf_name = argv[optind];
 348
 349         printf("interface = %s, family = %d, type = %d, proto = %d\n",
 350                intf_name, family, type, proto);
 351
 352         if ((sockfd = socket(family, type, proto)) < 0) {
 353                 perror("socket");
 354                 return 1;
 355         }
 356
 357         addr.can_family = family;
 358         addr.can_ifindex = if_nametoindex(intf_name);
 359
 360         if (bind(sockfd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 361                 perror("bind");
 362                 close(sockfd);
 363                 return 1;
 364         }
 365
 366         if (echo_gen)
 367                 err = can_echo_gen();
 368         else
 369                 err = can_echo_dut();
 370
 371         if (verbose)
 372                 printf("Exiting...\n");
 373
 374         close(sockfd);
 375         return err;
 376 }