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
  77         nbytes = read(pty, &buf, sizeof(buf)-1);
  78         if (nbytes <= 0) {
  79                 /* nbytes == 0 : no error but pty decriptor has been closed */
  80                 if (nbytes < 0)
  81                         perror("read pty");
  82
  83                 return 1;
  84         }
  85
  86 rx_restart:
  87         /* remove trailing '\r' characters to be robust against some apps */
  88         while (buf[0] == '\r' && nbytes > 0) {
  89                 for (tmp = 0; tmp < nbytes; tmp++)
  90                         buf[tmp] = buf[tmp+1];
  91                 nbytes--;
  92         }
  93
  94         if (!nbytes)
  95                 return 0;
  96
  97         cmd = buf[0];
  98         buf[nbytes] = 0;
  99
 100 #ifdef DEBUG
 101         for (tmp = 0; tmp < nbytes; tmp++)
 102                 if (buf[tmp] == '\r')
 103                         putchar('@');
 104                 else
 105                         putchar(buf[tmp]);
 106         printf("\n");
 107 #endif
 108
 109         /* check for filter configuration commands */
 110         if (cmd == 'm' || cmd == 'M') {
 111                 buf[9] = 0; /* terminate filter string */
 112                 ptr = 9;
 113 #if 0
 114                 /* the filter is no SocketCAN filter :-( */
 115
 116                 /* TODO: behave like a SJA1000 controller specific filter */
 117
 118                 if (cmd == 'm') {
 119                         fi->can_id = strtoul(buf+1,NULL,16);
 120                         fi->can_id &= CAN_EFF_MASK;
 121                 } else {
 122                         fi->can_mask = strtoul(buf+1,NULL,16);
 123                         fi->can_mask &= CAN_EFF_MASK;
 124                 }
 125
 126                 if (*is_open)
 127                         setsockopt(socket, SOL_CAN_RAW,
 128                                    CAN_RAW_FILTER, fi,
 129                                    sizeof(struct can_filter));
 130 #endif
 131                 goto rx_out_ack;
 132         }
 133
 134
 135         /* check for timestamp on/off command */
 136         if (cmd == 'Z') {
 137                 *tstamp = buf[1] & 0x01;
 138                 ptr = 2;
 139                 goto rx_out_ack;
 140         }
 141
 142         /* check for 'O'pen command */
 143         if (cmd == 'O') {
 144                 setsockopt(socket, SOL_CAN_RAW,
 145                            CAN_RAW_FILTER, fi,
 146                            sizeof(struct can_filter));
 147                 ptr = 1;
 148                 *is_open = 1;
 149                 goto rx_out_ack;
 150         }
 151
 152         /* check for 'C'lose command */
 153         if (cmd == 'C') {
 154                 setsockopt(socket, SOL_CAN_RAW, CAN_RAW_FILTER,
 155                            NULL, 0);
 156                 ptr = 1;
 157                 *is_open = 0;
 158                 goto rx_out_ack;
 159         }
 160
 161         /* check for 'V'ersion command */
 162         if (cmd == 'V') {
 163                 sprintf(replybuf, "V1013\r");
 164                 tmp = strlen(replybuf);
 165                 ptr = 1;
 166                 goto rx_out;
 167         }
 168
 169         /* check for serial 'N'umber command */
 170         if (cmd == 'N') {
 171                 sprintf(replybuf, "N4242\r");
 172                 tmp = strlen(replybuf);
 173                 ptr = 1;
 174                 goto rx_out;
 175         }
 176
 177         /* check for read status 'F'lags */
 178         if (cmd == 'F') {
 179                 sprintf(replybuf, "F00\r");
 180                 tmp = strlen(replybuf);
 181                 ptr = 1;
 182                 goto rx_out;
 183         }
 184
 185         /* correctly answer unsupported commands */
 186         if (cmd == 'U') {
 187                 ptr = 2;
 188                 goto rx_out_ack;
 189         }
 190         if (cmd == 'S') {
 191                 ptr = 2;
 192                 goto rx_out_ack;
 193         }
 194         if (cmd == 's') {
 195                 ptr = 5;
 196                 goto rx_out_ack;
 197         }
 198         if (cmd == 'P' || cmd == 'A') {
 199                 ptr = 1;
 200                 goto rx_out_nack;
 201         }
 202         if (cmd == 'X') {
 203                 ptr = 2;
 204                 if (buf[1] & 0x01)
 205                         goto rx_out_ack;
 206                 else
 207                         goto rx_out_nack;
 208         }
 209
 210         /* catch unknown commands */
 211         if ((cmd != 't') && (cmd != 'T') &&
 212             (cmd != 'r') && (cmd != 'R')) {
 213                 ptr = nbytes-1;
 214                 goto rx_out_nack;
 215         }
 216
 217         if (cmd & 0x20) /* tiny chars 'r' 't' => SFF */
 218                 ptr = 4; /* dlc position tiiid */
 219         else
 220                 ptr = 9; /* dlc position Tiiiiiiiid */
 221
 222         *(unsigned long long *) (&frame.data) = 0ULL; /* clear data[] */
 223
 224         if ((cmd | 0x20) == 'r' && buf[ptr] != '0') {
 225
 226                 /*
 227                  * RTR frame without dlc information!
 228                  * This is against the SLCAN spec but sent
 229                  * by a commercial CAN tool ... so we are
 230                  * robust against this protocol violation.
 231                  */
 232
 233                 frame.can_dlc = buf[ptr]; /* save following byte */
 234
 235                 buf[ptr] = 0; /* terminate can_id string */
 236
 237                 frame.can_id = strtoul(buf+1, NULL, 16);
 238                 frame.can_id |= CAN_RTR_FLAG;
 239
 240                 if (!(cmd & 0x20)) /* NO tiny chars => EFF */
 241                         frame.can_id |= CAN_EFF_FLAG;
 242
 243                 buf[ptr]  = frame.can_dlc; /* restore following byte */
 244                 frame.can_dlc = 0;
 245                 ptr--; /* we have no dlc component in the violation case */
 246
 247         } else {
 248
 249                 if (!(buf[ptr] >= '0' && buf[ptr] < '9'))
 250                         goto rx_out_nack;
 251
 252                 frame.can_dlc = buf[ptr] - '0'; /* get dlc from ASCII val */
 253
 254                 buf[ptr] = 0; /* terminate can_id string */
 255
 256                 frame.can_id = strtoul(buf+1, NULL, 16);
 257
 258                 if (!(cmd & 0x20)) /* NO tiny chars => EFF */
 259                         frame.can_id |= CAN_EFF_FLAG;
 260
 261                 if ((cmd | 0x20) == 'r') /* RTR frame */
 262                         frame.can_id |= CAN_RTR_FLAG;
 263
 264                 for (i = 0, ptr++; i < frame.can_dlc; i++) {
 265
 266                         tmp = asc2nibble(buf[ptr++]);
 267                         if (tmp > 0x0F)
 268                                 goto rx_out_nack;
 269                         frame.data[i] = (tmp << 4);
 270                         tmp = asc2nibble(buf[ptr++]);
 271                         if (tmp > 0x0F)
 272                                 goto rx_out_nack;
 273                         frame.data[i] |= tmp;
 274                 }
 275                 /* point to last real data */
 276                 if (frame.can_dlc)
 277                         ptr--;
 278         }
 279
 280         tmp = write(socket, &frame, sizeof(frame));
 281         if (tmp != sizeof(frame)) {
 282                 perror("write socket");
 283                 return 1;
 284         }
 285
 286 rx_out_ack:
 287         replybuf[0] = '\r';
 288         tmp = 1;
 289         goto rx_out;
 290 rx_out_nack:
 291         replybuf[0] = '\a';
 292         tmp = 1;
 293 rx_out:
 294         tmp = write(pty, replybuf, tmp);
 295         if (tmp < 0) {
 296                 perror("write pty replybuf");
 297                 return 1;
 298         }
 299
 300         /* check if there is another command in this buffer */
 301         if (nbytes > ptr+1) {
 302                 for (tmp = 0, ptr++; ptr+tmp < nbytes; tmp++)
 303                         buf[tmp] = buf[ptr+tmp];
 304                 nbytes = tmp;
 305                 goto rx_restart;
 306         }
 307
 308         return 0;
 309 }
 310
 311 /* read CAN frames from CAN interface and write it to the pty */
 312 int can2pty(int pty, int socket, int *tstamp)
 313 {
 314         int nbytes;
 315         char cmd;
 316         char buf[SLC_MTU];
 317         int ptr;
 318         struct can_frame frame;
 319         int i;
 320
 321         nbytes = read(socket, &frame, sizeof(frame));
 322         if (nbytes != sizeof(frame)) {
 323                 perror("read socket");
 324                 return 1;
 325         }
 326
 327         /* convert to slcan ASCII frame */
 328         if (frame.can_id & CAN_RTR_FLAG)
 329                 cmd = 'R'; /* becomes 'r' in SFF format */
 330         else
 331                 cmd = 'T'; /* becomes 't' in SFF format */
 332
 333         if (frame.can_id & CAN_EFF_FLAG)
 334                 sprintf(buf, "%c%08X%d", cmd,
 335                         frame.can_id & CAN_EFF_MASK,
 336                         frame.can_dlc);
 337         else
 338                 sprintf(buf, "%c%03X%d", cmd | 0x20,
 339                         frame.can_id & CAN_SFF_MASK,
 340                         frame.can_dlc);
 341
 342         ptr = strlen(buf);
 343
 344         for (i = 0; i < frame.can_dlc; i++)
 345                 sprintf(&buf[ptr + 2*i], "%02X",
 346                         frame.data[i]);
 347
 348         if (*tstamp) {
 349                 struct timeval tv;
 350
 351                 if (ioctl(socket, SIOCGSTAMP, &tv) < 0)
 352                         perror("SIOCGSTAMP");
 353
 354                 sprintf(&buf[ptr + 2*frame.can_dlc], "%04lX",
 355                         (tv.tv_sec%60)*1000 + tv.tv_usec/1000);
 356         }
 357
 358         strcat(buf, "\r"); /* add terminating character */
 359         nbytes = write(pty, buf, strlen(buf));
 360         if (nbytes < 0) {
 361                 perror("write pty");
 362                 return 1;
 363         }
 364         fflush(NULL);
 365
 366         return 0;
 367 }
 368
 369
 370 int main(int argc, char **argv)
 371 {
 372         fd_set rdfs;
 373         int p; /* pty master file */
 374         int s; /* can raw socket */
 375         struct sockaddr_can addr;
 376         struct termios topts;
 377         struct ifreq ifr;
 378         int running = 1;
 379         int tstamp = 0;
 380         int is_open = 0;
 381         struct can_filter fi;
 382
 383         /* check command line options */
 384         if (argc != 3) {
 385                 fprintf(stderr, "\n");
 386                 fprintf(stderr, "%s creates a pty for applications using"
 387                         " the slcan ASCII protocol and\n", argv[0]);
 388                 fprintf(stderr, "converts the ASCII data to a CAN network"
 389                         " interface (and vice versa)\n\n");
 390                 fprintf(stderr, "Usage: %s <pty> <can interface>\n", argv[0]);
 391                 fprintf(stderr, "e.g. '%s /dev/ptyc0 can0' creates"
 392                         " /dev/ttyc0 for the slcan application\n", argv[0]);
 393                 fprintf(stderr, "e.g. for pseudo-terminal '%s %s can0' creates"
 394                         " /dev/pts/N\n", argv[0], DEVICE_NAME_PTMX);
 395                 fprintf(stderr, "\n");
 396                 return 1;
 397         }
 398
 399         /* open pty */
 400         p = open(argv[1], O_RDWR);
 401         if (p < 0) {
 402                 perror("open pty");
 403                 return 1;
 404         }
 405
 406         if (tcgetattr(p, &topts)) {
 407                 perror("tcgetattr");
 408                 return 1;
 409         }
 410
 411         /* disable local echo which would cause double frames */
 412         topts.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK |
 413                            ECHONL | ECHOPRT | ECHOKE | ICRNL);
 414         tcsetattr(p, TCSANOW, &topts);
 415
 416         /* Support for the Unix 98 pseudo-terminal interface /dev/ptmx /dev/pts/N */
 417         if  (strcmp(argv[1], DEVICE_NAME_PTMX) == 0) {
 418
 419                 char *name_pts = NULL;  /* slave pseudo-terminal device name */
 420
 421                 if (grantpt(p) < 0) {
 422                         perror("grantpt");
 423                         return 1;
 424                 }
 425
 426                 if (unlockpt(p) < 0) {
 427                         perror("unlockpt");
 428                         return 1;
 429                 }
 430
 431                 name_pts = ptsname(p);
 432                 if (name_pts == NULL) {
 433                         perror("ptsname");
 434                         return 1;
 435                 }
 436                 printf("open: %s: slave pseudo-terminal is %s\n", argv[1], name_pts);
 437         }
 438
 439         /* open socket */
 440         s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 441         if (s < 0) {
 442                 perror("socket");
 443                 return 1;
 444         }
 445
 446         addr.can_family = AF_CAN;
 447
 448         strcpy(ifr.ifr_name, argv[2]);
 449         if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 450                 perror("SIOCGIFINDEX");
 451                 return 1;
 452         }
 453         addr.can_ifindex = ifr.ifr_ifindex;
 454
 455         /* disable reception of CAN frames until we are opened by 'O' */
 456         setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
 457
 458         if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 459                 perror("bind");
 460                 return 1;
 461         }
 462
 463         /* open filter by default */
 464         fi.can_id   = 0;
 465         fi.can_mask = 0;
 466
 467         while (running) {
 468
 469                 FD_ZERO(&rdfs);
 470                 FD_SET(0, &rdfs);
 471                 FD_SET(p, &rdfs);
 472                 FD_SET(s, &rdfs);
 473
 474                 if (select(s+1, &rdfs, NULL, NULL, NULL) < 0) {
 475                         perror("select");
 476                         return 1;
 477                 }
 478
 479                 if (FD_ISSET(0, &rdfs)) {
 480                         running = 0;
 481                         continue;
 482                 }
 483
 484                 if (FD_ISSET(p, &rdfs))
 485                         if (pty2can(p, s, &fi, &is_open, &tstamp)) {
 486                         running = 0;
 487                         continue;
 488                 }
 489
 490                 if (FD_ISSET(s, &rdfs))
 491                         if (can2pty(p, s, &tstamp)) {
 492                         running = 0;
 493                         continue;
 494                 }
 495         }
 496
 497         close(p);
 498         close(s);
 499
 500         return 0;
 501 }