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