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 /* allow PDUs greater 4095 bytes according ISO 15765-2:2015 */
  81 #define MAX_PDU_LENGTH 6000
  82
  83 int b64hex(char *asc, unsigned char *bin, int len)
  84 {
  85         int i;
  86
  87         for (i = 0; i < len; i++) {
  88                 if (!sscanf(asc+(i*2), "%2hhx", bin+i))
  89                         return 1;
  90         }
  91         return 0;
  92 }
  93
  94 void childdied(int i)
  95 {
  96         wait(NULL);
  97 }
  98
  99 void print_usage(char *prg)
 100 {
 101         fprintf(stderr, "\nUsage: %s -l <port> -s <can_id> -d <can_id> [options] <CAN interface>\n", prg);
 102         fprintf(stderr, "Options: (* = mandatory)\n");
 103         fprintf(stderr, "\n");
 104         fprintf(stderr, "ip adressing:\n");
 105         fprintf(stderr, " *       -l <port>    (local port for the server)\n");
 106         fprintf(stderr, "\n");
 107         fprintf(stderr, "isotp adressing:\n");
 108         fprintf(stderr, " *       -s <can_id>  (source can_id. Use 8 digits for extended IDs)\n");
 109         fprintf(stderr, " *       -d <can_id>  (destination can_id. Use 8 digits for extended IDs)\n");
 110         fprintf(stderr, "         -x <addr>[:<rxaddr>] (extended addressing / opt. separate rxaddr)\n");
 111         fprintf(stderr, "         -L <mtu>:<tx_dl>:<tx_flags> (link layer options for CAN FD)\n");
 112         fprintf(stderr, "\n");
 113         fprintf(stderr, "padding:\n");
 114         fprintf(stderr, "         -p [tx]:[rx] (set and enable tx/rx padding bytes)\n");
 115         fprintf(stderr, "         -P <mode>    (check rx padding for (l)ength (c)ontent (a)ll)\n");
 116         fprintf(stderr, "\n");
 117         fprintf(stderr, "rx path: (config, which is sent to the sender / data source)\n");
 118         fprintf(stderr, "         -b <bs>      (blocksize. 0 = off)\n");
 119         fprintf(stderr, "         -m <val>     (STmin in ms/ns. See spec.)\n");
 120         fprintf(stderr, "         -w <num>     (max. wait frame transmissions)\n");
 121         fprintf(stderr, "\n");
 122         fprintf(stderr, "tx path: (config, which changes local tx settings)\n");
 123         fprintf(stderr, "         -t <time ns> (transmit time in nanosecs)\n");
 124         fprintf(stderr, "\n");
 125         fprintf(stderr, "All values except for '-l' and '-t' are expected in hexadecimal values.\n");
 126         fprintf(stderr, "\n");
 127 }
 128
 129 int main(int argc, char **argv)
 130 {
 131         extern int optind, opterr, optopt;
 132         int opt;
 133
 134         int sl, sa, sc; /* (L)isten, (A)ccept, (C)AN sockets */
 135         struct sockaddr_in  saddr, clientaddr;
 136         struct sockaddr_can caddr;
 137         static struct can_isotp_options opts;
 138         static struct can_isotp_fc_options fcopts;
 139         static struct can_isotp_ll_options llopts;
 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;
 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[MAX_PDU_LENGTH + 1];   /* isotp socket message buffer (4095 + test_for_too_long_byte)*/
 158         char rxmsg[MAX_PDU_LENGTH * 2 + 4]; /* isotp->tcp ASCII message buffer (4095*2 + < > \n null) */
 159         char txmsg[MAX_PDU_LENGTH * 2 + 3]; /* 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:P:b:m:w:t:L: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                 {
 184                         int elements = sscanf(optarg, "%hhx:%hhx",
 185                                               &opts.ext_address,
 186                                               &opts.rx_ext_address);
 187
 188                         if (elements == 1)
 189                                 opts.flags |= CAN_ISOTP_EXTEND_ADDR;
 190                         else if (elements == 2)
 191                                 opts.flags |= (CAN_ISOTP_EXTEND_ADDR | CAN_ISOTP_RX_EXT_ADDR);
 192                         else {
 193                                 printf("incorrect extended addr values '%s'.\n", optarg);
 194                                 print_usage(basename(argv[0]));
 195                                 exit(0);
 196                         }
 197                         break;
 198                 }
 199
 200                 case 'p':
 201                 {
 202                         int elements = sscanf(optarg, "%hhx:%hhx",
 203                                               &opts.txpad_content,
 204                                               &opts.rxpad_content);
 205
 206                         if (elements == 1)
 207                                 opts.flags |= CAN_ISOTP_TX_PADDING;
 208                         else if (elements == 2)
 209                                 opts.flags |= (CAN_ISOTP_TX_PADDING | CAN_ISOTP_RX_PADDING);
 210                         else if (sscanf(optarg, ":%hhx", &opts.rxpad_content) == 1)
 211                                 opts.flags |= CAN_ISOTP_RX_PADDING;
 212                         else {
 213                                 printf("incorrect padding values '%s'.\n", optarg);
 214                                 print_usage(basename(argv[0]));
 215                                 exit(0);
 216                         }
 217                         break;
 218                 }
 219
 220                 case 'P':
 221                         if (optarg[0] == 'l')
 222                                 opts.flags |= CAN_ISOTP_CHK_PAD_LEN;
 223                         else if (optarg[0] == 'c')
 224                                 opts.flags |= CAN_ISOTP_CHK_PAD_DATA;
 225                         else if (optarg[0] == 'a')
 226                                 opts.flags |= (CAN_ISOTP_CHK_PAD_LEN | CAN_ISOTP_CHK_PAD_DATA);
 227                         else {
 228                                 printf("unknown padding check option '%c'.\n", optarg[0]);
 229                                 print_usage(basename(argv[0]));
 230                                 exit(0);
 231                         }
 232                         break;
 233
 234                 case 'b':
 235                         fcopts.bs = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 236                         break;
 237
 238                 case 'm':
 239                         fcopts.stmin = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 240                         break;
 241
 242                 case 'w':
 243                         fcopts.wftmax = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 244                         break;
 245
 246                 case 't':
 247                         opts.frame_txtime = strtoul(optarg, (char **)NULL, 10);
 248                         break;
 249
 250                 case 'L':
 251                         if (sscanf(optarg, "%hhu:%hhu:%hhu",
 252                                    &llopts.mtu,
 253                                    &llopts.tx_dl,
 254                                    &llopts.tx_flags) != 3) {
 255                                 printf("unknown link layer options '%s'.\n", optarg);
 256                                 print_usage(basename(argv[0]));
 257                                 exit(0);
 258                         }
 259                         break;
 260
 261                 case 'v':
 262                         verbose = 1;
 263                         break;
 264
 265                 case '?':
 266                         print_usage(basename(argv[0]));
 267                         exit(0);
 268                         break;
 269
 270                 default:
 271                         fprintf(stderr, "Unknown option %c\n", opt);
 272                         print_usage(basename(argv[0]));
 273                         exit(1);
 274                         break;
 275                 }
 276         }
 277
 278         if ((argc - optind != 1) || (local_port == 0) ||
 279             (caddr.can_addr.tp.tx_id == NO_CAN_ID) ||
 280             (caddr.can_addr.tp.rx_id == NO_CAN_ID)) {
 281                 print_usage(basename(argv[0]));
 282                 exit(1);
 283         }
 284
 285         sigemptyset(&sigset);
 286         signalaction.sa_handler = &childdied;
 287         signalaction.sa_mask = sigset;
 288         signalaction.sa_flags = 0;
 289         sigaction(SIGCHLD, &signalaction, NULL);  /* signal for dying child */
 290
 291         if((sl = socket(PF_INET, SOCK_STREAM, 0)) < 0) {
 292                 perror("inetsocket");
 293                 exit(1);
 294         }
 295
 296         saddr.sin_family = AF_INET;
 297         saddr.sin_addr.s_addr = htonl(INADDR_ANY);
 298         saddr.sin_port = htons(local_port);
 299
 300         while(bind(sl,(struct sockaddr*)&saddr, sizeof(saddr)) < 0) {
 301                 printf(".");
 302                 fflush(NULL);
 303                 usleep(100000);
 304         }
 305
 306         if (listen(sl, 3) != 0) {
 307                 perror("listen");
 308                 exit(1);
 309         }
 310
 311         while (1) {
 312                 sa = accept(sl,(struct sockaddr *)&clientaddr, &sin_size);
 313                 if (sa > 0 ){
 314
 315                         if (fork())
 316                                 close(sa);
 317                         else
 318                                 break;
 319                 }
 320                 else {
 321                         if (errno != EINTR) {
 322                                 /*
 323                                  * If the cause for the error was NOT the
 324                                  * signal from a dying child => give an error
 325                                  */
 326                                 perror("accept");
 327                                 exit(1);
 328                         }
 329                 }
 330         }
 331
 332         if ((sc = socket(PF_CAN, SOCK_DGRAM, CAN_ISOTP)) < 0) {
 333                 perror("socket");
 334                 exit(1);
 335         }
 336
 337         setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
 338         setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts));
 339
 340         if (llopts.tx_dl) {
 341                 if (setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_LL_OPTS, &llopts, sizeof(llopts)) < 0) {
 342                         perror("link layer sockopt");
 343                         exit(1);
 344                 }
 345         }
 346
 347         caddr.can_family = AF_CAN;
 348         strcpy(ifr.ifr_name, argv[optind]);
 349         if (ioctl(sc, SIOCGIFINDEX, &ifr) < 0) {
 350                 perror("SIOCGIFINDEX");
 351                 exit(1);
 352         }
 353         caddr.can_ifindex = ifr.ifr_ifindex;
 354
 355         if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
 356                 perror("bind");
 357                 exit(1);
 358         }
 359
 360         while (1) {
 361
 362                 FD_ZERO(&readfds);
 363                 FD_SET(sc, &readfds);
 364                 FD_SET(sa, &readfds);
 365
 366                 select((sc > sa)?sc+1:sa+1, &readfds, NULL, NULL, NULL);
 367
 368                 if (FD_ISSET(sc, &readfds)) {
 369
 370
 371                         nbytes = read(sc, &msg, MAX_PDU_LENGTH + 1);
 372
 373                         if (nbytes < 1 || nbytes > MAX_PDU_LENGTH) {
 374                                 perror("read from isotp socket");
 375                                 exit(1);
 376                         }
 377
 378                         rxmsg[0] = '<';
 379
 380                         for ( i = 0; i < nbytes; i++)
 381                                 sprintf(rxmsg + 1 + 2*i, "%02X", msg[i]);
 382
 383                         /* finalize string for sending */
 384                         strcat(rxmsg, ">\n");
 385
 386                         if (verbose)
 387                                 printf("CAN>TCP %s", rxmsg);
 388
 389                         send(sa, rxmsg, strlen(rxmsg), 0);
 390                 }
 391
 392
 393                 if (FD_ISSET(sa, &readfds)) {
 394
 395                         if (read(sa, txmsg+idx, 1) < 1) {
 396                                 perror("read from tcp/ip socket");
 397                                 exit(1);
 398                         }
 399
 400                         if (!idx) {
 401                                 if (txmsg[0] == '<')
 402                                         idx = 1;
 403
 404                                 continue;
 405                         }
 406
 407                         /* max len is 4095*2 + '<' + '>' = 8192. The buffer index starts with 0 */
 408                         if (idx > MAX_PDU_LENGTH * 2 + 1) {
 409                                 idx = 0;
 410                                 continue;
 411                         }
 412
 413                         if (txmsg[idx] != '>') {
 414                                 idx++;
 415                                 continue;
 416                         }
 417
 418                         txmsg[idx+1] = 0;
 419                         idx = 0;
 420
 421                         /* must be an even number of bytes and at least one data byte <XX> */
 422                         if (strlen(txmsg) < 4 || strlen(txmsg) % 2)
 423                                 continue;
 424
 425                         if (verbose)
 426                                 printf("TCP>CAN %s\n", txmsg);
 427
 428                         nbytes = (strlen(txmsg)-2)/2;
 429                         if (b64hex(txmsg+1, msg, nbytes) == 0)
 430                                 send(sc, msg, nbytes, 0);
 431                 }
 432         }
 433
 434         close(sc);
 435         close(sa);
 436
 437         return 0;
 438 }