lib.c

   1 /*
   2  * lib.c - library for command line tools
   3  *
   4  * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above copyright
  13  *    notice, this list of conditions and the following disclaimer in the
  14  *    documentation and/or other materials provided with the distribution.
  15  * 3. Neither the name of Volkswagen nor the names of its contributors
  16  *    may be used to endorse or promote products derived from this software
  17  *    without specific prior written permission.
  18  *
  19  * Alternatively, provided that this notice is retained in full, this
  20  * software may be distributed under the terms of the GNU General
  21  * Public License ("GPL") version 2, in which case the provisions of the
  22  * GPL apply INSTEAD OF those given above.
  23  *
  24  * The provided data structures and external interfaces from this code
  25  * are not restricted to be used by modules with a GPL compatible license.
  26  *
  27  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  30  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  31  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  32  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  33  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  34  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  35  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  36  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  37  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  38  * DAMAGE.
  39  *
  40  * Send feedback to <linux-can@vger.kernel.org>
  41  *
  42  */
  43
  44 #include <stdio.h>
  45 #include <string.h>
  46 #include <stdint.h>
  47
  48 #include <sys/socket.h> /* for sa_family_t */
  49 #include <linux/can.h>
  50 #include <linux/can/error.h>
  51
  52 #include "lib.h"
  53
  54 #define CANID_DELIM '#'
  55 #define DATA_SEPERATOR '.'
  56
  57 /* CAN DLC to real data length conversion helpers */
  58
  59 static const unsigned char dlc2len[] = {0, 1, 2, 3, 4, 5, 6, 7,
  60                                         8, 12, 16, 20, 24, 32, 48, 64};
  61
  62 /* get data length from can_dlc with sanitized can_dlc */
  63 unsigned char can_dlc2len(unsigned char can_dlc)
  64 {
  65         return dlc2len[can_dlc & 0x0F];
  66 }
  67
  68 static const unsigned char len2dlc[] = {0, 1, 2, 3, 4, 5, 6, 7, 8,              /* 0 - 8 */
  69                                         9, 9, 9, 9,                             /* 9 - 12 */
  70                                         10, 10, 10, 10,                         /* 13 - 16 */
  71                                         11, 11, 11, 11,                         /* 17 - 20 */
  72                                         12, 12, 12, 12,                         /* 21 - 24 */
  73                                         13, 13, 13, 13, 13, 13, 13, 13,         /* 25 - 32 */
  74                                         14, 14, 14, 14, 14, 14, 14, 14,         /* 33 - 40 */
  75                                         14, 14, 14, 14, 14, 14, 14, 14,         /* 41 - 48 */
  76                                         15, 15, 15, 15, 15, 15, 15, 15,         /* 49 - 56 */
  77                                         15, 15, 15, 15, 15, 15, 15, 15};        /* 57 - 64 */
  78
  79 /* map the sanitized data length to an appropriate data length code */
  80 unsigned char can_len2dlc(unsigned char len)
  81 {
  82         if (len > 64)
  83                 return 0xF;
  84
  85         return len2dlc[len];
  86 }
  87
  88 unsigned char asc2nibble(char c) {
  89
  90         if ((c >= '0') && (c <= '9'))
  91                 return c - '0';
  92
  93         if ((c >= 'A') && (c <= 'F'))
  94                 return c - 'A' + 10;
  95
  96         if ((c >= 'a') && (c <= 'f'))
  97                 return c - 'a' + 10;
  98
  99         return 16; /* error */
 100 }
 101
 102 int hexstring2data(char *arg, unsigned char *data, int maxdlen) {
 103
 104         int len = strlen(arg);
 105         int i;
 106         unsigned char tmp;
 107
 108         if (!len || len%2 || len > maxdlen*2)
 109                 return 1;
 110
 111         memset(data, 0, maxdlen);
 112
 113         for (i=0; i < len/2; i++) {
 114
 115                 tmp = asc2nibble(*(arg+(2*i)));
 116                 if (tmp > 0x0F)
 117                         return 1;
 118
 119                 data[i] = (tmp << 4);
 120
 121                 tmp = asc2nibble(*(arg+(2*i)+1));
 122                 if (tmp > 0x0F)
 123                         return 1;
 124
 125                 data[i] |= tmp;
 126         }
 127
 128         return 0;
 129 }
 130
 131 int parse_canframe(char *cs, struct canfd_frame *cf) {
 132         /* documentation see lib.h */
 133
 134         int i, idx, dlen, len;
 135         int maxdlen = CAN_MAX_DLEN;
 136         int ret = CAN_MTU;
 137         unsigned char tmp;
 138
 139         len = strlen(cs);
 140         //printf("'%s' len %d\n", cs, len);
 141
 142         memset(cf, 0, sizeof(*cf)); /* init CAN FD frame, e.g. LEN = 0 */
 143
 144         if (len < 4)
 145                 return 0;
 146
 147         if (cs[3] == CANID_DELIM) { /* 3 digits */
 148
 149                 idx = 4;
 150                 for (i=0; i<3; i++){
 151                         if ((tmp = asc2nibble(cs[i])) > 0x0F)
 152                                 return 0;
 153                         cf->can_id |= (tmp << (2-i)*4);
 154                 }
 155
 156         } else if (cs[8] == CANID_DELIM) { /* 8 digits */
 157
 158                 idx = 9;
 159                 for (i=0; i<8; i++){
 160                         if ((tmp = asc2nibble(cs[i])) > 0x0F)
 161                                 return 0;
 162                         cf->can_id |= (tmp << (7-i)*4);
 163                 }
 164                 if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe?  */
 165                         cf->can_id |= CAN_EFF_FLAG;   /* then it is an extended frame */
 166
 167         } else
 168                 return 0;
 169
 170         if((cs[idx] == 'R') || (cs[idx] == 'r')){ /* RTR frame */
 171                 cf->can_id |= CAN_RTR_FLAG;
 172
 173                 /* check for optional DLC value for CAN 2.0B frames */
 174                 if(cs[++idx] && (tmp = asc2nibble(cs[idx])) <= CAN_MAX_DLC)
 175                         cf->len = tmp;
 176
 177                 return ret;
 178         }
 179
 180         if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */
 181
 182                 maxdlen = CANFD_MAX_DLEN;
 183                 ret = CANFD_MTU;
 184
 185                 /* CAN FD frame <canid>##<flags><data>* */
 186                 if ((tmp = asc2nibble(cs[idx+1])) > 0x0F)
 187                         return 0;
 188
 189                 cf->flags = tmp;
 190                 idx += 2;
 191         }
 192
 193         for (i=0, dlen=0; i < maxdlen; i++){
 194
 195                 if(cs[idx] == DATA_SEPERATOR) /* skip (optional) seperator */
 196                         idx++;
 197
 198                 if(idx >= len) /* end of string => end of data */
 199                         break;
 200
 201                 if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
 202                         return 0;
 203                 cf->data[i] = (tmp << 4);
 204                 if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
 205                         return 0;
 206                 cf->data[i] |= tmp;
 207                 dlen++;
 208         }
 209         cf->len = dlen;
 210
 211         return ret;
 212 }
 213
 214 void fprint_canframe(FILE *stream , struct canfd_frame *cf, char *eol, int sep, int maxdlen) {
 215         /* documentation see lib.h */
 216
 217         char buf[CL_CFSZ]; /* max length */
 218
 219         sprint_canframe(buf, cf, sep, maxdlen);
 220         fprintf(stream, "%s", buf);
 221         if (eol)
 222                 fprintf(stream, "%s", eol);
 223 }
 224
 225 void sprint_canframe(char *buf , struct canfd_frame *cf, int sep, int maxdlen) {
 226         /* documentation see lib.h */
 227
 228         int i,offset;
 229         int len = (cf->len > maxdlen) ? maxdlen : cf->len;
 230
 231         if (cf->can_id & CAN_ERR_FLAG) {
 232                 sprintf(buf, "%08X#", cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
 233                 offset = 9;
 234         } else if (cf->can_id & CAN_EFF_FLAG) {
 235                 sprintf(buf, "%08X#", cf->can_id & CAN_EFF_MASK);
 236                 offset = 9;
 237         } else {
 238                 sprintf(buf, "%03X#", cf->can_id & CAN_SFF_MASK);
 239                 offset = 4;
 240         }
 241
 242         /* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
 243         if (maxdlen == CAN_MAX_DLEN && cf->can_id & CAN_RTR_FLAG) {
 244
 245                 /* print a given CAN 2.0B DLC if it's not zero */
 246                 if (cf->len && cf->len <= CAN_MAX_DLC)
 247                         sprintf(buf+offset, "R%d", cf->len);
 248                 else
 249                         sprintf(buf+offset, "R");
 250
 251                 return;
 252         }
 253
 254         if (maxdlen == CANFD_MAX_DLEN) {
 255                 /* add CAN FD specific escape char and flags */
 256                 sprintf(buf+offset, "#%X", cf->flags & 0xF);
 257                 offset += 2;
 258                 if (sep && len)
 259                         sprintf(buf+offset++, ".");
 260         }
 261
 262         for (i = 0; i < len; i++) {
 263                 sprintf(buf+offset, "%02X", cf->data[i]);
 264                 offset += 2;
 265                 if (sep && (i+1 < len))
 266                         sprintf(buf+offset++, ".");
 267         }
 268 }
 269
 270 void fprint_long_canframe(FILE *stream , struct canfd_frame *cf, char *eol, int view, int maxdlen) {
 271         /* documentation see lib.h */
 272
 273         char buf[CL_LONGCFSZ];
 274
 275         sprint_long_canframe(buf, cf, view, maxdlen);
 276         fprintf(stream, "%s", buf);
 277         if ((view & CANLIB_VIEW_ERROR) && (cf->can_id & CAN_ERR_FLAG)) {
 278                 snprintf_can_error_frame(buf, sizeof(buf), cf, "\n\t");
 279                 fprintf(stream, "\n\t%s", buf);
 280         }
 281         if (eol)
 282                 fprintf(stream, "%s", eol);
 283 }
 284
 285 void sprint_long_canframe(char *buf , struct canfd_frame *cf, int view, int maxdlen) {
 286         /* documentation see lib.h */
 287
 288         int i, j, dlen, offset;
 289         int len = (cf->len > maxdlen)? maxdlen : cf->len;
 290
 291         if (cf->can_id & CAN_ERR_FLAG) {
 292                 sprintf(buf, "%08X  ", cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
 293                 offset = 10;
 294         } else if (cf->can_id & CAN_EFF_FLAG) {
 295                 sprintf(buf, "%08X  ", cf->can_id & CAN_EFF_MASK);
 296                 offset = 10;
 297         } else {
 298                 if (view & CANLIB_VIEW_INDENT_SFF) {
 299                         sprintf(buf, "     %03X  ", cf->can_id & CAN_SFF_MASK);
 300                         offset = 10;
 301                 } else {
 302                         sprintf(buf, "%03X  ", cf->can_id & CAN_SFF_MASK);
 303                         offset = 5;
 304                 }
 305         }
 306
 307         if (maxdlen == CAN_MAX_DLEN) {
 308                 sprintf(buf+offset, " [%d] ", len);
 309                 /* standard CAN frames may have RTR enabled */
 310                 if (cf->can_id & CAN_RTR_FLAG) {
 311                         sprintf(buf+offset+5, " remote request");
 312                         return;
 313                 }
 314         } else {
 315                 sprintf(buf+offset, "[%02d] ", len);
 316         }
 317         offset += 5;
 318
 319         if (view & CANLIB_VIEW_BINARY) {
 320                 dlen = 9; /* _10101010 */
 321                 if (view & CANLIB_VIEW_SWAP) {
 322                         for (i = len - 1; i >= 0; i--) {
 323                                 buf[offset++] = (i == len-1)?' ':SWAP_DELIMITER;
 324                                 for (j = 7; j >= 0; j--)
 325                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 326                         }
 327                 } else {
 328                         for (i = 0; i < len; i++) {
 329                                 buf[offset++] = ' ';
 330                                 for (j = 7; j >= 0; j--)
 331                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 332                         }
 333                 }
 334                 buf[offset] = 0; /* terminate string */
 335         } else {
 336                 dlen = 3; /* _AA */
 337                 if (view & CANLIB_VIEW_SWAP) {
 338                         for (i = len - 1; i >= 0; i--) {
 339                                 sprintf(buf+offset, "%c%02X",
 340                                         (i == len-1)?' ':SWAP_DELIMITER,
 341                                         cf->data[i]);
 342                                 offset += dlen;
 343                         }
 344                 } else {
 345                         for (i = 0; i < len; i++) {
 346                                 sprintf(buf+offset, " %02X", cf->data[i]);
 347                                 offset += dlen;
 348                         }
 349                 }
 350         }
 351
 352         /*
 353          * The ASCII & ERRORFRAME output is put at a fixed len behind the data.
 354          * For now we support ASCII output only for payload length up to 8 bytes.
 355          * Does it make sense to write 64 ASCII byte behind 64 ASCII HEX data on the console?
 356          */
 357         if (len > CAN_MAX_DLEN)
 358                 return;
 359
 360         if (cf->can_id & CAN_ERR_FLAG)
 361                 sprintf(buf+offset, "%*s", dlen*(8-len)+13, "ERRORFRAME");
 362         else if (view & CANLIB_VIEW_ASCII) {
 363                 j = dlen*(8-len)+4;
 364                 if (view & CANLIB_VIEW_SWAP) {
 365                         sprintf(buf+offset, "%*s", j, "`");
 366                         offset += j;
 367                         for (i = len - 1; i >= 0; i--)
 368                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 369                                         buf[offset++] = cf->data[i];
 370                                 else
 371                                         buf[offset++] = '.';
 372
 373                         sprintf(buf+offset, "`");
 374                 } else {
 375                         sprintf(buf+offset, "%*s", j, "'");
 376                         offset += j;
 377                         for (i = 0; i < len; i++)
 378                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 379                                         buf[offset++] = cf->data[i];
 380                                 else
 381                                         buf[offset++] = '.';
 382
 383                         sprintf(buf+offset, "'");
 384                 }
 385         }
 386 }
 387
 388 static const char *error_classes[] = {
 389         "tx-timeout",
 390         "lost-arbitration",
 391         "controller-problem",
 392         "protocol-violation",
 393         "transceiver-status",
 394         "no-acknowledgement-on-tx",
 395         "bus-off",
 396         "bus-error",
 397         "restarted-after-bus-off",
 398 };
 399
 400 static const char *controller_problems[] = {
 401         "rx-overflow",
 402         "tx-overflow",
 403         "rx-error-warning",
 404         "tx-error-warning",
 405         "rx-error-passive",
 406         "tx-error-passive",
 407 };
 408
 409 static const char *protocol_violation_types[] = {
 410         "single-bit-error",
 411         "frame-format-error",
 412         "bit-stuffing-error",
 413         "tx-dominant-bit-error",
 414         "tx-recessive-bit-error",
 415         "bus-overload",
 416         "back-to-error-active",
 417         "error-on-tx",
 418 };
 419
 420 static const char *protocol_violation_locations[] = {
 421         "unspecified",
 422         "unspecified",
 423         "id.28-to-id.28",
 424         "start-of-frame",
 425         "bit-srtr",
 426         "bit-ide",
 427         "id.20-to-id.18",
 428         "id.17-to-id.13",
 429         "crc-sequence",
 430         "reserved-bit-0",
 431         "data-field",
 432         "data-length-code",
 433         "bit-rtr",
 434         "reserved-bit-1",
 435         "id.4-to-id.0",
 436         "id.12-to-id.5",
 437         "unspecified",
 438         "active-error-flag",
 439         "intermission",
 440         "tolerate-dominant-bits",
 441         "unspecified",
 442         "unspecified",
 443         "passive-error-flag",
 444         "error-delimiter",
 445         "crc-delimiter",
 446         "acknowledge-slot",
 447         "end-of-frame",
 448         "acknowledge-delimiter",
 449         "overload-flag",
 450         "unspecified",
 451         "unspecified",
 452         "unspecified",
 453 };
 454
 455 #ifndef ARRAY_SIZE
 456 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
 457 #endif
 458
 459 static int snprintf_error_data(char *buf, size_t len, uint8_t err,
 460                                const char **arr, int arr_len)
 461 {
 462         int i, n = 0, count = 0;
 463
 464         if (!err || len <= 0)
 465                 return 0;
 466
 467         for (i = 0; i < arr_len; i++) {
 468                 if (err & (1 << i)) {
 469                         if (count)
 470                                 n += snprintf(buf + n, len - n, ",");
 471                         n += snprintf(buf + n, len - n, "%s", arr[i]);
 472                         count++;
 473                 }
 474         }
 475
 476         return n;
 477 }
 478
 479 static int snprintf_error_lostarb(char *buf, size_t len, struct canfd_frame *cf)
 480 {
 481         if (len <= 0)
 482                 return 0;
 483         return snprintf(buf, len, "{at bit %d}", cf->data[0]);
 484 }
 485
 486 static int snprintf_error_ctrl(char *buf, size_t len, struct canfd_frame *cf)
 487 {
 488         int n = 0;
 489
 490         if (len <= 0)
 491                 return 0;
 492
 493         n += snprintf(buf + n, len - n, "{");
 494         n += snprintf_error_data(buf + n, len - n, cf->data[1],
 495                                 controller_problems,
 496                                 ARRAY_SIZE(controller_problems));
 497         n += snprintf(buf + n, len - n, "}");
 498
 499         return n;
 500 }
 501
 502 static int snprintf_error_prot(char *buf, size_t len, struct canfd_frame *cf)
 503 {
 504         int n = 0;
 505
 506         if (len <= 0)
 507                 return 0;
 508
 509         n += snprintf(buf + n, len - n, "{{");
 510         n += snprintf_error_data(buf + n, len - n, cf->data[2],
 511                                 protocol_violation_types,
 512                                 ARRAY_SIZE(protocol_violation_types));
 513         n += snprintf(buf + n, len - n, "}{");
 514         if (cf->data[3] > 0 &&
 515             cf->data[3] < ARRAY_SIZE(protocol_violation_locations))
 516                 n += snprintf(buf + n, len - n, "%s",
 517                               protocol_violation_locations[cf->data[3]]);
 518         n += snprintf(buf + n, len - n, "}}");
 519
 520         return n;
 521 }
 522
 523 void snprintf_can_error_frame(char *buf, size_t len, struct canfd_frame *cf,
 524                               char* sep)
 525 {
 526         canid_t class, mask;
 527         int i, n = 0, classes = 0;
 528         char *defsep = ",";
 529
 530         if (!(cf->can_id & CAN_ERR_FLAG))
 531                 return;
 532
 533         class = cf->can_id & CAN_EFF_MASK;
 534         if (class > (1 << ARRAY_SIZE(error_classes))) {
 535                 fprintf(stderr, "Error class %#x is invalid\n", class);
 536                 return;
 537         }
 538
 539         if (!sep)
 540                 sep = defsep;
 541
 542         for (i = 0; i < ARRAY_SIZE(error_classes); i++) {
 543                 mask = 1 << i;
 544                 if (class & mask) {
 545                         if (classes)
 546                                 n += snprintf(buf + n, len - n, "%s", sep);
 547                         n += snprintf(buf + n, len - n, "%s", error_classes[i]);
 548                         if (mask == CAN_ERR_LOSTARB)
 549                                 n += snprintf_error_lostarb(buf + n, len - n,
 550                                                            cf);
 551                         if (mask == CAN_ERR_CRTL)
 552                                 n += snprintf_error_ctrl(buf + n, len - n, cf);
 553                         if (mask == CAN_ERR_PROT)
 554                                 n += snprintf_error_prot(buf + n, len - n, cf);
 555                         classes++;
 556                 }
 557         }
 558
 559         if (cf->data[6] || cf->data[7]) {
 560                 n += snprintf(buf + n, len - n, "%s", sep);
 561                 n += snprintf(buf + n, len - n, "error-counter-tx-rx{{%d}{%d}}",
 562                               cf->data[6], cf->data[7]);
 563         }
 564 }