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                 if (view & CANLIB_VIEW_INDENT_SFF) {
 292                         sprintf(buf, "     %03X  ", cf->can_id & CAN_SFF_MASK);
 293                         offset = 10;
 294                 } else {
 295                         sprintf(buf, "%03X  ", cf->can_id & CAN_SFF_MASK);
 296                         offset = 5;
 297                 }
 298         }
 299
 300         if (maxdlen == CAN_MAX_DLEN) {
 301                 sprintf(buf+offset, " [%d] ", len);
 302                 /* standard CAN frames may have RTR enabled */
 303                 if (cf->can_id & CAN_RTR_FLAG) {
 304                         sprintf(buf+offset+5, " remote request");
 305                         return;
 306                 }
 307         } else {
 308                 sprintf(buf+offset, "[%02d] ", len);
 309         }
 310         offset += 5;
 311
 312         if (view & CANLIB_VIEW_BINARY) {
 313                 dlen = 9; /* _10101010 */
 314                 if (view & CANLIB_VIEW_SWAP) {
 315                         for (i = len - 1; i >= 0; i--) {
 316                                 buf[offset++] = (i == len-1)?' ':SWAP_DELIMITER;
 317                                 for (j = 7; j >= 0; j--)
 318                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 319                         }
 320                 } else {
 321                         for (i = 0; i < len; i++) {
 322                                 buf[offset++] = ' ';
 323                                 for (j = 7; j >= 0; j--)
 324                                         buf[offset++] = (1<<j & cf->data[i])?'1':'0';
 325                         }
 326                 }
 327                 buf[offset] = 0; /* terminate string */
 328         } else {
 329                 dlen = 3; /* _AA */
 330                 if (view & CANLIB_VIEW_SWAP) {
 331                         for (i = len - 1; i >= 0; i--) {
 332                                 sprintf(buf+offset, "%c%02X",
 333                                         (i == len-1)?' ':SWAP_DELIMITER,
 334                                         cf->data[i]);
 335                                 offset += dlen;
 336                         }
 337                 } else {
 338                         for (i = 0; i < len; i++) {
 339                                 sprintf(buf+offset, " %02X", cf->data[i]);
 340                                 offset += dlen;
 341                         }
 342                 }
 343         }
 344
 345         /*
 346          * The ASCII & ERRORFRAME output is put at a fixed len behind the data.
 347          * For now we support ASCII output only for payload length up to 8 bytes.
 348          * Does it make sense to write 64 ASCII byte behind 64 ASCII HEX data on the console?
 349          */
 350         if (len > CAN_MAX_DLEN)
 351                 return;
 352
 353         if (cf->can_id & CAN_ERR_FLAG)
 354                 sprintf(buf+offset, "%*s", dlen*(8-len)+13, "ERRORFRAME");
 355         else if (view & CANLIB_VIEW_ASCII) {
 356                 j = dlen*(8-len)+4;
 357                 if (view & CANLIB_VIEW_SWAP) {
 358                         sprintf(buf+offset, "%*s", j, "`");
 359                         offset += j;
 360                         for (i = len - 1; i >= 0; i--)
 361                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 362                                         buf[offset++] = cf->data[i];
 363                                 else
 364                                         buf[offset++] = '.';
 365
 366                         sprintf(buf+offset, "`");
 367                 } else {
 368                         sprintf(buf+offset, "%*s", j, "'");
 369                         offset += j;
 370                         for (i = 0; i < len; i++)
 371                                 if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
 372                                         buf[offset++] = cf->data[i];
 373                                 else
 374                                         buf[offset++] = '.';
 375
 376                         sprintf(buf+offset, "'");
 377                 }
 378         }
 379 }
 380
 381 static const char *error_classes[] = {
 382         "tx-timeout",
 383         "lost-arbitration",
 384         "controller-problem",
 385         "protocol-violation",
 386         "transceiver-status",
 387         "no-acknowledgement-on-tx",
 388         "bus-off",
 389         "bus-error",
 390         "restarted-after-bus-off",
 391 };
 392
 393 static const char *controller_problems[] = {
 394         "rx-overflow",
 395         "tx-overflow",
 396         "rx-error-warning",
 397         "tx-error-warning",
 398         "rx-error-passive",
 399         "tx-error-passive",
 400 };
 401
 402 static const char *protocol_violation_types[] = {
 403         "single-bit-error",
 404         "frame-format-error",
 405         "bit-stuffing-error",
 406         "tx-dominant-bit-error",
 407         "tx-recessive-bit-error",
 408         "bus-overload",
 409         "back-to-error-active",
 410         "error-on-tx",
 411 };
 412
 413 static const char *protocol_violation_locations[] = {
 414         "unspecified",
 415         "unspecified",
 416         "id.28-to-id.28",
 417         "start-of-frame",
 418         "bit-srtr",
 419         "bit-ide",
 420         "id.20-to-id.18",
 421         "id.17-to-id.13",
 422         "crc-sequence",
 423         "reserved-bit-0",
 424         "data-field",
 425         "data-length-code",
 426         "bit-rtr",
 427         "reserved-bit-1",
 428         "id.4-to-id.0",
 429         "id.12-to-id.5",
 430         "unspecified",
 431         "active-error-flag",
 432         "intermission",
 433         "tolerate-dominant-bits",
 434         "unspecified",
 435         "unspecified",
 436         "passive-error-flag",
 437         "error-delimiter",
 438         "crc-delimiter",
 439         "acknowledge-slot",
 440         "end-of-frame",
 441         "acknowledge-delimiter",
 442         "overload-flag",
 443         "unspecified",
 444         "unspecified",
 445         "unspecified",
 446 };
 447
 448 #ifndef ARRAY_SIZE
 449 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
 450 #endif
 451
 452 static int snprintf_error_data(char *buf, size_t len, uint8_t err,
 453                                const char **arr, int arr_len)
 454 {
 455         int i, n = 0, count = 0;
 456
 457         if (!err || len <= 0)
 458                 return 0;
 459
 460         for (i = 0; i < arr_len; i++) {
 461                 if (err & (1 << i)) {
 462                         if (count)
 463                                 n += snprintf(buf + n, len - n, ",");
 464                         n += snprintf(buf + n, len - n, "%s", arr[i]);
 465                         count++;
 466                 }
 467         }
 468
 469         return n;
 470 }
 471
 472 static int snprintf_error_lostarb(char *buf, size_t len, struct canfd_frame *cf)
 473 {
 474         if (len <= 0)
 475                 return 0;
 476         return snprintf(buf, len, "{at bit %d}", cf->data[0]);
 477 }
 478
 479 static int snprintf_error_ctrl(char *buf, size_t len, struct canfd_frame *cf)
 480 {
 481         int n = 0;
 482
 483         if (len <= 0)
 484                 return 0;
 485
 486         n += snprintf(buf + n, len - n, "{");
 487         n += snprintf_error_data(buf + n, len - n, cf->data[1],
 488                                 controller_problems,
 489                                 ARRAY_SIZE(controller_problems));
 490         n += snprintf(buf + n, len - n, "}");
 491
 492         return n;
 493 }
 494
 495 static int snprintf_error_prot(char *buf, size_t len, struct canfd_frame *cf)
 496 {
 497         int n = 0;
 498
 499         if (len <= 0)
 500                 return 0;
 501
 502         n += snprintf(buf + n, len - n, "{{");
 503         n += snprintf_error_data(buf + n, len - n, cf->data[2],
 504                                 protocol_violation_types,
 505                                 ARRAY_SIZE(protocol_violation_types));
 506         n += snprintf(buf + n, len - n, "}{");
 507         if (cf->data[3] > 0 &&
 508             cf->data[3] < ARRAY_SIZE(protocol_violation_locations))
 509                 n += snprintf(buf + n, len - n, "%s",
 510                               protocol_violation_locations[cf->data[3]]);
 511         n += snprintf(buf + n, len - n, "}}");
 512
 513         return n;
 514 }
 515
 516 void snprintf_can_error_frame(char *buf, size_t len, struct canfd_frame *cf,
 517                               char* sep)
 518 {
 519         canid_t class, mask;
 520         int i, n = 0, classes = 0;
 521         char *defsep = ",";
 522
 523         if (!(cf->can_id & CAN_ERR_FLAG))
 524                 return;
 525
 526         class = cf->can_id & CAN_EFF_MASK;
 527         if (class > (1 << ARRAY_SIZE(error_classes))) {
 528                 fprintf(stderr, "Error class %#x is invalid\n", class);
 529                 return;
 530         }
 531
 532         if (!sep)
 533                 sep = defsep;
 534
 535         for (i = 0; i < ARRAY_SIZE(error_classes); i++) {
 536                 mask = 1 << i;
 537                 if (class & mask) {
 538                         if (classes)
 539                                 n += snprintf(buf + n, len - n, "%s", sep);
 540                         n += snprintf(buf + n, len - n, "%s", error_classes[i]);
 541                         if (mask == CAN_ERR_LOSTARB)
 542                                 n += snprintf_error_lostarb(buf + n, len - n,
 543                                                            cf);
 544                         if (mask == CAN_ERR_CRTL)
 545                                 n += snprintf_error_ctrl(buf + n, len - n, cf);
 546                         if (mask == CAN_ERR_PROT)
 547                                 n += snprintf_error_prot(buf + n, len - n, cf);
 548                         classes++;
 549                 }
 550         }
 551
 552         if (cf->data[6] || cf->data[7]) {
 553                 n += snprintf(buf + n, len - n, "%s", sep);
 554                 n += snprintf(buf + n, len - n, "error-counter-tx-rx{{%d}{%d}}",
 555                               cf->data[6], cf->data[7]);
 556         }
 557 }