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         socklen_t sin_size = sizeof(clientaddr);
 141         socklen_t caddrlen = sizeof(caddr);
 142
 143         struct sigaction signalaction;
 144         sigset_t sigset;
 145
 146         fd_set readfds;
 147
 148         int i;
 149         int nbytes;
 150
 151         int local_port = 0;
 152         int verbose = 0;
 153
 154         int idx = 0; /* index in txmsg[] */
 155
 156         unsigned char msg[MAX_PDU_LENGTH + 1];   /* isotp socket message buffer (4095 + test_for_too_long_byte)*/
 157         char rxmsg[MAX_PDU_LENGTH * 2 + 4]; /* isotp->tcp ASCII message buffer (4095*2 + < > \n null) */
 158         char txmsg[MAX_PDU_LENGTH * 2 + 3]; /* tcp->isotp ASCII message buffer (4095*2 + < > null) */
 159
 160         /* mark missing mandatory commandline options as missing */
 161         caddr.can_addr.tp.tx_id = caddr.can_addr.tp.rx_id = NO_CAN_ID;
 162
 163         while ((opt = getopt(argc, argv, "l:s:d:x:p:P:b:m:w:t:L:v?")) != -1) {
 164                 switch (opt) {
 165                 case 'l':
 166                         local_port = strtoul(optarg, (char **)NULL, 10);
 167                         break;
 168
 169                 case 's':
 170                         caddr.can_addr.tp.tx_id = strtoul(optarg, (char **)NULL, 16);
 171                         if (strlen(optarg) > 7)
 172                                 caddr.can_addr.tp.tx_id |= CAN_EFF_FLAG;
 173                         break;
 174
 175                 case 'd':
 176                         caddr.can_addr.tp.rx_id = strtoul(optarg, (char **)NULL, 16);
 177                         if (strlen(optarg) > 7)
 178                                 caddr.can_addr.tp.rx_id |= CAN_EFF_FLAG;
 179                         break;
 180
 181                 case 'x':
 182                 {
 183                         int elements = sscanf(optarg, "%hhx:%hhx",
 184                                               &opts.ext_address,
 185                                               &opts.rx_ext_address);
 186
 187                         if (elements == 1)
 188                                 opts.flags |= CAN_ISOTP_EXTEND_ADDR;
 189                         else if (elements == 2)
 190                                 opts.flags |= (CAN_ISOTP_EXTEND_ADDR | CAN_ISOTP_RX_EXT_ADDR);
 191                         else {
 192                                 printf("incorrect extended addr values '%s'.\n", optarg);
 193                                 print_usage(basename(argv[0]));
 194                                 exit(0);
 195                         }
 196                         break;
 197                 }
 198
 199                 case 'p':
 200                 {
 201                         int elements = sscanf(optarg, "%hhx:%hhx",
 202                                               &opts.txpad_content,
 203                                               &opts.rxpad_content);
 204
 205                         if (elements == 1)
 206                                 opts.flags |= CAN_ISOTP_TX_PADDING;
 207                         else if (elements == 2)
 208                                 opts.flags |= (CAN_ISOTP_TX_PADDING | CAN_ISOTP_RX_PADDING);
 209                         else if (sscanf(optarg, ":%hhx", &opts.rxpad_content) == 1)
 210                                 opts.flags |= CAN_ISOTP_RX_PADDING;
 211                         else {
 212                                 printf("incorrect padding values '%s'.\n", optarg);
 213                                 print_usage(basename(argv[0]));
 214                                 exit(0);
 215                         }
 216                         break;
 217                 }
 218
 219                 case 'P':
 220                         if (optarg[0] == 'l')
 221                                 opts.flags |= CAN_ISOTP_CHK_PAD_LEN;
 222                         else if (optarg[0] == 'c')
 223                                 opts.flags |= CAN_ISOTP_CHK_PAD_DATA;
 224                         else if (optarg[0] == 'a')
 225                                 opts.flags |= (CAN_ISOTP_CHK_PAD_LEN | CAN_ISOTP_CHK_PAD_DATA);
 226                         else {
 227                                 printf("unknown padding check option '%c'.\n", optarg[0]);
 228                                 print_usage(basename(argv[0]));
 229                                 exit(0);
 230                         }
 231                         break;
 232
 233                 case 'b':
 234                         fcopts.bs = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 235                         break;
 236
 237                 case 'm':
 238                         fcopts.stmin = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 239                         break;
 240
 241                 case 'w':
 242                         fcopts.wftmax = strtoul(optarg, (char **)NULL, 16) & 0xFF;
 243                         break;
 244
 245                 case 't':
 246                         opts.frame_txtime = strtoul(optarg, (char **)NULL, 10);
 247                         break;
 248
 249                 case 'L':
 250                         if (sscanf(optarg, "%hhu:%hhu:%hhu",
 251                                    &llopts.mtu,
 252                                    &llopts.tx_dl,
 253                                    &llopts.tx_flags) != 3) {
 254                                 printf("unknown link layer options '%s'.\n", optarg);
 255                                 print_usage(basename(argv[0]));
 256                                 exit(0);
 257                         }
 258                         break;
 259
 260                 case 'v':
 261                         verbose = 1;
 262                         break;
 263
 264                 case '?':
 265                         print_usage(basename(argv[0]));
 266                         exit(0);
 267                         break;
 268
 269                 default:
 270                         fprintf(stderr, "Unknown option %c\n", opt);
 271                         print_usage(basename(argv[0]));
 272                         exit(1);
 273                         break;
 274                 }
 275         }
 276
 277         if ((argc - optind != 1) || (local_port == 0) ||
 278             (caddr.can_addr.tp.tx_id == NO_CAN_ID) ||
 279             (caddr.can_addr.tp.rx_id == NO_CAN_ID)) {
 280                 print_usage(basename(argv[0]));
 281                 exit(1);
 282         }
 283
 284         sigemptyset(&sigset);
 285         signalaction.sa_handler = &childdied;
 286         signalaction.sa_mask = sigset;
 287         signalaction.sa_flags = 0;
 288         sigaction(SIGCHLD, &signalaction, NULL);  /* signal for dying child */
 289
 290         if((sl = socket(PF_INET, SOCK_STREAM, 0)) < 0) {
 291                 perror("inetsocket");
 292                 exit(1);
 293         }
 294
 295         saddr.sin_family = AF_INET;
 296         saddr.sin_addr.s_addr = htonl(INADDR_ANY);
 297         saddr.sin_port = htons(local_port);
 298
 299         while(bind(sl,(struct sockaddr*)&saddr, sizeof(saddr)) < 0) {
 300                 printf(".");
 301                 fflush(NULL);
 302                 usleep(100000);
 303         }
 304
 305         if (listen(sl, 3) != 0) {
 306                 perror("listen");
 307                 exit(1);
 308         }
 309
 310         while (1) {
 311                 sa = accept(sl,(struct sockaddr *)&clientaddr, &sin_size);
 312                 if (sa > 0 ){
 313
 314                         if (fork())
 315                                 close(sa);
 316                         else
 317                                 break;
 318                 }
 319                 else {
 320                         if (errno != EINTR) {
 321                                 /*
 322                                  * If the cause for the error was NOT the
 323                                  * signal from a dying child => give an error
 324                                  */
 325                                 perror("accept");
 326                                 exit(1);
 327                         }
 328                 }
 329         }
 330
 331         if ((sc = socket(PF_CAN, SOCK_DGRAM, CAN_ISOTP)) < 0) {
 332                 perror("socket");
 333                 exit(1);
 334         }
 335
 336         setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
 337         setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts));
 338
 339         if (llopts.tx_dl) {
 340                 if (setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_LL_OPTS, &llopts, sizeof(llopts)) < 0) {
 341                         perror("link layer sockopt");
 342                         exit(1);
 343                 }
 344         }
 345
 346         caddr.can_family = AF_CAN;
 347         caddr.can_ifindex = if_nametoindex(argv[optind]);
 348
 349         if (bind(sc, (struct sockaddr *)&caddr, caddrlen) < 0) {
 350                 perror("bind");
 351                 exit(1);
 352         }
 353
 354         while (1) {
 355
 356                 FD_ZERO(&readfds);
 357                 FD_SET(sc, &readfds);
 358                 FD_SET(sa, &readfds);
 359
 360                 select((sc > sa)?sc+1:sa+1, &readfds, NULL, NULL, NULL);
 361
 362                 if (FD_ISSET(sc, &readfds)) {
 363
 364
 365                         nbytes = read(sc, &msg, MAX_PDU_LENGTH + 1);
 366
 367                         if (nbytes < 1 || nbytes > MAX_PDU_LENGTH) {
 368                                 perror("read from isotp socket");
 369                                 exit(1);
 370                         }
 371
 372                         rxmsg[0] = '<';
 373
 374                         for ( i = 0; i < nbytes; i++)
 375                                 sprintf(rxmsg + 1 + 2*i, "%02X", msg[i]);
 376
 377                         /* finalize string for sending */
 378                         strcat(rxmsg, ">\n");
 379
 380                         if (verbose)
 381                                 printf("CAN>TCP %s", rxmsg);
 382
 383                         send(sa, rxmsg, strlen(rxmsg), 0);
 384                 }
 385
 386
 387                 if (FD_ISSET(sa, &readfds)) {
 388
 389                         if (read(sa, txmsg+idx, 1) < 1) {
 390                                 perror("read from tcp/ip socket");
 391                                 exit(1);
 392                         }
 393
 394                         if (!idx) {
 395                                 if (txmsg[0] == '<')
 396                                         idx = 1;
 397
 398                                 continue;
 399                         }
 400
 401                         /* max len is 4095*2 + '<' + '>' = 8192. The buffer index starts with 0 */
 402                         if (idx > MAX_PDU_LENGTH * 2 + 1) {
 403                                 idx = 0;
 404                                 continue;
 405                         }
 406
 407                         if (txmsg[idx] != '>') {
 408                                 idx++;
 409                                 continue;
 410                         }
 411
 412                         txmsg[idx+1] = 0;
 413                         idx = 0;
 414
 415                         /* must be an even number of bytes and at least one data byte <XX> */
 416                         if (strlen(txmsg) < 4 || strlen(txmsg) % 2)
 417                                 continue;
 418
 419                         if (verbose)
 420                                 printf("TCP>CAN %s\n", txmsg);
 421
 422                         nbytes = (strlen(txmsg)-2)/2;
 423                         if (b64hex(txmsg+1, msg, nbytes) == 0)
 424                                 send(sc, msg, nbytes, 0);
 425                 }
 426         }
 427
 428         close(sc);
 429         close(sa);
 430
 431         return 0;
 432 }