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                 return ret;
 173         }
 174
 175         if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */
 176
 177                 maxdlen = CANFD_MAX_DLEN;
 178                 ret = CANFD_MTU;
 179
 180                 /* CAN FD frame <canid>##<flags><data>* */
 181                 if ((tmp = asc2nibble(cs[idx+1])) > 0x0F)
 182                         return 0;
 183
 184                 cf->flags = tmp;
 185                 idx += 2;
 186         }
 187
 188         for (i=0, dlen=0; i < maxdlen; i++){
 189
 190                 if(cs[idx] == DATA_SEPERATOR) /* skip (optional) seperator */
 191                         idx++;
 192
 193                 if(idx >= len) /* end of string => end of data */
 194                         break;
 195
 196                 if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
 197                         return 0;
 198                 cf->data[i] = (tmp << 4);
 199                 if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
 200                         return 0;
 201                 cf->data[i] |= tmp;
 202                 dlen++;
 203         }
 204         cf->len = dlen;
 205
 206         return ret;
 207 }
 208
 209 void fprint_canframe(FILE *stream , struct canfd_frame *cf, char *eol, int sep, int maxdlen) {
 210         /* documentation see lib.h */
 211
 212         char buf[CL_CFSZ]; /* max length */
 213
 214         sprint_canframe(buf, cf, sep, maxdlen);
 215         fprintf(stream, "%s", buf);
 216         if (eol)
 217                 fprintf(stream, "%s", eol);
 218 }
 219
 220 void sprint_canframe(char *buf , struct canfd_frame *cf, int sep, int maxdlen) {
 221         /* documentation see lib.h */
 222
 223         int i,offset;
 224         int len = (cf->len > maxdlen) ? maxdlen : cf->len;
 225
 226         if (cf->can_id & CAN_ERR_FLAG) {
 227                 sprintf(buf, "%08X#", cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
 228                 offset = 9;
 229         } else if (cf->can_id & CAN_EFF_FLAG) {
 230                 sprintf(buf, "%08X#", cf->can_id & CAN_EFF_MASK);
 231                 offset = 9;
 232         } else {
 233                 sprintf(buf, "%03X#", cf->can_id & CAN_SFF_MASK);
 234                 offset = 4;
 235         }
 236
 237         /* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
 238         if (maxdlen == CAN_MAX_DLEN && cf->can_id & CAN_RTR_FLAG) {
 239                 sprintf(buf+offset, "R");
 240                 return;
 241         }
 242
 243         if (maxdlen == CANFD_MAX_DLEN) {
 244                 /* add CAN FD specific escape char and flags */
 245                 sprintf(buf+offset, "#%X", cf->flags & 0xF);
 246                 offset += 2;
 247                 if (sep && len)
 248                         sprintf(buf+offset++, ".");
 249         }
 250
 251         for (i = 0; i < len; i++) {
 252                 sprintf(buf+offset, "%02X", cf->data[i]);
 253                 offset += 2;
 254                 if (sep && (i+1 < len))
 255                         sprintf(buf+offset++, ".");
 256         }
 257 }
 258
 259 void fprint_long_canframe(FILE *stream , struct canfd_frame *cf, char *eol, int view, int maxdlen) {
 260         /* documentation see lib.h */
 261
 262         char buf[CL_LONGCFSZ];
 263
 264         sprint_long_canframe(buf, cf, view, maxdlen);
 265         fprintf(stream, "%s", buf);
 266         if ((view & CANLIB_VIEW_ERROR) && (cf->can_id & CAN_ERR_FLAG)) {
 267                 snprintf_can_error_frame(buf, sizeof(buf), cf, "\n\t");
 268                 fprintf(stream, "\n\t%s", buf);
 269         }
 270         if (eol)
 271                 fprintf(stream, "%s", eol);
 272 }
 273
 274 void sprint_long_canframe(char *buf , struct canfd_frame *cf, int view, int maxdlen) {
 275         /* documentation see lib.h */
 276
 277         int i, j, dlen, offset;
 278         int len = (cf->len > maxdlen)? maxdlen : cf->len;
 279
 280         if (cf->can_id & CAN_ERR_FLAG) {
 281                 sprintf(buf, "%08X  ", cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
 282                 offset = 10;
 283         } else if (cf->can_id & CAN_EFF_FLAG) {
 284                 sprintf(buf, "%08X  ", cf->can_id & CAN_EFF_MASK);
 285                 offset = 10;
 286         } else {
 287                 if (view & CANLIB_VIEW_INDENT_SFF) {
 288                         sprintf(buf, "     %03X  ", cf->can_id & CAN_SFF_MASK);
 289                         offset = 10;
 290                 } else {
 291                         sprintf(buf, "%03X  ", cf->can_id & CAN_SFF_MASK);
 292                         offset = 5;
 293                 }
 294         }
 295
 296         if (maxdlen == CAN_MAX_DLEN) {
 297                 sprintf(buf+offset, " [%d] ", len);
 298                 /* standard CAN frames may have RTR enabled */
 299                 if (cf->can_id & CAN_RTR_FLAG) {
 300                         sprintf(buf+offset+5, " remote request");
 301                         return;
 302                 }
 303         } else {
 304                 sprintf(buf+offset, "[%02d] ", len);
 305         }
 306         offset += 5;
 307
 308         if (view & CANLIB_VIEW_BINARY) {
 309                 dlen = 9; /* _10101010 */
 310                 if (view & CANLIB_VIEW_SWAP) {
 311                         for (i = len - 1; i >= 0; i--) {
 312                                 buf[offset++] = (i == len-1)?' ':SWAP_DELIMITER;
 313                                 for (j = 7; j >= 0; j--)
 314                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 315                         }
 316                 } else {
 317                         for (i = 0; i < len; i++) {
 318                                 buf[offset++] = ' ';
 319                                 for (j = 7; j >= 0; j--)
 320                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 321                         }
 322                 }
 323                 buf[offset] = 0; /* terminate string */
 324         } else {
 325                 dlen = 3; /* _AA */
 326                 if (view & CANLIB_VIEW_SWAP) {
 327                         for (i = len - 1; i >= 0; i--) {
 328                                 sprintf(buf+offset, "%c%02X",
 329                                         (i == len-1)?' ':SWAP_DELIMITER,
 330                                         cf->data[i]);
 331                                 offset += dlen;
 332                         }
 333                 } else {
 334                         for (i = 0; i < len; i++) {
 335                                 sprintf(buf+offset, " %02X", cf->data[i]);
 336                                 offset += dlen;
 337                         }
 338                 }
 339         }
 340
 341         /*
 342          * The ASCII & ERRORFRAME output is put at a fixed len behind the data.
 343          * For now we support ASCII output only for payload length up to 8 bytes.
 344          * Does it make sense to write 64 ASCII byte behind 64 ASCII HEX data on the console?
 345          */
 346         if (len > CAN_MAX_DLEN)
 347                 return;
 348
 349         if (cf->can_id & CAN_ERR_FLAG)
 350                 sprintf(buf+offset, "%*s", dlen*(8-len)+13, "ERRORFRAME");
 351         else if (view & CANLIB_VIEW_ASCII) {
 352                 j = dlen*(8-len)+4;
 353                 if (view & CANLIB_VIEW_SWAP) {
 354                         sprintf(buf+offset, "%*s", j, "`");
 355                         offset += j;
 356                         for (i = len - 1; i >= 0; i--)
 357                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 358                                         buf[offset++] = cf->data[i];
 359                                 else
 360                                         buf[offset++] = '.';
 361
 362                         sprintf(buf+offset, "`");
 363                 } else {
 364                         sprintf(buf+offset, "%*s", j, "'");
 365                         offset += j;
 366                         for (i = 0; i < len; i++)
 367                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 368                                         buf[offset++] = cf->data[i];
 369                                 else
 370                                         buf[offset++] = '.';
 371
 372                         sprintf(buf+offset, "'");
 373                 }
 374         }
 375 }
 376
 377 static const char *error_classes[] = {
 378         "tx-timeout",
 379         "lost-arbitration",
 380         "controller-problem",
 381         "protocol-violation",
 382         "transceiver-status",
 383         "no-acknowledgement-on-tx",
 384         "bus-off",
 385         "bus-error",
 386         "restarted-after-bus-off",
 387 };
 388
 389 static const char *controller_problems[] = {
 390         "rx-overflow",
 391         "tx-overflow",
 392         "rx-error-warning",
 393         "tx-error-warning",
 394         "rx-error-passive",
 395         "tx-error-passive",
 396 };
 397
 398 static const char *protocol_violation_types[] = {
 399         "single-bit-error",
 400         "frame-format-error",
 401         "bit-stuffing-error",
 402         "tx-dominant-bit-error",
 403         "tx-recessive-bit-error",
 404         "bus-overload",
 405         "back-to-error-active",
 406         "error-on-tx",
 407 };
 408
 409 static const char *protocol_violation_locations[] = {
 410         "unspecified",
 411         "unspecified",
 412         "id.28-to-id.28",
 413         "start-of-frame",
 414         "bit-srtr",
 415         "bit-ide",
 416         "id.20-to-id.18",
 417         "id.17-to-id.13",
 418         "crc-sequence",
 419         "reserved-bit-0",
 420         "data-field",
 421         "data-length-code",
 422         "bit-rtr",
 423         "reserved-bit-1",
 424         "id.4-to-id.0",
 425         "id.12-to-id.5",
 426         "unspecified",
 427         "active-error-flag",
 428         "intermission",
 429         "tolerate-dominant-bits",
 430         "unspecified",
 431         "unspecified",
 432         "passive-error-flag",
 433         "error-delimiter",
 434         "crc-delimiter",
 435         "acknowledge-slot",
 436         "end-of-frame",
 437         "acknowledge-delimiter",
 438         "overload-flag",
 439         "unspecified",
 440         "unspecified",
 441         "unspecified",
 442 };
 443
 444 #ifndef ARRAY_SIZE
 445 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
 446 #endif
 447
 448 static int snprintf_error_data(char *buf, size_t len, uint8_t err,
 449                                const char **arr, int arr_len)
 450 {
 451         int i, n = 0, count = 0;
 452
 453         if (!err || len <= 0)
 454                 return 0;
 455
 456         for (i = 0; i < arr_len; i++) {
 457                 if (err & (1 << i)) {
 458                         if (count)
 459                                 n += snprintf(buf + n, len - n, ",");
 460                         n += snprintf(buf + n, len - n, "%s", arr[i]);
 461                         count++;
 462                 }
 463         }
 464
 465         return n;
 466 }
 467
 468 static int snprintf_error_lostarb(char *buf, size_t len, struct canfd_frame *cf)
 469 {
 470         if (len <= 0)
 471                 return 0;
 472         return snprintf(buf, len, "{at bit %d}", cf->data[0]);
 473 }
 474
 475 static int snprintf_error_ctrl(char *buf, size_t len, struct canfd_frame *cf)
 476 {
 477         int n = 0;
 478
 479         if (len <= 0)
 480                 return 0;
 481
 482         n += snprintf(buf + n, len - n, "{");
 483         n += snprintf_error_data(buf + n, len - n, cf->data[1],
 484                                 controller_problems,
 485                                 ARRAY_SIZE(controller_problems));
 486         n += snprintf(buf + n, len - n, "}");
 487
 488         return n;
 489 }
 490
 491 static int snprintf_error_prot(char *buf, size_t len, struct canfd_frame *cf)
 492 {
 493         int n = 0;
 494
 495         if (len <= 0)
 496                 return 0;
 497
 498         n += snprintf(buf + n, len - n, "{{");
 499         n += snprintf_error_data(buf + n, len - n, cf->data[2],
 500                                 protocol_violation_types,
 501                                 ARRAY_SIZE(protocol_violation_types));
 502         n += snprintf(buf + n, len - n, "}{");
 503         if (cf->data[3] > 0 &&
 504             cf->data[3] < ARRAY_SIZE(protocol_violation_locations))
 505                 n += snprintf(buf + n, len - n, "%s",
 506                               protocol_violation_locations[cf->data[3]]);
 507         n += snprintf(buf + n, len - n, "}}");
 508
 509         return n;
 510 }
 511
 512 void snprintf_can_error_frame(char *buf, size_t len, struct canfd_frame *cf,
 513                               char* sep)
 514 {
 515         canid_t class, mask;
 516         int i, n = 0, classes = 0;
 517         char *defsep = ",";
 518
 519         if (!(cf->can_id & CAN_ERR_FLAG))
 520                 return;
 521
 522         class = cf->can_id & CAN_EFF_MASK;
 523         if (class > (1 << ARRAY_SIZE(error_classes))) {
 524                 fprintf(stderr, "Error class %#x is invalid\n", class);
 525                 return;
 526         }
 527
 528         if (!sep)
 529                 sep = defsep;
 530
 531         for (i = 0; i < ARRAY_SIZE(error_classes); i++) {
 532                 mask = 1 << i;
 533                 if (class & mask) {
 534                         if (classes)
 535                                 n += snprintf(buf + n, len - n, "%s", sep);
 536                         n += snprintf(buf + n, len - n, "%s", error_classes[i]);
 537                         if (mask == CAN_ERR_LOSTARB)
 538                                 n += snprintf_error_lostarb(buf + n, len - n,
 539                                                            cf);
 540                         if (mask == CAN_ERR_CRTL)
 541                                 n += snprintf_error_ctrl(buf + n, len - n, cf);
 542                         if (mask == CAN_ERR_PROT)
 543                                 n += snprintf_error_prot(buf + n, len - n, cf);
 544                         classes++;
 545                 }
 546         }
 547
 548         if (cf->data[6] || cf->data[7]) {
 549                 n += snprintf(buf + n, len - n, "%s", sep);
 550                 n += snprintf(buf + n, len - n, "error-counter-tx-rx{{%d}{%d}}",
 551                               cf->data[6], cf->data[7]);
 552         }
 553 }