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