canlogserver.c

   1 /*
   2  *  $Id$
   3  */
   4
   5 /*
   6  * canlogserver.c
   7  *
   8  * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
   9  * All rights reserved.
  10  *
  11  * Redistribution and use in source and binary forms, with or without
  12  * modification, are permitted provided that the following conditions
  13  * are met:
  14  * 1. Redistributions of source code must retain the above copyright
  15  *    notice, this list of conditions and the following disclaimer.
  16  * 2. Redistributions in binary form must reproduce the above copyright
  17  *    notice, this list of conditions and the following disclaimer in the
  18  *    documentation and/or other materials provided with the distribution.
  19  * 3. Neither the name of Volkswagen nor the names of its contributors
  20  *    may be used to endorse or promote products derived from this software
  21  *    without specific prior written permission.
  22  *
  23  * Alternatively, provided that this notice is retained in full, this
  24  * software may be distributed under the terms of the GNU General
  25  * Public License ("GPL") version 2, in which case the provisions of the
  26  * GPL apply INSTEAD OF those given above.
  27  *
  28  * The provided data structures and external interfaces from this code
  29  * are not restricted to be used by modules with a GPL compatible license.
  30  *
  31  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  32  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  33  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  34  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  35  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  36  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  37  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  38  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  39  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  40  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  41  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  42  * DAMAGE.
  43  *
  44  * Send feedback to <linux-can@vger.kernel.org>
  45  *
  46  */
  47
  48 #include <stdio.h>
  49 #include <stdlib.h>
  50 #include <unistd.h>
  51 #include <string.h>
  52 #include <signal.h>
  53 #include <ctype.h>
  54 #include <libgen.h>
  55 #include <time.h>
  56
  57 #include <sys/time.h>
  58 #include <sys/wait.h>
  59 #include <sys/types.h>
  60 #include <sys/socket.h>
  61 #include <sys/ioctl.h>
  62 #include <sys/uio.h>
  63 #include <net/if.h>
  64 #include <netinet/in.h>
  65
  66 #include <linux/can.h>
  67 #include <linux/can/raw.h>
  68 #include <signal.h>
  69 #include <errno.h>
  70
  71 #include "lib.h"
  72
  73 #define MAXDEV 6 /* change sscanf()'s manually if changed here */
  74 #define ANYDEV "any"
  75 #define ANL "\r\n" /* newline in ASC mode */
  76
  77 #define COMMENTSZ 200
  78 #define BUFSZ (sizeof("(1345212884.318850)") + IFNAMSIZ + 4 + CL_CFSZ + COMMENTSZ) /* for one line in the logfile */
  79
  80 #define DEFPORT 28700
  81
  82 static char devname[MAXDEV][IFNAMSIZ+1];
  83 static int  dindex[MAXDEV];
  84 static int  max_devname_len;
  85
  86 extern int optind, opterr, optopt;
  87
  88 static volatile int running = 1;
  89
  90 void print_usage(char *prg)
  91 {
  92         fprintf(stderr, "\nUsage: %s [options] <CAN interface>+\n", prg);
  93         fprintf(stderr, "  (use CTRL-C to terminate %s)\n\n", prg);
  94         fprintf(stderr, "Options: -m <mask>   (ID filter mask.  Default 0x00000000) *\n");
  95         fprintf(stderr, "         -v <value>  (ID filter value. Default 0x00000000) *\n");
  96         fprintf(stderr, "         -i <0|1>    (invert the specified ID filter) *\n");
  97         fprintf(stderr, "         -e <emask>  (mask for error frames)\n");
  98         fprintf(stderr, "         -p <port>   (listen on port <port>. Default: %d)\n", DEFPORT);
  99         fprintf(stderr, "\n");
 100         fprintf(stderr, "* The CAN ID filter matches, when ...\n");
 101         fprintf(stderr, "       <received_can_id> & mask == value & mask\n");
 102         fprintf(stderr, "\n");
 103         fprintf(stderr, "When using more than one CAN interface the options\n");
 104         fprintf(stderr, "m/v/i/e have comma seperated values e.g. '-m 0,7FF,0'\n");
 105         fprintf(stderr, "\nUse interface name '%s' to receive from all CAN interfaces.\n\n", ANYDEV);
 106 }
 107
 108 int idx2dindex(int ifidx, int socket)
 109 {
 110         int i;
 111         struct ifreq ifr;
 112
 113         for (i=0; i<MAXDEV; i++) {
 114                 if (dindex[i] == ifidx)
 115                         return i;
 116         }
 117
 118         /* create new interface index cache entry */
 119
 120         /* remove index cache zombies first */
 121         for (i=0; i < MAXDEV; i++) {
 122                 if (dindex[i]) {
 123                         ifr.ifr_ifindex = dindex[i];
 124                         if (ioctl(socket, SIOCGIFNAME, &ifr) < 0)
 125                                 dindex[i] = 0;
 126                 }
 127         }
 128
 129         for (i=0; i < MAXDEV; i++)
 130                 if (!dindex[i]) /* free entry */
 131                         break;
 132
 133         if (i == MAXDEV) {
 134                 printf("Interface index cache only supports %d interfaces.\n", MAXDEV);
 135                 exit(1);
 136         }
 137
 138         dindex[i] = ifidx;
 139
 140         ifr.ifr_ifindex = ifidx;
 141         if (ioctl(socket, SIOCGIFNAME, &ifr) < 0)
 142                 perror("SIOCGIFNAME");
 143
 144         if (max_devname_len < strlen(ifr.ifr_name))
 145                 max_devname_len = strlen(ifr.ifr_name);
 146
 147         strcpy(devname[i], ifr.ifr_name);
 148
 149 #ifdef DEBUG
 150         printf("new index %d (%s)\n", i, devname[i]);
 151 #endif
 152
 153         return i;
 154 }
 155
 156 /*
 157  * This is a Signalhandler. When we get a signal, that a child
 158  * terminated, we wait for it, so the zombie will disappear.
 159  */
 160 void childdied(int i)
 161 {
 162         wait(NULL);
 163 }
 164
 165 /*
 166  * This is a Signalhandler for a cought SIGTERM
 167  */
 168 void shutdown_gra(int i)
 169 {
 170         exit(0);
 171 }
 172
 173
 174 int main(int argc, char **argv)
 175 {
 176         struct sigaction signalaction;
 177         sigset_t sigset;
 178         fd_set rdfs;
 179         int s[MAXDEV];
 180         int socki, accsocket;
 181         canid_t mask[MAXDEV] = {0};
 182         canid_t value[MAXDEV] = {0};
 183         int inv_filter[MAXDEV] = {0};
 184         can_err_mask_t err_mask[MAXDEV] = {0};
 185         int opt, ret;
 186         int currmax = 1; /* we assume at least one can bus ;-) */
 187         struct sockaddr_can addr;
 188         struct can_filter rfilter;
 189         struct canfd_frame frame;
 190         const int canfd_on = 1;
 191         int nbytes, i, j, maxdlen;
 192         struct ifreq ifr;
 193         struct timeval tv, last_tv;
 194         int port = DEFPORT;
 195         struct sockaddr_in inaddr;
 196         struct sockaddr_in clientaddr;
 197         socklen_t sin_size = sizeof(clientaddr);
 198         char temp[BUFSZ];
 199
 200         sigemptyset(&sigset);
 201         signalaction.sa_handler = &childdied;
 202         signalaction.sa_mask = sigset;
 203         signalaction.sa_flags = 0;
 204         sigaction(SIGCHLD, &signalaction, NULL);  /* install signal for dying child */
 205         signalaction.sa_handler = &shutdown_gra;
 206         signalaction.sa_mask = sigset;
 207         signalaction.sa_flags = 0;
 208         sigaction(SIGTERM, &signalaction, NULL); /* install Signal for termination */
 209         sigaction(SIGINT, &signalaction, NULL); /* install Signal for termination */
 210
 211
 212         last_tv.tv_sec  = 0;
 213         last_tv.tv_usec = 0;
 214
 215         while ((opt = getopt(argc, argv, "m:v:i:e:p:?")) != -1) {
 216
 217                 switch (opt) {
 218                 case 'm':
 219                         i = sscanf(optarg, "%x,%x,%x,%x,%x,%x",
 220                                    &mask[0], &mask[1], &mask[2],
 221                                    &mask[3], &mask[4], &mask[5]);
 222                         if (i > currmax)
 223                                 currmax = i;
 224                         break;
 225
 226                 case 'v':
 227                         i = sscanf(optarg, "%x,%x,%x,%x,%x,%x",
 228                                    &value[0], &value[1], &value[2],
 229                                    &value[3], &value[4], &value[5]);
 230                         if (i > currmax)
 231                                 currmax = i;
 232                         break;
 233
 234                 case 'i':
 235                         i = sscanf(optarg, "%d,%d,%d,%d,%d,%d",
 236                                    &inv_filter[0], &inv_filter[1], &inv_filter[2],
 237                                    &inv_filter[3], &inv_filter[4], &inv_filter[5]);
 238                         if (i > currmax)
 239                                 currmax = i;
 240                         break;
 241
 242                 case 'e':
 243                         i = sscanf(optarg, "%x,%x,%x,%x,%x,%x",
 244                                    &err_mask[0], &err_mask[1], &err_mask[2],
 245                                    &err_mask[3], &err_mask[4], &err_mask[5]);
 246                         if (i > currmax)
 247                                 currmax = i;
 248                         break;
 249                 case 'p':
 250                         port = atoi(optarg);
 251                         break;
 252                 default:
 253                         print_usage(basename(argv[0]));
 254                         exit(1);
 255                         break;
 256                 }
 257         }
 258
 259         if (optind == argc) {
 260                 print_usage(basename(argv[0]));
 261                 exit(0);
 262         }
 263
 264         /* count in options higher than device count ? */
 265         if (optind + currmax > argc) {
 266                 printf("low count of CAN devices!\n");
 267                 return 1;
 268         }
 269
 270         currmax = argc - optind; /* find real number of CAN devices */
 271
 272         if (currmax > MAXDEV) {
 273                 printf("More than %d CAN devices!\n", MAXDEV);
 274                 return 1;
 275         }
 276
 277
 278         socki = socket(PF_INET, SOCK_STREAM, 0);
 279         if (socki < 0) {
 280                 perror("socket");
 281                 exit(1);
 282         }
 283
 284         inaddr.sin_family = AF_INET;
 285         inaddr.sin_addr.s_addr = htonl(INADDR_ANY);
 286         inaddr.sin_port = htons(port);
 287
 288         while(bind(socki, (struct sockaddr*)&inaddr, sizeof(inaddr)) < 0) {
 289                 printf(".");fflush(NULL);
 290                 usleep(100000);
 291         }
 292
 293         if (listen(socki, 3) != 0) {
 294                 perror("listen");
 295                 exit(1);
 296         }
 297
 298         while(1) {
 299                 accsocket = accept(socki, (struct sockaddr*)&clientaddr, &sin_size);
 300                 if (accsocket > 0) {
 301                         //printf("accepted\n");
 302                         if (!fork())
 303                                 break;
 304                         else
 305                                 close(accsocket);
 306                 }
 307                 else if (errno != EINTR) {
 308                         perror("accept");
 309                         exit(1);
 310                 }
 311         }
 312
 313         for (i=0; i<currmax; i++) {
 314
 315 #ifdef DEBUG
 316                 printf("open %d '%s' m%08X v%08X i%d e%d.\n",
 317                        i, argv[optind+i], mask[i], value[i],
 318                        inv_filter[i], err_mask[i]);
 319 #endif
 320
 321                 if ((s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
 322                         perror("socket");
 323                         return 1;
 324                 }
 325
 326                 if (mask[i] || value[i]) {
 327
 328                         printf("CAN ID filter[%d] for %s set to "
 329                                "mask = %08X, value = %08X %s\n",
 330                                i, argv[optind+i], mask[i], value[i],
 331                                (inv_filter[i]) ? "(inv_filter)" : "");
 332
 333                         rfilter.can_id   = value[i];
 334                         rfilter.can_mask = mask[i];
 335                         if (inv_filter[i])
 336                                 rfilter.can_id |= CAN_INV_FILTER;
 337
 338                         setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_FILTER,
 339                                    &rfilter, sizeof(rfilter));
 340                 }
 341
 342                 if (err_mask[i])
 343                         setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
 344                                    &err_mask[i], sizeof(err_mask[i]));
 345
 346                 /* try to switch the socket into CAN FD mode */
 347                 setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on));
 348
 349                 j = strlen(argv[optind+i]);
 350
 351                 if (!(j < IFNAMSIZ)) {
 352                         printf("name of CAN device '%s' is too long!\n", argv[optind+i]);
 353                         return 1;
 354                 }
 355
 356                 if (j > max_devname_len)
 357                         max_devname_len = j; /* for nice printing */
 358
 359                 addr.can_family = AF_CAN;
 360
 361                 if (strcmp(ANYDEV, argv[optind+i])) {
 362                         strcpy(ifr.ifr_name, argv[optind+i]);
 363                         if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
 364                                 perror("SIOCGIFINDEX");
 365                                 exit(1);
 366                         }
 367                         addr.can_ifindex = ifr.ifr_ifindex;
 368                 }
 369                 else
 370                         addr.can_ifindex = 0; /* any can interface */
 371
 372                 if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 373                         perror("bindcan");
 374                         return 1;
 375                 }
 376         }
 377
 378         while (running) {
 379
 380                 FD_ZERO(&rdfs);
 381                 for (i=0; i<currmax; i++)
 382                         FD_SET(s[i], &rdfs);
 383
 384                 if ((ret = select(s[currmax-1]+1, &rdfs, NULL, NULL, NULL)) < 0) {
 385                         //perror("select");
 386                         running = 0;
 387                         continue;
 388                 }
 389
 390                 for (i=0; i<currmax; i++) {  /* check all CAN RAW sockets */
 391
 392                         if (FD_ISSET(s[i], &rdfs)) {
 393
 394                                 socklen_t len = sizeof(addr);
 395                                 int idx;
 396
 397                                 if ((nbytes = recvfrom(s[i], &frame, CANFD_MTU, 0,
 398                                                        (struct sockaddr*)&addr, &len)) < 0) {
 399                                         perror("read");
 400                                         return 1;
 401                                 }
 402
 403                                 if ((size_t)nbytes == CAN_MTU)
 404                                         maxdlen = CAN_MAX_DLEN;
 405                                 else if ((size_t)nbytes == CANFD_MTU)
 406                                         maxdlen = CANFD_MAX_DLEN;
 407                                 else {
 408                                         fprintf(stderr, "read: incomplete CAN frame\n");
 409                                         return 1;
 410                                 }
 411
 412                                 if (ioctl(s[i], SIOCGSTAMP, &tv) < 0)
 413                                         perror("SIOCGSTAMP");
 414
 415
 416                                 idx = idx2dindex(addr.can_ifindex, s[i]);
 417
 418                                 sprintf(temp, "(%ld.%06ld) %*s ",
 419                                         tv.tv_sec, tv.tv_usec, max_devname_len, devname[idx]);
 420                                 sprint_canframe(temp+strlen(temp), &frame, 0, maxdlen);
 421                                 strcat(temp, "\n");
 422
 423                                 if (write(accsocket, temp, strlen(temp)) < 0) {
 424                                         perror("writeaccsock");
 425                                         return 1;
 426                                 }
 427
 428 #if 0
 429                                 /* print CAN frame in log file style to stdout */
 430                                 printf("(%ld.%06ld) ", tv.tv_sec, tv.tv_usec);
 431                                 printf("%*s ", max_devname_len, devname[idx]);
 432                                 fprint_canframe(stdout, &frame, "\n", 0, maxdlen);
 433 #endif
 434                         }
 435
 436                 }
 437         }
 438
 439         for (i=0; i<currmax; i++)
 440                 close(s[i]);
 441
 442         close(accsocket);
 443         return 0;
 444 }