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