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 const char hex_asc_upper[] = "0123456789ABCDEF";
  58
  59 #define hex_asc_upper_lo(x)     hex_asc_upper[((x) & 0x0F)]
  60 #define hex_asc_upper_hi(x)     hex_asc_upper[((x) & 0xF0) >> 4]
  61
  62 static inline void put_hex_byte(char *buf, __u8 byte)
  63 {
  64         buf[0] = hex_asc_upper_hi(byte);
  65         buf[1] = hex_asc_upper_lo(byte);
  66 }
  67
  68 static inline void _put_id(char *buf, int end_offset, canid_t id)
  69 {
  70         /* build 3 (SFF) or 8 (EFF) digit CAN identifier */
  71         while (end_offset >= 0) {
  72                 buf[end_offset--] = hex_asc_upper[id & 0xF];
  73                 id >>= 4;
  74         }
  75 }
  76
  77 #define put_sff_id(buf, id) _put_id(buf, 2, id)
  78 #define put_eff_id(buf, id) _put_id(buf, 7, id)
  79
  80 /* CAN DLC to real data length conversion helpers */
  81
  82 static const unsigned char dlc2len[] = {0, 1, 2, 3, 4, 5, 6, 7,
  83                                         8, 12, 16, 20, 24, 32, 48, 64};
  84
  85 /* get data length from can_dlc with sanitized can_dlc */
  86 unsigned char can_dlc2len(unsigned char can_dlc)
  87 {
  88         return dlc2len[can_dlc & 0x0F];
  89 }
  90
  91 static const unsigned char len2dlc[] = {0, 1, 2, 3, 4, 5, 6, 7, 8,              /* 0 - 8 */
  92                                         9, 9, 9, 9,                             /* 9 - 12 */
  93                                         10, 10, 10, 10,                         /* 13 - 16 */
  94                                         11, 11, 11, 11,                         /* 17 - 20 */
  95                                         12, 12, 12, 12,                         /* 21 - 24 */
  96                                         13, 13, 13, 13, 13, 13, 13, 13,         /* 25 - 32 */
  97                                         14, 14, 14, 14, 14, 14, 14, 14,         /* 33 - 40 */
  98                                         14, 14, 14, 14, 14, 14, 14, 14,         /* 41 - 48 */
  99                                         15, 15, 15, 15, 15, 15, 15, 15,         /* 49 - 56 */
 100                                         15, 15, 15, 15, 15, 15, 15, 15};        /* 57 - 64 */
 101
 102 /* map the sanitized data length to an appropriate data length code */
 103 unsigned char can_len2dlc(unsigned char len)
 104 {
 105         if (len > 64)
 106                 return 0xF;
 107
 108         return len2dlc[len];
 109 }
 110
 111 unsigned char asc2nibble(char c) {
 112
 113         if ((c >= '0') && (c <= '9'))
 114                 return c - '0';
 115
 116         if ((c >= 'A') && (c <= 'F'))
 117                 return c - 'A' + 10;
 118
 119         if ((c >= 'a') && (c <= 'f'))
 120                 return c - 'a' + 10;
 121
 122         return 16; /* error */
 123 }
 124
 125 int hexstring2data(char *arg, unsigned char *data, int maxdlen) {
 126
 127         int len = strlen(arg);
 128         int i;
 129         unsigned char tmp;
 130
 131         if (!len || len%2 || len > maxdlen*2)
 132                 return 1;
 133
 134         memset(data, 0, maxdlen);
 135
 136         for (i=0; i < len/2; i++) {
 137
 138                 tmp = asc2nibble(*(arg+(2*i)));
 139                 if (tmp > 0x0F)
 140                         return 1;
 141
 142                 data[i] = (tmp << 4);
 143
 144                 tmp = asc2nibble(*(arg+(2*i)+1));
 145                 if (tmp > 0x0F)
 146                         return 1;
 147
 148                 data[i] |= tmp;
 149         }
 150
 151         return 0;
 152 }
 153
 154 int parse_canframe(char *cs, struct canfd_frame *cf) {
 155         /* documentation see lib.h */
 156
 157         int i, idx, dlen, len;
 158         int maxdlen = CAN_MAX_DLEN;
 159         int ret = CAN_MTU;
 160         unsigned char tmp;
 161
 162         len = strlen(cs);
 163         //printf("'%s' len %d\n", cs, len);
 164
 165         memset(cf, 0, sizeof(*cf)); /* init CAN FD frame, e.g. LEN = 0 */
 166
 167         if (len < 4)
 168                 return 0;
 169
 170         if (cs[3] == CANID_DELIM) { /* 3 digits */
 171
 172                 idx = 4;
 173                 for (i=0; i<3; i++){
 174                         if ((tmp = asc2nibble(cs[i])) > 0x0F)
 175                                 return 0;
 176                         cf->can_id |= (tmp << (2-i)*4);
 177                 }
 178
 179         } else if (cs[8] == CANID_DELIM) { /* 8 digits */
 180
 181                 idx = 9;
 182                 for (i=0; i<8; i++){
 183                         if ((tmp = asc2nibble(cs[i])) > 0x0F)
 184                                 return 0;
 185                         cf->can_id |= (tmp << (7-i)*4);
 186                 }
 187                 if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe?  */
 188                         cf->can_id |= CAN_EFF_FLAG;   /* then it is an extended frame */
 189
 190         } else
 191                 return 0;
 192
 193         if((cs[idx] == 'R') || (cs[idx] == 'r')){ /* RTR frame */
 194                 cf->can_id |= CAN_RTR_FLAG;
 195
 196                 /* check for optional DLC value for CAN 2.0B frames */
 197                 if(cs[++idx] && (tmp = asc2nibble(cs[idx])) <= CAN_MAX_DLC)
 198                         cf->len = tmp;
 199
 200                 return ret;
 201         }
 202
 203         if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */
 204
 205                 maxdlen = CANFD_MAX_DLEN;
 206                 ret = CANFD_MTU;
 207
 208                 /* CAN FD frame <canid>##<flags><data>* */
 209                 if ((tmp = asc2nibble(cs[idx+1])) > 0x0F)
 210                         return 0;
 211
 212                 cf->flags = tmp;
 213                 idx += 2;
 214         }
 215
 216         for (i=0, dlen=0; i < maxdlen; i++){
 217
 218                 if(cs[idx] == DATA_SEPERATOR) /* skip (optional) separator */
 219                         idx++;
 220
 221                 if(idx >= len) /* end of string => end of data */
 222                         break;
 223
 224                 if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
 225                         return 0;
 226                 cf->data[i] = (tmp << 4);
 227                 if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
 228                         return 0;
 229                 cf->data[i] |= tmp;
 230                 dlen++;
 231         }
 232         cf->len = dlen;
 233
 234         return ret;
 235 }
 236
 237 void fprint_canframe(FILE *stream , struct canfd_frame *cf, char *eol, int sep, int maxdlen) {
 238         /* documentation see lib.h */
 239
 240         char buf[CL_CFSZ]; /* max length */
 241
 242         sprint_canframe(buf, cf, sep, maxdlen);
 243         fprintf(stream, "%s", buf);
 244         if (eol)
 245                 fprintf(stream, "%s", eol);
 246 }
 247
 248 void sprint_canframe(char *buf , struct canfd_frame *cf, int sep, int maxdlen) {
 249         /* documentation see lib.h */
 250
 251         int i,offset;
 252         int len = (cf->len > maxdlen) ? maxdlen : cf->len;
 253
 254         if (cf->can_id & CAN_ERR_FLAG) {
 255                 put_eff_id(buf, cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
 256                 buf[8] = '#';
 257                 offset = 9;
 258         } else if (cf->can_id & CAN_EFF_FLAG) {
 259                 put_eff_id(buf, cf->can_id & CAN_EFF_MASK);
 260                 buf[8] = '#';
 261                 offset = 9;
 262         } else {
 263                 put_sff_id(buf, cf->can_id & CAN_SFF_MASK);
 264                 buf[3] = '#';
 265                 offset = 4;
 266         }
 267
 268         /* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
 269         if (maxdlen == CAN_MAX_DLEN && cf->can_id & CAN_RTR_FLAG) {
 270                 buf[offset++] = 'R';
 271                 /* print a given CAN 2.0B DLC if it's not zero */
 272                 if (cf->len && cf->len <= CAN_MAX_DLC)
 273                         buf[offset++] = hex_asc_upper[cf->len & 0xF];
 274
 275                 buf[offset] = 0;
 276                 return;
 277         }
 278
 279         if (maxdlen == CANFD_MAX_DLEN) {
 280                 /* add CAN FD specific escape char and flags */
 281                 buf[offset++] = '#';
 282                 buf[offset++] = hex_asc_upper[cf->flags & 0xF];
 283                 if (sep && len)
 284                         buf[offset++] = '.';
 285         }
 286
 287         for (i = 0; i < len; i++) {
 288                 put_hex_byte(buf + offset, cf->data[i]);
 289                 offset += 2;
 290                 if (sep && (i+1 < len))
 291                         buf[offset++] = '.';
 292         }
 293
 294         buf[offset] = 0;
 295 }
 296
 297 void fprint_long_canframe(FILE *stream , struct canfd_frame *cf, char *eol, int view, int maxdlen) {
 298         /* documentation see lib.h */
 299
 300         char buf[CL_LONGCFSZ];
 301
 302         sprint_long_canframe(buf, cf, view, maxdlen);
 303         fprintf(stream, "%s", buf);
 304         if ((view & CANLIB_VIEW_ERROR) && (cf->can_id & CAN_ERR_FLAG)) {
 305                 snprintf_can_error_frame(buf, sizeof(buf), cf, "\n\t");
 306                 fprintf(stream, "\n\t%s", buf);
 307         }
 308         if (eol)
 309                 fprintf(stream, "%s", eol);
 310 }
 311
 312 void sprint_long_canframe(char *buf , struct canfd_frame *cf, int view, int maxdlen) {
 313         /* documentation see lib.h */
 314
 315         int i, j, dlen, offset;
 316         int len = (cf->len > maxdlen)? maxdlen : cf->len;
 317
 318         /* initialize space for CAN-ID and length information */
 319         memset(buf, ' ', 15);
 320
 321         if (cf->can_id & CAN_ERR_FLAG) {
 322                 put_eff_id(buf, cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
 323                 offset = 10;
 324         } else if (cf->can_id & CAN_EFF_FLAG) {
 325                 put_eff_id(buf, cf->can_id & CAN_EFF_MASK);
 326                 offset = 10;
 327         } else {
 328                 if (view & CANLIB_VIEW_INDENT_SFF) {
 329                         put_sff_id(buf + 5, cf->can_id & CAN_SFF_MASK);
 330                         offset = 10;
 331                 } else {
 332                         put_sff_id(buf, cf->can_id & CAN_SFF_MASK);
 333                         offset = 5;
 334                 }
 335         }
 336
 337         /* The len value is sanitized by maxdlen (see above) */
 338         if (maxdlen == CAN_MAX_DLEN) {
 339                 buf[offset + 1] = '[';
 340                 buf[offset + 2] = len + '0';
 341                 buf[offset + 3] = ']';
 342
 343                 /* standard CAN frames may have RTR enabled */
 344                 if (cf->can_id & CAN_RTR_FLAG) {
 345                         sprintf(buf+offset+5, " remote request");
 346                         return;
 347                 }
 348         } else {
 349                 buf[offset] = '[';
 350                 buf[offset + 1] = (len/10) + '0';
 351                 buf[offset + 2] = (len%10) + '0';
 352                 buf[offset + 3] = ']';
 353         }
 354         offset += 5;
 355
 356         if (view & CANLIB_VIEW_BINARY) {
 357                 dlen = 9; /* _10101010 */
 358                 if (view & CANLIB_VIEW_SWAP) {
 359                         for (i = len - 1; i >= 0; i--) {
 360                                 buf[offset++] = (i == len-1)?' ':SWAP_DELIMITER;
 361                                 for (j = 7; j >= 0; j--)
 362                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 363                         }
 364                 } else {
 365                         for (i = 0; i < len; i++) {
 366                                 buf[offset++] = ' ';
 367                                 for (j = 7; j >= 0; j--)
 368                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 369                         }
 370                 }
 371         } else {
 372                 dlen = 3; /* _AA */
 373                 if (view & CANLIB_VIEW_SWAP) {
 374                         for (i = len - 1; i >= 0; i--) {
 375                                 if (i == len-1)
 376                                         buf[offset++] = ' ';
 377                                 else
 378                                         buf[offset++] = SWAP_DELIMITER;
 379
 380                                 put_hex_byte(buf + offset, cf->data[i]);
 381                                 offset += 2;
 382                         }
 383                 } else {
 384                         for (i = 0; i < len; i++) {
 385                                 buf[offset++] = ' ';
 386                                 put_hex_byte(buf + offset, cf->data[i]);
 387                                 offset += 2;
 388                         }
 389                 }
 390         }
 391
 392         buf[offset] = 0; /* terminate string */
 393
 394         /*
 395          * The ASCII & ERRORFRAME output is put at a fixed len behind the data.
 396          * For now we support ASCII output only for payload length up to 8 bytes.
 397          * Does it make sense to write 64 ASCII byte behind 64 ASCII HEX data on the console?
 398          */
 399         if (len > CAN_MAX_DLEN)
 400                 return;
 401
 402         if (cf->can_id & CAN_ERR_FLAG)
 403                 sprintf(buf+offset, "%*s", dlen*(8-len)+13, "ERRORFRAME");
 404         else if (view & CANLIB_VIEW_ASCII) {
 405                 j = dlen*(8-len)+4;
 406                 if (view & CANLIB_VIEW_SWAP) {
 407                         sprintf(buf+offset, "%*s", j, "`");
 408                         offset += j;
 409                         for (i = len - 1; i >= 0; i--)
 410                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 411                                         buf[offset++] = cf->data[i];
 412                                 else
 413                                         buf[offset++] = '.';
 414
 415                         sprintf(buf+offset, "`");
 416                 } else {
 417                         sprintf(buf+offset, "%*s", j, "'");
 418                         offset += j;
 419                         for (i = 0; i < len; i++)
 420                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 421                                         buf[offset++] = cf->data[i];
 422                                 else
 423                                         buf[offset++] = '.';
 424
 425                         sprintf(buf+offset, "'");
 426                 }
 427         }
 428 }
 429
 430 static const char *error_classes[] = {
 431         "tx-timeout",
 432         "lost-arbitration",
 433         "controller-problem",
 434         "protocol-violation",
 435         "transceiver-status",
 436         "no-acknowledgement-on-tx",
 437         "bus-off",
 438         "bus-error",
 439         "restarted-after-bus-off",
 440 };
 441
 442 static const char *controller_problems[] = {
 443         "rx-overflow",
 444         "tx-overflow",
 445         "rx-error-warning",
 446         "tx-error-warning",
 447         "rx-error-passive",
 448         "tx-error-passive",
 449         "back-to-error-active",
 450 };
 451
 452 static const char *protocol_violation_types[] = {
 453         "single-bit-error",
 454         "frame-format-error",
 455         "bit-stuffing-error",
 456         "tx-dominant-bit-error",
 457         "tx-recessive-bit-error",
 458         "bus-overload",
 459         "active-error",
 460         "error-on-tx",
 461 };
 462
 463 static const char *protocol_violation_locations[] = {
 464         "unspecified",
 465         "unspecified",
 466         "id.28-to-id.28",
 467         "start-of-frame",
 468         "bit-srtr",
 469         "bit-ide",
 470         "id.20-to-id.18",
 471         "id.17-to-id.13",
 472         "crc-sequence",
 473         "reserved-bit-0",
 474         "data-field",
 475         "data-length-code",
 476         "bit-rtr",
 477         "reserved-bit-1",
 478         "id.4-to-id.0",
 479         "id.12-to-id.5",
 480         "unspecified",
 481         "active-error-flag",
 482         "intermission",
 483         "tolerate-dominant-bits",
 484         "unspecified",
 485         "unspecified",
 486         "passive-error-flag",
 487         "error-delimiter",
 488         "crc-delimiter",
 489         "acknowledge-slot",
 490         "end-of-frame",
 491         "acknowledge-delimiter",
 492         "overload-flag",
 493         "unspecified",
 494         "unspecified",
 495         "unspecified",
 496 };
 497
 498 #ifndef ARRAY_SIZE
 499 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
 500 #endif
 501
 502 static int snprintf_error_data(char *buf, size_t len, uint8_t err,
 503                                const char **arr, int arr_len)
 504 {
 505         int i, n = 0, count = 0;
 506
 507         if (!err || len <= 0)
 508                 return 0;
 509
 510         for (i = 0; i < arr_len; i++) {
 511                 if (err & (1 << i)) {
 512                         if (count)
 513                                 n += snprintf(buf + n, len - n, ",");
 514                         n += snprintf(buf + n, len - n, "%s", arr[i]);
 515                         count++;
 516                 }
 517         }
 518
 519         return n;
 520 }
 521
 522 static int snprintf_error_lostarb(char *buf, size_t len, const struct canfd_frame *cf)
 523 {
 524         if (len <= 0)
 525                 return 0;
 526         return snprintf(buf, len, "{at bit %d}", cf->data[0]);
 527 }
 528
 529 static int snprintf_error_ctrl(char *buf, size_t len, const struct canfd_frame *cf)
 530 {
 531         int n = 0;
 532
 533         if (len <= 0)
 534                 return 0;
 535
 536         n += snprintf(buf + n, len - n, "{");
 537         n += snprintf_error_data(buf + n, len - n, cf->data[1],
 538                                 controller_problems,
 539                                 ARRAY_SIZE(controller_problems));
 540         n += snprintf(buf + n, len - n, "}");
 541
 542         return n;
 543 }
 544
 545 static int snprintf_error_prot(char *buf, size_t len, const struct canfd_frame *cf)
 546 {
 547         int n = 0;
 548
 549         if (len <= 0)
 550                 return 0;
 551
 552         n += snprintf(buf + n, len - n, "{{");
 553         n += snprintf_error_data(buf + n, len - n, cf->data[2],
 554                                 protocol_violation_types,
 555                                 ARRAY_SIZE(protocol_violation_types));
 556         n += snprintf(buf + n, len - n, "}{");
 557         if (cf->data[3] > 0 &&
 558             cf->data[3] < ARRAY_SIZE(protocol_violation_locations))
 559                 n += snprintf(buf + n, len - n, "%s",
 560                               protocol_violation_locations[cf->data[3]]);
 561         n += snprintf(buf + n, len - n, "}}");
 562
 563         return n;
 564 }
 565
 566 void snprintf_can_error_frame(char *buf, size_t len, const struct canfd_frame *cf,
 567                   const char* sep)
 568 {
 569         canid_t class, mask;
 570         int i, n = 0, classes = 0;
 571         char *defsep = ",";
 572
 573         if (!(cf->can_id & CAN_ERR_FLAG))
 574                 return;
 575
 576         class = cf->can_id & CAN_EFF_MASK;
 577         if (class > (1 << ARRAY_SIZE(error_classes))) {
 578                 fprintf(stderr, "Error class %#x is invalid\n", class);
 579                 return;
 580         }
 581
 582         if (!sep)
 583                 sep = defsep;
 584
 585         for (i = 0; i < ARRAY_SIZE(error_classes); i++) {
 586                 mask = 1 << i;
 587                 if (class & mask) {
 588                         if (classes)
 589                                 n += snprintf(buf + n, len - n, "%s", sep);
 590                         n += snprintf(buf + n, len - n, "%s", error_classes[i]);
 591                         if (mask == CAN_ERR_LOSTARB)
 592                                 n += snprintf_error_lostarb(buf + n, len - n,
 593                                                            cf);
 594                         if (mask == CAN_ERR_CRTL)
 595                                 n += snprintf_error_ctrl(buf + n, len - n, cf);
 596                         if (mask == CAN_ERR_PROT)
 597                                 n += snprintf_error_prot(buf + n, len - n, cf);
 598                         classes++;
 599                 }
 600         }
 601
 602         if (cf->data[6] || cf->data[7]) {
 603                 n += snprintf(buf + n, len - n, "%s", sep);
 604                 n += snprintf(buf + n, len - n, "error-counter-tx-rx{{%d}{%d}}",
 605                               cf->data[6], cf->data[7]);
 606         }
 607 }