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 <socketcan-users@lists.berlios.de>
  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/types.h>
  59 #include <sys/socket.h>
  60 #include <sys/ioctl.h>
  61 #include <sys/uio.h>
  62 #include <net/if.h>
  63 #include <netinet/in.h>
  64
  65 #include <linux/can.h>
  66 #include <linux/can/raw.h>
  67 #include <signal.h>
  68 #include <wait.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 DEFPORT 28700
  78
  79 static char devname[MAXDEV][IFNAMSIZ+1];
  80 static int  dindex[MAXDEV];
  81 static int  max_devname_len;
  82
  83 extern int optind, opterr, optopt;
  84
  85 static volatile int running = 1;
  86
  87 void print_usage(char *prg)
  88 {
  89     fprintf(stderr, "Usage: %s [can-interfaces]\n", prg);
  90     fprintf(stderr, "  (use CTRL-C to terminate %s)\n", prg);
  91     fprintf(stderr, "Options: -m <mask>   (default 0x00000000)\n");
  92     fprintf(stderr, "         -v <value>  (default 0x00000000)\n");
  93     fprintf(stderr, "         -i <0|1>    (inv_filter)\n");
  94     fprintf(stderr, "         -e <emask>  (mask for error frames)\n");
  95     fprintf(stderr, "         -p <port>   (Network port on which %s listens. Default: %d)\n", prg, DEFPORT);
  96     fprintf(stderr, "\n");
  97     fprintf(stderr, "When using more than one CAN interface the options\n");
  98     fprintf(stderr, "m/v/i/e have comma seperated values e.g. '-m 0,7FF,0'\n");
  99     fprintf(stderr, "Use interface name '%s' to receive from all can-interfaces\n", ANYDEV);
 100 }
 101
 102 int idx2dindex(int ifidx, int socket)
 103 {
 104     int i;
 105     struct ifreq ifr;
 106
 107     for (i=0; i<MAXDEV; i++) {
 108         if (dindex[i] == ifidx)
 109             return i;
 110     }
 111
 112     /* create new interface index cache entry */
 113
 114     /* remove index cache zombies first */
 115     for (i=0; i < MAXDEV; i++) {
 116         if (dindex[i]) {
 117             ifr.ifr_ifindex = dindex[i];
 118             if (ioctl(socket, SIOCGIFNAME, &ifr) < 0)
 119                 dindex[i] = 0;
 120         }
 121     }
 122
 123     for (i=0; i < MAXDEV; i++)
 124         if (!dindex[i]) /* free entry */
 125             break;
 126
 127     if (i == MAXDEV) {
 128         printf("Interface index cache only supports %d interfaces.\n", MAXDEV);
 129         exit(1);
 130     }
 131
 132     dindex[i] = ifidx;
 133
 134     ifr.ifr_ifindex = ifidx;
 135     if (ioctl(socket, SIOCGIFNAME, &ifr) < 0)
 136         perror("SIOCGIFNAME");
 137
 138     if (max_devname_len < strlen(ifr.ifr_name))
 139         max_devname_len = strlen(ifr.ifr_name);
 140
 141     strcpy(devname[i], ifr.ifr_name);
 142
 143 #ifdef DEBUG
 144     printf("new index %d (%s)\n", i, devname[i]);
 145 #endif
 146
 147     return i;
 148 }
 149
 150 /*
 151  * This is a Signalhandler. When we get a signal, that a child
 152  * terminated, we wait for it, so the zombie will disappear.
 153  */
 154 void childdied(int i)
 155 {
 156     wait(NULL);
 157 }
 158
 159 /*
 160  * This is a Signalhandler for a cought SIGTERM
 161  */
 162 void shutdown_gra(int i)
 163 {
 164     exit(0);
 165 }
 166
 167
 168 int main(int argc, char **argv)
 169 {
 170     struct sigaction signalaction;
 171     sigset_t sigset;
 172     fd_set rdfs;
 173     int s[MAXDEV];
 174     int socki, accsocket;
 175     canid_t mask[MAXDEV] = {0};
 176     canid_t value[MAXDEV] = {0};
 177     int inv_filter[MAXDEV] = {0};
 178     can_err_mask_t err_mask[MAXDEV] = {0};
 179     int opt, ret;
 180     int currmax = 1; /* we assume at least one can bus ;-) */
 181     struct sockaddr_can addr;
 182     struct can_filter rfilter;
 183     struct can_frame frame;
 184     int nbytes, i, j;
 185     struct ifreq ifr;
 186     struct timeval tv, last_tv;
 187     int port = DEFPORT;
 188     struct sockaddr_in inaddr;
 189     struct sockaddr_in clientaddr;
 190     socklen_t sin_size = sizeof(clientaddr);
 191     char temp[128];
 192
 193     sigemptyset(&sigset);
 194     signalaction.sa_handler = &childdied;
 195     signalaction.sa_mask = sigset;
 196     signalaction.sa_flags = 0;
 197     sigaction(SIGCHLD, &signalaction, NULL);  /* install signal for dying child */
 198     signalaction.sa_handler = &shutdown_gra;
 199     signalaction.sa_mask = sigset;
 200     signalaction.sa_flags = 0;
 201     sigaction(SIGTERM, &signalaction, NULL); /* install Signal for termination */
 202     sigaction(SIGINT, &signalaction, NULL); /* install Signal for termination */
 203
 204
 205     last_tv.tv_sec  = 0;
 206     last_tv.tv_usec = 0;
 207
 208     while ((opt = getopt(argc, argv, "m:v:i:e:p:")) != -1) {
 209
 210         switch (opt) {
 211         case 'm':
 212             i = sscanf(optarg, "%x,%x,%x,%x,%x,%x",
 213                        &mask[0], &mask[1], &mask[2],
 214                        &mask[3], &mask[4], &mask[5]);
 215             if (i > currmax)
 216                 currmax = i;
 217             break;
 218
 219         case 'v':
 220             i = sscanf(optarg, "%x,%x,%x,%x,%x,%x",
 221                        &value[0], &value[1], &value[2],
 222                        &value[3], &value[4], &value[5]);
 223             if (i > currmax)
 224                 currmax = i;
 225             break;
 226
 227         case 'i':
 228             i = sscanf(optarg, "%d,%d,%d,%d,%d,%d",
 229                        &inv_filter[0], &inv_filter[1], &inv_filter[2],
 230                        &inv_filter[3], &inv_filter[4], &inv_filter[5]);
 231             if (i > currmax)
 232                 currmax = i;
 233             break;
 234
 235         case 'e':
 236             i = sscanf(optarg, "%x,%x,%x,%x,%x,%x",
 237                        &err_mask[0], &err_mask[1], &err_mask[2],
 238                        &err_mask[3], &err_mask[4], &err_mask[5]);
 239             if (i > currmax)
 240                 currmax = i;
 241             break;
 242         case 'p':
 243             port = atoi(optarg);
 244             break;
 245         default:
 246             fprintf(stderr, "Unknown option %c\n", opt);
 247             print_usage(basename(argv[0]));
 248             exit(1);
 249             break;
 250         }
 251     }
 252
 253     if (optind == argc) {
 254         print_usage(basename(argv[0]));
 255         exit(0);
 256     }
 257
 258     /* count in options higher than device count ? */
 259     if (optind + currmax > argc) {
 260         printf("low count of CAN devices!\n");
 261         return 1;
 262     }
 263
 264     currmax = argc - optind; /* find real number of CAN devices */
 265
 266     if (currmax > MAXDEV) {
 267         printf("More than %d CAN devices!\n", MAXDEV);
 268         return 1;
 269     }
 270
 271
 272     socki = socket(PF_INET, SOCK_STREAM, 0);
 273     if (socki < 0) {
 274         perror("socket");
 275         exit(1);
 276     }
 277
 278     inaddr.sin_family = AF_INET;
 279     inaddr.sin_addr.s_addr = htonl(INADDR_ANY);
 280     inaddr.sin_port = htons(port);
 281
 282     while(bind(socki, (struct sockaddr*)&inaddr, sizeof(inaddr)) < 0) {
 283         printf(".");fflush(NULL);
 284         usleep(100000);
 285     }
 286
 287     if (listen(socki, 3) != 0) {
 288         perror("listen");
 289         exit(1);
 290     }
 291
 292     while(1) {
 293         accsocket = accept(socki, (struct sockaddr*)&clientaddr, &sin_size);
 294         if (accsocket > 0) {
 295             //printf("accepted\n");
 296             if (!fork())
 297                 break;
 298             else
 299                 close(accsocket);
 300         }
 301         else if (errno != EINTR) {
 302             perror("accept");
 303             exit(1);
 304         }
 305     }
 306
 307     for (i=0; i<currmax; i++) {
 308
 309 #ifdef DEBUG
 310         printf("open %d '%s' m%08X v%08X i%d e%d.\n",
 311                i, argv[optind+i], mask[i], value[i],
 312                inv_filter[i], err_mask[i]);
 313 #endif
 314
 315         if ((s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
 316             perror("socket");
 317             return 1;
 318         }
 319
 320         if (mask[i] || value[i]) {
 321
 322             printf("CAN ID filter[%d] for %s set to "
 323                    "mask = %08X, value = %08X %s\n",
 324                    i, argv[optind+i], mask[i], value[i],
 325                    (inv_filter[i]) ? "(inv_filter)" : "");
 326
 327             rfilter.can_id   = value[i];
 328             rfilter.can_mask = mask[i];
 329             if (inv_filter[i])
 330                 rfilter.can_id |= CAN_INV_FILTER;
 331
 332             setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_FILTER,
 333                        &rfilter, sizeof(rfilter));
 334         }
 335
 336         if (err_mask[i])
 337             setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
 338                        &err_mask[i], sizeof(err_mask[i]));
 339
 340         j = strlen(argv[optind+i]);
 341
 342         if (!(j < IFNAMSIZ)) {
 343             printf("name of CAN device '%s' is too long!\n", argv[optind+i]);
 344             return 1;
 345         }
 346
 347         if (j > max_devname_len)
 348             max_devname_len = j; /* for nice printing */
 349
 350         addr.can_family = AF_CAN;
 351
 352         if (strcmp(ANYDEV, argv[optind+i])) {
 353             strcpy(ifr.ifr_name, argv[optind+i]);
 354             if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
 355                 perror("SIOCGIFINDEX");
 356                 exit(1);
 357             }
 358             addr.can_ifindex = ifr.ifr_ifindex;
 359         }
 360         else
 361             addr.can_ifindex = 0; /* any can interface */
 362
 363         if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 364             perror("bindcan");
 365             return 1;
 366         }
 367     }
 368
 369     while (running) {
 370
 371         FD_ZERO(&rdfs);
 372         for (i=0; i<currmax; i++)
 373             FD_SET(s[i], &rdfs);
 374
 375         if ((ret = select(s[currmax-1]+1, &rdfs, NULL, NULL, NULL)) < 0) {
 376             //perror("select");
 377             running = 0;
 378             continue;
 379         }
 380
 381         for (i=0; i<currmax; i++) {  /* check all CAN RAW sockets */
 382
 383             if (FD_ISSET(s[i], &rdfs)) {
 384
 385                 socklen_t len = sizeof(addr);
 386                 int idx;
 387
 388                 if ((nbytes = recvfrom(s[i], &frame,
 389                                        sizeof(struct can_frame), 0,
 390                                        (struct sockaddr*)&addr, &len)) < 0) {
 391                     perror("read");
 392                     return 1;
 393                 }
 394
 395                 if (nbytes < sizeof(struct can_frame)) {
 396                     fprintf(stderr, "read: incomplete CAN frame\n");
 397                     return 1;
 398                 }
 399
 400                 if (ioctl(s[i], SIOCGSTAMP, &tv) < 0)
 401                     perror("SIOCGSTAMP");
 402
 403
 404                 idx = idx2dindex(addr.can_ifindex, s[i]);
 405
 406                 sprintf(temp, "(%ld.%06ld) %*s ",
 407                         tv.tv_sec, tv.tv_usec, max_devname_len, devname[idx]);
 408                 sprint_canframe(temp+strlen(temp), &frame, 0);
 409                 strcat(temp, "\n");
 410
 411                 if (write(accsocket, temp, strlen(temp)) < 0) {
 412                     perror("writeaccsock");
 413                     return 1;
 414                 }
 415
 416                 /* printf("%s\n",temp2); */
 417
 418 #if 0
 419                 /* print CAN frame in log file style to stdout */
 420                 printf("(%ld.%06ld) ", tv.tv_sec, tv.tv_usec);
 421                 printf("%*s ", max_devname_len, devname[idx]);
 422                 fprint_canframe(stdout, &frame, "\n", 0);
 423 #endif
 424             }
 425
 426         }
 427     }
 428
 429     for (i=0; i<currmax; i++)
 430         close(s[i]);
 431
 432     close(accsocket);
 433     return 0;
 434 }