lib.c

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