Added new socketoptions to force the isotp protocol to intentionally

[sojka/can-utils.git] / lib.c

diff --git a/lib.c b/lib.c
index cfc7669cb43918a786a753962197c2c79e3976f7..0b9148817b1e72d33b45f083d994582cc9a6b57f 100644 (file)
--- a/lib.c
+++ b/lib.c
@@ -59,7 +59,7 @@
 #define MAX_CANFRAME      "12345678#01.23.45.67.89.AB.CD.EF"
 #define MAX_LONG_CANFRAME "12345678  [8] 10101010 10101010 10101010 10101010 10101010 10101010 10101010 10101010   '........'"
 
-static int asc2nibble(char c) {
+unsigned char asc2nibble(char c) {
 
        if ((c >= '0') && (c <= '9'))
                return c - '0';
@@ -73,10 +73,38 @@ static int asc2nibble(char c) {
        return 16; /* error */
 }
 
+int hexstring2candata(char *arg, struct can_frame *cf) {
+
+       int len = strlen(arg);
+       int i;
+       unsigned char tmp;
+
+       if (!len || len%2 || len > 16)
+               return 1;
+
+       for (i=0; i < len/2; i++) {
+
+               tmp = asc2nibble(*(arg+(2*i)));
+               if (tmp > 0x0F)
+                       return 1;
+
+               cf->data[i] = (tmp << 4);
+
+               tmp = asc2nibble(*(arg+(2*i)+1));
+               if (tmp > 0x0F)
+                       return 1;
+
+               cf->data[i] |= tmp;
+       }
+
+       return 0;
+}
+
 int parse_canframe(char *cs, struct can_frame *cf) {
        /* documentation see lib.h */
 
-       int i, idx, dlc, len, tmp;
+       int i, idx, dlc, len;
+       unsigned char tmp;
 
        len = strlen(cs);
        //printf("'%s' len %d\n", cs, len);
@@ -86,10 +114,16 @@ int parse_canframe(char *cs, struct can_frame *cf) {
        if (len < 4)
                return 1;
 
-       if (!((cs[3] == CANID_DELIM) || (cs[8] == CANID_DELIM)))
-               return 1;
+       if (cs[3] == CANID_DELIM) { /* 3 digits */
 
-       if (cs[8] == CANID_DELIM) { /* 8 digits */
+               idx = 4;
+               for (i=0; i<3; i++){
+                       if ((tmp = asc2nibble(cs[i])) > 0x0F)
+                               return 1;
+                       cf->can_id |= (tmp << (2-i)*4);
+               }
+
+       } else if (cs[8] == CANID_DELIM) { /* 8 digits */
 
                idx = 9;
                for (i=0; i<8; i++){
@@ -100,15 +134,8 @@ int parse_canframe(char *cs, struct can_frame *cf) {
                if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe?  */
                        cf->can_id |= CAN_EFF_FLAG;   /* then it is an extended frame */
 
-       } else { /* 3 digits */
-
-               idx = 4;
-               for (i=0; i<3; i++){
-                       if ((tmp = asc2nibble(cs[i])) > 0x0F)
-                               return 1;
-                       cf->can_id |= (tmp << (2-i)*4);
-               }
-       }
+       } else
+               return 1;
 
        if((cs[idx] == 'R') || (cs[idx] == 'r')){ /* RTR frame */
                cf->can_id |= CAN_RTR_FLAG;
@@ -152,6 +179,7 @@ void sprint_canframe(char *buf , struct can_frame *cf, int sep) {
        /* documentation see lib.h */
 
        int i,offset;
+       int dlc = (cf->can_dlc > 8)? 8 : cf->can_dlc;
 
        if (cf->can_id & CAN_ERR_FLAG) {
                sprintf(buf, "%08X#", cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
@@ -167,10 +195,10 @@ void sprint_canframe(char *buf , struct can_frame *cf, int sep) {
        if (cf->can_id & CAN_RTR_FLAG) /* there are no ERR frames with RTR */
                sprintf(buf+offset, "R");
        else
-               for (i = 0; i < cf->can_dlc; i++) {
+               for (i = 0; i < dlc; i++) {
                        sprintf(buf+offset, "%02X", cf->data[i]);
                        offset += 2;
-                       if (sep && (i+1 < cf->can_dlc))
+                       if (sep && (i+1 < dlc))
                                sprintf(buf+offset++, ".");
                }
 
@@ -192,6 +220,7 @@ void sprint_long_canframe(char *buf , struct can_frame *cf, int view) {
        /* documentation see lib.h */
 
        int i, j, dlen, offset;
+       int dlc = (cf->can_dlc > 8)? 8 : cf->can_dlc;
 
        if (cf->can_id & CAN_ERR_FLAG) {
                sprintf(buf, "%8X  ", cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
@@ -204,7 +233,7 @@ void sprint_long_canframe(char *buf , struct can_frame *cf, int view) {
                offset = 5;
        }
 
-       sprintf(buf+offset, "[%d]", cf->can_dlc);
+       sprintf(buf+offset, "[%d]", dlc);
        offset += 3;
 
        if (cf->can_id & CAN_RTR_FLAG) { /* there are no ERR frames with RTR */
@@ -214,34 +243,62 @@ void sprint_long_canframe(char *buf , struct can_frame *cf, int view) {
 
        if (view & CANLIB_VIEW_BINARY) {
                dlen = 9; /* _10101010 */
-               for (i = 0; i < cf->can_dlc; i++) {
-                       buf[offset++] = ' ';
-                       for (j = 7; j >= 0; j--)
-                               buf[offset++] = (1<<j & cf->data[i])?'1':'0';
+               if (view & CANLIB_VIEW_SWAP) {
+                       for (i = dlc - 1; i >= 0; i--) {
+                               buf[offset++] = (i == dlc-1)?' ':SWAP_DELIMITER;
+                               for (j = 7; j >= 0; j--)
+                                       buf[offset++] = (1<<j & cf->data[i])?'1':'0';
+                       }
+               } else {
+                       for (i = 0; i < dlc; i++) {
+                               buf[offset++] = ' ';
+                               for (j = 7; j >= 0; j--)
+                                       buf[offset++] = (1<<j & cf->data[i])?'1':'0';
+                       }
                }
                buf[offset] = 0; /* terminate string */
        } else {
                dlen = 3; /* _AA */
-               for (i = 0; i < cf->can_dlc; i++) {
-                       sprintf(buf+offset, " %02X", cf->data[i]);
-                       offset += dlen;
+               if (view & CANLIB_VIEW_SWAP) {
+                       for (i = dlc - 1; i >= 0; i--) {
+                               sprintf(buf+offset, "%c%02X",
+                                       (i == dlc-1)?' ':SWAP_DELIMITER,
+                                       cf->data[i]);
+                               offset += dlen;
+                       }
+               } else {
+                       for (i = 0; i < dlc; i++) {
+                               sprintf(buf+offset, " %02X", cf->data[i]);
+                               offset += dlen;
+                       }
                }
        }
 
        if (cf->can_id & CAN_ERR_FLAG)
-               sprintf(buf+offset, "%*s", dlen*(8-cf->can_dlc)+13, "ERRORFRAME");
+               sprintf(buf+offset, "%*s", dlen*(8-dlc)+13, "ERRORFRAME");
        else if (view & CANLIB_VIEW_ASCII) {
-               j = dlen*(8-cf->can_dlc)+4;
-               sprintf(buf+offset, "%*s", j, "'");
-               offset += j;
-
-               for (i = 0; i < cf->can_dlc; i++)
-                       if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
-                               buf[offset++] = cf->data[i];
-                       else
-                               buf[offset++] = '.';
-
-               sprintf(buf+offset, "'");
+               j = dlen*(8-dlc)+4;
+               if (view & CANLIB_VIEW_SWAP) {
+                       sprintf(buf+offset, "%*s", j, "`");
+                       offset += j;
+                       for (i = dlc - 1; i >= 0; i--)
+                               if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
+                                       buf[offset++] = cf->data[i];
+                               else
+                                       buf[offset++] = '.';
+
+                       sprintf(buf+offset, "`");
+               } else {
+                       sprintf(buf+offset, "%*s", j, "'");
+                       offset += j;
+                       for (i = 0; i < dlc; i++)
+                               if ((cf->data[i] > 0x1F) && (cf->data[i] < 0x7F))
+                                       buf[offset++] = cf->data[i];
+                               else
+                                       buf[offset++] = '.';
+
+                       sprintf(buf+offset, "'");
+               }
        } 
 }