isotpserver.c

   1 /*
   2  *  $Id$
   3  */
   4
   5 /*
   6  * isotpserver.c
   7  *
   8  * Implements a socket server which understands ASCII HEX
   9  * messages for simple TCP/IP <-> ISO 15765-2 bridging.
  10  *
  11  * General message format: <[data]+>
  12  *
  13  * e.g. for an eight bytes PDU
  14  *
  15  * <1122334455667788>
  16  *
  17  * Valid ISO 15625-2 PDUs have a length from 1-4095 bytes.
  18  *
  19  * Authors:
  20  * Andre Naujoks (the socket server stuff)
  21  * Oliver Hartkopp (the rest)
  22  *
  23  * Copyright (c) 2002-2010 Volkswagen Group Electronic Research
  24  * All rights reserved.
  25  *
  26  * Redistribution and use in source and binary forms, with or without
  27  * modification, are permitted provided that the following conditions
  28  * are met:
  29  * 1. Redistributions of source code must retain the above copyright
  30  *    notice, this list of conditions and the following disclaimer.
  31  * 2. Redistributions in binary form must reproduce the above copyright
  32  *    notice, this list of conditions and the following disclaimer in the
  33  *    documentation and/or other materials provided with the distribution.
  34  * 3. Neither the name of Volkswagen nor the names of its contributors
  35  *    may be used to endorse or promote products derived from this software
  36  *    without specific prior written permission.
  37  *
  38  * Alternatively, provided that this notice is retained in full, this
  39  * software may be distributed under the terms of the GNU General
  40  * Public License ("GPL") version 2, in which case the provisions of the
  41  * GPL apply INSTEAD OF those given above.
  42  *
  43  * The provided data structures and external interfaces from this code
  44  * are not restricted to be used by modules with a GPL compatible license.
  45  *
  46  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  47  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  48  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  49  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  50  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  51  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  52  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  53  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  54  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  55  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  56  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  57  * DAMAGE.
  58  *
  59  * Send feedback to <linux-can@vger.kernel.org>
  60  *
  61  */
  62
  63 #include <stdio.h>
  64 #include <stdlib.h>
  65 #include <libgen.h>
  66 #include <unistd.h>
  67 #include <string.h>
  68 #include <signal.h>
  69 #include <errno.h>
  70
  71 #include <sys/types.h>
  72 #include <sys/wait.h>
  73 #include <sys/socket.h>
  74 #include <sys/ioctl.h>
  75 #include <sys/uio.h>
  76 #include <net/if.h>
  77 #include <netinet/in.h>
  78
  79 #include <linux/can.h>
  80 #include <linux/can/isotp.h>
  81
  82 #define NO_CAN_ID 0xFFFFFFFFU
  83
  84 int b64hex(char *asc, unsigned char *bin, int len)
  85 {
  86         int i;
  87
  88         for (i = 0; i < len; i++) {
  89                 if (!sscanf(asc+(i*2), "%2hhx", bin+i))
  90                         return 1;
  91         }
  92         return 0;
  93 }
  94
  95 void childdied(int i)
  96 {
  97         wait(NULL);
  98 }
  99
 100 void print_usage(char *prg)
 101 {
 102         fprintf(stderr, "\nUsage: %s -l <port> -s <can_id> -d <can_id> [options] <CAN interface>\n", prg);
 103         fprintf(stderr, "Options: (* = mandatory)\n");
 104         fprintf(stderr, "\n");
 105         fprintf(stderr, "ip adressing:\n");
 106         fprintf(stderr, " *       -l <port>    (local port for the server)\n");
 107         fprintf(stderr, "\n");
 108         fprintf(stderr, "isotp adressing:\n");
 109         fprintf(stderr, " *       -s <can_id>  (source can_id. Use 8 digits for extended IDs)\n");
 110         fprintf(stderr, " *       -d <can_id>  (destination can_id. Use 8 digits for extended IDs)\n");
 111         fprintf(stderr, "         -x <addr>    (extended addressing mode)\n");
 112         fprintf(stderr, "\n");
 113         fprintf(stderr, "padding:\n");
 114         fprintf(stderr, "         -p <byte>    (set and enable tx padding byte)\n");
 115         fprintf(stderr, "         -r <byte>    (set and enable rx padding byte)\n");
 116         fprintf(stderr, "         -P <mode>    (check padding in SF/CF. (l)ength (c)ontent (a)ll)\n");
 117         fprintf(stderr, "\n");
 118         fprintf(stderr, "rx path: (config, which is sent to the sender / data source)\n");
 119         fprintf(stderr, "         -b <bs>      (blocksize. 0 = off)\n");
 120         fprintf(stderr, "         -m <val>     (STmin in ms/ns. See spec.)\n");
 121         fprintf(stderr, "         -w <num>     (max. wait frame transmissions)\n");
 122         fprintf(stderr, "\n");
 123         fprintf(stderr, "tx path: (config, which changes local tx settings)\n");
 124         fprintf(stderr, "         -t <time ns> (transmit time in nanosecs)\n");
 125         fprintf(stderr, "\n");
 126         fprintf(stderr, "All values except for '-l' and '-t' are expected in hexadecimal values.\n");
 127         fprintf(stderr, "\n");
 128 }
 129
 130 int main(int argc, char **argv)
 131 {
 132         extern int optind, opterr, optopt;
 133         int opt;
 134
 135         int sl, sa, sc; /* (L)isten, (A)ccept, (C)AN sockets */
 136         struct sockaddr_in  saddr, clientaddr;
 137         struct sockaddr_can caddr;
 138         static struct can_isotp_options opts;
 139         static struct can_isotp_fc_options fcopts;
 140         struct ifreq ifr;
 141         socklen_t sin_size = sizeof(clientaddr);
 142         socklen_t caddrlen = sizeof(caddr);
 143
 144         struct sigaction signalaction;
 145         sigset_t sigset;
 146
 147         fd_set readfds;
 148
 149         int i, ret;
 150         int nbytes;
 151
 152         int local_port = 0;
 153         int verbose = 0;
 154
 155         int idx = 0; /* index in txmsg[] */
 156
 157         unsigned char msg[4096];   /* isotp socket message buffer (4095 + test_for_too_long_byte)*/
 158         char rxmsg[8194]; /* isotp->tcp ASCII message buffer (4095*2 + < > \n null) */
 159         char txmsg[8193]; /* tcp->isotp ASCII message buffer (4095*2 + < > null) */
 160
 161         /* mark missing mandatory commandline options as missing */
 162         caddr.can_addr.tp.tx_id = caddr.can_addr.tp.rx_id = NO_CAN_ID;
 163
 164         while ((opt = getopt(argc, argv, "l:s:d:x:p:r:P:b:m:w:t:v?")) != -1) {
 165                 switch (opt) {
 166                 case 'l':
 167                         local_port = strtoul(optarg, (char **)NULL, 10);
 168                         break;
 169
 170                 case 's':
 171                         caddr.can_addr.tp.tx_id = strtoul(optarg, (char **)NULL, 16);
 172                         if (strlen(optarg) > 7)
 173                                 caddr.can_addr.tp.tx_id |= CAN_EFF_FLAG;
 174                         break;
 175
 176                 case 'd':
 177                         caddr.can_addr.tp.rx_id = strtoul(optarg, (char **)NULL, 16);
 178                         if (strlen(optarg) > 7)
 179                                 caddr.can_addr.tp.rx_id |= CAN_EFF_FLAG;
 180                         break;
 181
 182                 case 'x':
 183                         opts.flags |= CAN_ISOTP_EXTEND_ADDR;
 184                         opts.ext_address = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 185                         break;
 186
 187                 case 'p':
 188                         opts.flags |= CAN_ISOTP_TX_PADDING;
 189                         opts.txpad_content = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 190                         break;
 191
 192                 case 'r':
 193                         opts.flags |= CAN_ISOTP_RX_PADDING;
 194                         opts.rxpad_content = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 195                         break;
 196
 197                 case 'P':
 198                         if (optarg[0] == 'l')
 199                                 opts.flags |= CAN_ISOTP_CHK_PAD_LEN;
 200                         else if (optarg[0] == 'c')
 201                                 opts.flags |= CAN_ISOTP_CHK_PAD_DATA;
 202                         else if (optarg[0] == 'a')
 203                                 opts.flags |= (CAN_ISOTP_CHK_PAD_DATA | CAN_ISOTP_CHK_PAD_DATA);
 204                         else {
 205                                 printf("unknown padding check option '%c'.\n", optarg[0]);
 206                                 print_usage(basename(argv[0]));
 207                                 exit(0);
 208                         }
 209                         break;
 210
 211                 case 'b':
 212                         fcopts.bs = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 213                         break;
 214
 215                 case 'm':
 216                         fcopts.stmin = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 217                         break;
 218
 219                 case 'w':
 220                         fcopts.wftmax = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 221                         break;
 222
 223                 case 't':
 224                         opts.frame_txtime = strtoul(optarg, (char **)NULL, 10);
 225                         break;
 226
 227                 case 'v':
 228                         verbose = 1;
 229                         break;
 230
 231                 case '?':
 232                         print_usage(basename(argv[0]));
 233                         exit(0);
 234                         break;
 235
 236                 default:
 237                         fprintf(stderr, "Unknown option %c\n", opt);
 238                         print_usage(basename(argv[0]));
 239                         exit(1);
 240                         break;
 241                 }
 242         }
 243
 244         if ((argc - optind != 1) || (local_port == 0) ||
 245             (caddr.can_addr.tp.tx_id == NO_CAN_ID) ||
 246             (caddr.can_addr.tp.rx_id == NO_CAN_ID)) {
 247                 print_usage(basename(argv[0]));
 248                 exit(1);
 249         }
 250
 251         sigemptyset(&sigset);
 252         signalaction.sa_handler = &childdied;
 253         signalaction.sa_mask = sigset;
 254         signalaction.sa_flags = 0;
 255         sigaction(SIGCHLD, &signalaction, NULL);  /* signal for dying child */
 256
 257         if((sl = socket(PF_INET, SOCK_STREAM, 0)) < 0) {
 258                 perror("inetsocket");
 259                 exit(1);
 260         }
 261
 262         saddr.sin_family = AF_INET;
 263         saddr.sin_addr.s_addr = htonl(INADDR_ANY);
 264         saddr.sin_port = htons(local_port);
 265
 266         while(bind(sl,(struct sockaddr*)&saddr, sizeof(saddr)) < 0) {
 267                 printf(".");
 268                 fflush(NULL);
 269                 usleep(100000);
 270         }
 271
 272         if (listen(sl, 3) != 0) {
 273                 perror("listen");
 274                 exit(1);
 275         }
 276
 277         while (1) {
 278                 sa = accept(sl,(struct sockaddr *)&clientaddr, &sin_size);
 279                 if (sa > 0 ){
 280
 281                         if (fork())
 282                                 close(sa);
 283                         else
 284                                 break;
 285                 }
 286                 else {
 287                         if (errno != EINTR) {
 288                                 /*
 289                                  * If the cause for the error was NOT the
 290                                  * signal from a dying child => give an error
 291                                  */
 292                                 perror("accept");
 293                                 exit(1);
 294                         }
 295                 }
 296         }
 297
 298         if ((sc = socket(PF_CAN, SOCK_DGRAM, CAN_ISOTP)) < 0) {
 299                 perror("socket");
 300                 exit(1);
 301         }
 302
 303         setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
 304         setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts));
 305
 306         caddr.can_family = AF_CAN;
 307         strcpy(ifr.ifr_name, argv[optind]);
 308         if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
 309                 perror("SIOCGIFINDEX");
 310                 exit(1);
 311         }
 312         caddr.can_ifindex = ifr.ifr_ifindex;
 313
 314         if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
 315                 perror("bind");
 316                 exit(1);
 317         }
 318
 319         while (1) {
 320
 321                 FD_ZERO(&readfds);
 322                 FD_SET(sc, &readfds);
 323                 FD_SET(sa, &readfds);
 324
 325                 ret = select((sc > sa)?sc+1:sa+1, &readfds, NULL, NULL, NULL);
 326
 327                 if (FD_ISSET(sc, &readfds)) {
 328
 329
 330                         nbytes = read(sc, &msg, 4096);
 331
 332                         if (nbytes < 1 || nbytes > 4095) {
 333                                 perror("read from isotp socket");
 334                                 exit(1);
 335                         }
 336
 337                         rxmsg[0] = '<';
 338
 339                         for ( i = 0; i < nbytes; i++)
 340                                 sprintf(rxmsg + 1 + 2*i, "%02X", msg[i]);
 341
 342                         /* finalize string for sending */
 343                         strcat(rxmsg, ">\n");
 344
 345                         if (verbose)
 346                                 printf("CAN>TCP %s", rxmsg);
 347
 348                         send(sa, rxmsg, strlen(rxmsg), 0);
 349                 }
 350
 351
 352                 if (FD_ISSET(sa, &readfds)) {
 353
 354                         if (read(sa, txmsg+idx, 1) < 1) {
 355                                 perror("read from tcp/ip socket");
 356                                 exit(1);
 357                         }
 358
 359                         if (!idx) {
 360                                 if (txmsg[0] == '<')
 361                                         idx = 1;
 362
 363                                 continue;
 364                         }
 365
 366                         /* max len is 4095*2 + '<' + '>' = 8192. The buffer index starts with 0 */
 367                         if (idx > 8191) {
 368                                 idx = 0;
 369                                 continue;
 370                         }
 371
 372                         if (txmsg[idx] != '>') {
 373                                 idx++;
 374                                 continue;
 375                         }
 376
 377                         txmsg[idx+1] = 0;
 378                         idx = 0;
 379
 380                         /* must be an even number of bytes and at least one data byte <XX> */
 381                         if (strlen(txmsg) < 4 || strlen(txmsg) % 2)
 382                                 continue;
 383
 384                         if (verbose)
 385                                 printf("TCP>CAN %s\n", txmsg);
 386
 387                         nbytes = (strlen(txmsg)-2)/2;
 388                         if (b64hex(txmsg+1, msg, nbytes) == 0)
 389                                 send(sc, msg, nbytes, 0);
 390                 }
 391         }
 392
 393         close(sc);
 394         close(sa);
 395
 396         return 0;
 397 }