slcanpty.c

   1 /*
   2  * slcanpty.c -  creates a pty for applications using the slcan ASCII protocol
   3  * and converts the ASCII data to a CAN network interface (and vice versa)
   4  *
   5  * Copyright (c)2009 Oliver Hartkopp
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  20  *
  21  * Send feedback to <linux-can@vger.kernel.org>
  22  *
  23  */
  24
  25 /* To get ptsname grantpt and unlockpt definitions from stdlib.h */
  26 #define _GNU_SOURCE
  27
  28 #include <stdio.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31 #include <unistd.h>
  32 #include <fcntl.h>
  33 #include <termios.h>
  34
  35 #include <net/if.h>
  36 #include <sys/socket.h>
  37 #include <sys/ioctl.h>
  38 #include <sys/stat.h>
  39 #include <sys/types.h>
  40
  41 #include <linux/can.h>
  42 #include <linux/can/raw.h>
  43
  44 /* maximum rx buffer len: extended CAN frame with timestamp */
  45 #define SLC_MTU (sizeof("T1111222281122334455667788EA5F\r")+1)
  46 #define DEVICE_NAME_PTMX "/dev/ptmx"
  47
  48 #define DEBUG
  49
  50 static int asc2nibble(char c)
  51 {
  52
  53         if ((c >= '0') && (c <= '9'))
  54                 return c - '0';
  55
  56         if ((c >= 'A') && (c <= 'F'))
  57                 return c - 'A' + 10;
  58
  59         if ((c >= 'a') && (c <= 'f'))
  60                 return c - 'a' + 10;
  61
  62         return 16; /* error */
  63 }
  64
  65 /* read data from pty, send CAN frames to CAN socket and answer commands */
  66 int pty2can(int pty, int socket, struct can_filter *fi,
  67             int *is_open, int *tstamp)
  68 {
  69         int nbytes;
  70         char cmd;
  71         char buf[200];
  72         char replybuf[10]; /* for answers to received commands */
  73         int ptr;
  74         struct can_frame frame;
  75         int tmp, i;
  76         static int rxoffset = 0; /* points to the end of an received incomplete SLCAN message */
  77
  78         nbytes = read(pty, &buf[rxoffset], sizeof(buf)-rxoffset-1);
  79         if (nbytes <= 0) {
  80                 /* nbytes == 0 : no error but pty decriptor has been closed */
  81                 if (nbytes < 0)
  82                         perror("read pty");
  83
  84                 return 1;
  85         }
  86
  87         /* reset incomplete message offset */
  88         nbytes += rxoffset;
  89         rxoffset = 0;
  90
  91 rx_restart:
  92         /* remove trailing '\r' characters to be robust against some apps */
  93         while (buf[0] == '\r' && nbytes > 0) {
  94                 for (tmp = 0; tmp < nbytes; tmp++)
  95                         buf[tmp] = buf[tmp+1];
  96                 nbytes--;
  97         }
  98
  99         if (!nbytes)
 100                 return 0;
 101
 102         /* check if we can detect a complete SLCAN message including '\r' */
 103         for (tmp = 0; tmp < nbytes; tmp++) {
 104                 if (buf[tmp] == '\r')
 105                         break;
 106         }
 107
 108         /* no '\r' found in the message buffer? */
 109         if (tmp == nbytes) {
 110                 /* save incomplete message */
 111                 rxoffset = nbytes;
 112
 113                 /* leave here and read from pty again */
 114                 return 0;
 115         }
 116
 117         cmd = buf[0];
 118         buf[nbytes] = 0;
 119
 120 #ifdef DEBUG
 121         for (tmp = 0; tmp < nbytes; tmp++)
 122                 if (buf[tmp] == '\r')
 123                         putchar('@');
 124                 else
 125                         putchar(buf[tmp]);
 126         printf("\n");
 127 #endif
 128
 129         /* check for filter configuration commands */
 130         if (cmd == 'm' || cmd == 'M') {
 131                 buf[9] = 0; /* terminate filter string */
 132                 ptr = 9;
 133 #if 0
 134                 /* the filter is no SocketCAN filter :-( */
 135
 136                 /* TODO: behave like a SJA1000 controller specific filter */
 137
 138                 if (cmd == 'm') {
 139                         fi->can_id = strtoul(buf+1,NULL,16);
 140                         fi->can_id &= CAN_EFF_MASK;
 141                 } else {
 142                         fi->can_mask = strtoul(buf+1,NULL,16);
 143                         fi->can_mask &= CAN_EFF_MASK;
 144                 }
 145
 146                 if (*is_open)
 147                         setsockopt(socket, SOL_CAN_RAW,
 148                                    CAN_RAW_FILTER, fi,
 149                                    sizeof(struct can_filter));
 150 #endif
 151                 goto rx_out_ack;
 152         }
 153
 154
 155         /* check for timestamp on/off command */
 156         if (cmd == 'Z') {
 157                 *tstamp = buf[1] & 0x01;
 158                 ptr = 2;
 159                 goto rx_out_ack;
 160         }
 161
 162         /* check for 'O'pen command */
 163         if (cmd == 'O') {
 164                 setsockopt(socket, SOL_CAN_RAW,
 165                            CAN_RAW_FILTER, fi,
 166                            sizeof(struct can_filter));
 167                 ptr = 1;
 168                 *is_open = 1;
 169                 goto rx_out_ack;
 170         }
 171
 172         /* check for 'C'lose command */
 173         if (cmd == 'C') {
 174                 setsockopt(socket, SOL_CAN_RAW, CAN_RAW_FILTER,
 175                            NULL, 0);
 176                 ptr = 1;
 177                 *is_open = 0;
 178                 goto rx_out_ack;
 179         }
 180
 181         /* check for 'V'ersion command */
 182         if (cmd == 'V') {
 183                 sprintf(replybuf, "V1013\r");
 184                 tmp = strlen(replybuf);
 185                 ptr = 1;
 186                 goto rx_out;
 187         }
 188
 189         /* check for serial 'N'umber command */
 190         if (cmd == 'N') {
 191                 sprintf(replybuf, "N4242\r");
 192                 tmp = strlen(replybuf);
 193                 ptr = 1;
 194                 goto rx_out;
 195         }
 196
 197         /* check for read status 'F'lags */
 198         if (cmd == 'F') {
 199                 sprintf(replybuf, "F00\r");
 200                 tmp = strlen(replybuf);
 201                 ptr = 1;
 202                 goto rx_out;
 203         }
 204
 205         /* correctly answer unsupported commands */
 206         if (cmd == 'U') {
 207                 ptr = 2;
 208                 goto rx_out_ack;
 209         }
 210         if (cmd == 'S') {
 211                 ptr = 2;
 212                 goto rx_out_ack;
 213         }
 214         if (cmd == 's') {
 215                 ptr = 5;
 216                 goto rx_out_ack;
 217         }
 218         if (cmd == 'P' || cmd == 'A') {
 219                 ptr = 1;
 220                 goto rx_out_nack;
 221         }
 222         if (cmd == 'X') {
 223                 ptr = 2;
 224                 if (buf[1] & 0x01)
 225                         goto rx_out_ack;
 226                 else
 227                         goto rx_out_nack;
 228         }
 229
 230         /* catch unknown commands */
 231         if ((cmd != 't') && (cmd != 'T') &&
 232             (cmd != 'r') && (cmd != 'R')) {
 233                 ptr = nbytes-1;
 234                 goto rx_out_nack;
 235         }
 236
 237         if (cmd & 0x20) /* tiny chars 'r' 't' => SFF */
 238                 ptr = 4; /* dlc position tiiid */
 239         else
 240                 ptr = 9; /* dlc position Tiiiiiiiid */
 241
 242         *(unsigned long long *) (&frame.data) = 0ULL; /* clear data[] */
 243
 244         if ((cmd | 0x20) == 'r' && buf[ptr] != '0') {
 245
 246                 /*
 247                  * RTR frame without dlc information!
 248                  * This is against the SLCAN spec but sent
 249                  * by a commercial CAN tool ... so we are
 250                  * robust against this protocol violation.
 251                  */
 252
 253                 frame.can_dlc = buf[ptr]; /* save following byte */
 254
 255                 buf[ptr] = 0; /* terminate can_id string */
 256
 257                 frame.can_id = strtoul(buf+1, NULL, 16);
 258                 frame.can_id |= CAN_RTR_FLAG;
 259
 260                 if (!(cmd & 0x20)) /* NO tiny chars => EFF */
 261                         frame.can_id |= CAN_EFF_FLAG;
 262
 263                 buf[ptr]  = frame.can_dlc; /* restore following byte */
 264                 frame.can_dlc = 0;
 265                 ptr--; /* we have no dlc component in the violation case */
 266
 267         } else {
 268
 269                 if (!(buf[ptr] >= '0' && buf[ptr] < '9'))
 270                         goto rx_out_nack;
 271
 272                 frame.can_dlc = buf[ptr] - '0'; /* get dlc from ASCII val */
 273
 274                 buf[ptr] = 0; /* terminate can_id string */
 275
 276                 frame.can_id = strtoul(buf+1, NULL, 16);
 277
 278                 if (!(cmd & 0x20)) /* NO tiny chars => EFF */
 279                         frame.can_id |= CAN_EFF_FLAG;
 280
 281                 if ((cmd | 0x20) == 'r') /* RTR frame */
 282                         frame.can_id |= CAN_RTR_FLAG;
 283
 284                 for (i = 0, ptr++; i < frame.can_dlc; i++) {
 285
 286                         tmp = asc2nibble(buf[ptr++]);
 287                         if (tmp > 0x0F)
 288                                 goto rx_out_nack;
 289                         frame.data[i] = (tmp << 4);
 290                         tmp = asc2nibble(buf[ptr++]);
 291                         if (tmp > 0x0F)
 292                                 goto rx_out_nack;
 293                         frame.data[i] |= tmp;
 294                 }
 295                 /* point to last real data */
 296                 if (frame.can_dlc)
 297                         ptr--;
 298         }
 299
 300         tmp = write(socket, &frame, sizeof(frame));
 301         if (tmp != sizeof(frame)) {
 302                 perror("write socket");
 303                 return 1;
 304         }
 305
 306 rx_out_ack:
 307         replybuf[0] = '\r';
 308         tmp = 1;
 309         goto rx_out;
 310 rx_out_nack:
 311         replybuf[0] = '\a';
 312         tmp = 1;
 313 rx_out:
 314         tmp = write(pty, replybuf, tmp);
 315         if (tmp < 0) {
 316                 perror("write pty replybuf");
 317                 return 1;
 318         }
 319
 320         /* check if there is another command in this buffer */
 321         if (nbytes > ptr+1) {
 322                 for (tmp = 0, ptr++; ptr+tmp < nbytes; tmp++)
 323                         buf[tmp] = buf[ptr+tmp];
 324                 nbytes = tmp;
 325                 goto rx_restart;
 326         }
 327
 328         return 0;
 329 }
 330
 331 /* read CAN frames from CAN interface and write it to the pty */
 332 int can2pty(int pty, int socket, int *tstamp)
 333 {
 334         int nbytes;
 335         char cmd;
 336         char buf[SLC_MTU];
 337         int ptr;
 338         struct can_frame frame;
 339         int i;
 340
 341         nbytes = read(socket, &frame, sizeof(frame));
 342         if (nbytes != sizeof(frame)) {
 343                 perror("read socket");
 344                 return 1;
 345         }
 346
 347         /* convert to slcan ASCII frame */
 348         if (frame.can_id & CAN_RTR_FLAG)
 349                 cmd = 'R'; /* becomes 'r' in SFF format */
 350         else
 351                 cmd = 'T'; /* becomes 't' in SFF format */
 352
 353         if (frame.can_id & CAN_EFF_FLAG)
 354                 sprintf(buf, "%c%08X%d", cmd,
 355                         frame.can_id & CAN_EFF_MASK,
 356                         frame.can_dlc);
 357         else
 358                 sprintf(buf, "%c%03X%d", cmd | 0x20,
 359                         frame.can_id & CAN_SFF_MASK,
 360                         frame.can_dlc);
 361
 362         ptr = strlen(buf);
 363
 364         for (i = 0; i < frame.can_dlc; i++)
 365                 sprintf(&buf[ptr + 2*i], "%02X",
 366                         frame.data[i]);
 367
 368         if (*tstamp) {
 369                 struct timeval tv;
 370
 371                 if (ioctl(socket, SIOCGSTAMP, &tv) < 0)
 372                         perror("SIOCGSTAMP");
 373
 374                 sprintf(&buf[ptr + 2*frame.can_dlc], "%04lX",
 375                         (tv.tv_sec%60)*1000 + tv.tv_usec/1000);
 376         }
 377
 378         strcat(buf, "\r"); /* add terminating character */
 379         nbytes = write(pty, buf, strlen(buf));
 380         if (nbytes < 0) {
 381                 perror("write pty");
 382                 return 1;
 383         }
 384         fflush(NULL);
 385
 386         return 0;
 387 }
 388
 389 int check_select_stdin(void)
 390 {
 391         fd_set rdfs;
 392         struct timeval timeout;
 393         int ret;
 394
 395         FD_ZERO(&rdfs);
 396         FD_SET(0, &rdfs);
 397         timeout.tv_sec = 0;
 398         timeout.tv_usec = 0;
 399
 400         ret = select(1, &rdfs, NULL, NULL, &timeout);
 401
 402         if (ret < 0)
 403                 return 0; /* not selectable */
 404
 405         if (ret > 0 && getchar() == EOF)
 406                 return 0; /* EOF, eg. /dev/null */
 407
 408         return 1;
 409 }
 410
 411 int main(int argc, char **argv)
 412 {
 413         fd_set rdfs;
 414         int p; /* pty master file */
 415         int s; /* can raw socket */
 416         struct sockaddr_can addr;
 417         struct termios topts;
 418         struct ifreq ifr;
 419         int select_stdin = 0;
 420         int running = 1;
 421         int tstamp = 0;
 422         int is_open = 0;
 423         struct can_filter fi;
 424
 425         /* check command line options */
 426         if (argc != 3) {
 427                 fprintf(stderr, "\n");
 428                 fprintf(stderr, "%s creates a pty for applications using"
 429                         " the slcan ASCII protocol and\n", argv[0]);
 430                 fprintf(stderr, "converts the ASCII data to a CAN network"
 431                         " interface (and vice versa)\n\n");
 432                 fprintf(stderr, "Usage: %s <pty> <can interface>\n", argv[0]);
 433                 fprintf(stderr, "e.g. '%s /dev/ptyc0 can0' creates"
 434                         " /dev/ttyc0 for the slcan application\n", argv[0]);
 435                 fprintf(stderr, "e.g. for pseudo-terminal '%s %s can0' creates"
 436                         " /dev/pts/N\n", argv[0], DEVICE_NAME_PTMX);
 437                 fprintf(stderr, "\n");
 438                 return 1;
 439         }
 440
 441         select_stdin = check_select_stdin();
 442
 443         /* open pty */
 444         p = open(argv[1], O_RDWR);
 445         if (p < 0) {
 446                 perror("open pty");
 447                 return 1;
 448         }
 449
 450         if (tcgetattr(p, &topts)) {
 451                 perror("tcgetattr");
 452                 return 1;
 453         }
 454
 455         /* disable local echo which would cause double frames */
 456         topts.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK |
 457                            ECHONL | ECHOPRT | ECHOKE);
 458         topts.c_iflag &= ~(ICRNL);
 459         topts.c_iflag |= INLCR;
 460         tcsetattr(p, TCSANOW, &topts);
 461
 462         /* Support for the Unix 98 pseudo-terminal interface /dev/ptmx /dev/pts/N */
 463         if  (strcmp(argv[1], DEVICE_NAME_PTMX) == 0) {
 464
 465                 char *name_pts = NULL;  /* slave pseudo-terminal device name */
 466
 467                 if (grantpt(p) < 0) {
 468                         perror("grantpt");
 469                         return 1;
 470                 }
 471
 472                 if (unlockpt(p) < 0) {
 473                         perror("unlockpt");
 474                         return 1;
 475                 }
 476
 477                 name_pts = ptsname(p);
 478                 if (name_pts == NULL) {
 479                         perror("ptsname");
 480                         return 1;
 481                 }
 482                 printf("open: %s: slave pseudo-terminal is %s\n", argv[1], name_pts);
 483         }
 484
 485         /* open socket */
 486         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 487         if (s < 0) {
 488                 perror("socket");
 489                 return 1;
 490         }
 491
 492         addr.can_family = AF_CAN;
 493
 494         strcpy(ifr.ifr_name, argv[2]);
 495         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 496                 perror("SIOCGIFINDEX");
 497                 return 1;
 498         }
 499         addr.can_ifindex = ifr.ifr_ifindex;
 500
 501         /* disable reception of CAN frames until we are opened by 'O' */
 502         setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
 503
 504         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 505                 perror("bind");
 506                 return 1;
 507         }
 508
 509         /* open filter by default */
 510         fi.can_id   = 0;
 511         fi.can_mask = 0;
 512
 513         while (running) {
 514
 515                 FD_ZERO(&rdfs);
 516
 517                 if (select_stdin)
 518                         FD_SET(0, &rdfs);
 519
 520                 FD_SET(p, &rdfs);
 521                 FD_SET(s, &rdfs);
 522
 523                 if (select(s+1, &rdfs, NULL, NULL, NULL) < 0) {
 524                         perror("select");
 525                         return 1;
 526                 }
 527
 528                 if (FD_ISSET(0, &rdfs)) {
 529                         running = 0;
 530                         continue;
 531                 }
 532
 533                 if (FD_ISSET(p, &rdfs))
 534                         if (pty2can(p, s, &fi, &is_open, &tstamp)) {
 535                         running = 0;
 536                         continue;
 537                 }
 538
 539                 if (FD_ISSET(s, &rdfs))
 540                         if (can2pty(p, s, &tstamp)) {
 541                         running = 0;
 542                         continue;
 543                 }
 544         }
 545
 546         close(p);
 547         close(s);
 548
 549         return 0;
 550 }