slcanpty.c

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