isotpserver.c

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