Merge 0cfff2e5da into 95aae6bf83

isotpsniffer.c

@@ -59,6 +59,7 @@
 #include <linux/can.h>
 #include <linux/can/isotp.h>
 #include <linux/sockios.h>
+#include <errno.h>
 
 #define NO_CAN_ID 0xFFFFFFFFU
 
@@ -66,6 +67,8 @@
 #define FORMAT_ASCII 2
 #define FORMAT_DEFAULT (FORMAT_ASCII | FORMAT_HEX)
 
+#define PDU_BUF_SIZE 4096
+
 void print_usage(char *prg)
 {
 	fprintf(stderr, "\nUsage: %s [options] <CAN interface>\n", prg);
@@ -79,6 +82,7 @@ void print_usage(char *prg)
 	fprintf(stderr, "         -f <format>  (1 = HEX, 2 = ASCII, 3 = HEX & ASCII - default: %d)\n", FORMAT_DEFAULT);
 	fprintf(stderr, "         -L           (set link layer options for CAN FD)\n");
 	fprintf(stderr, "         -h <len>     (head: print only first <len> bytes)\n");
+	fprintf(stderr, "         -i           (ignore syscall errors to receive malformed PDUs)\n");
 	fprintf(stderr, "\nCAN IDs and addresses are given and expected in hexadecimal values.\n");
 	fprintf(stderr, "\n");
 }
@@ -189,15 +193,16 @@ int main(int argc, char **argv)
 	int head = 0;
 	int timestamp = 0;
 	int format = FORMAT_DEFAULT;
+	int ignore_errors = 0;
 	canid_t src = NO_CAN_ID;
 	canid_t dst = NO_CAN_ID;
 	extern int optind, opterr, optopt;
 	static struct timeval tv, last_tv;
 
-	unsigned char buffer[4096];
+	unsigned char buffer[PDU_BUF_SIZE];
 	int nbytes;
 
-	while ((opt = getopt(argc, argv, "s:d:x:X:h:ct:f:L?")) != -1) {
+	while ((opt = getopt(argc, argv, "s:d:x:X:h:ct:f:L?i")) != -1) {
 		switch (opt) {
 		case 's':
 			src = strtoul(optarg, NULL, 16);
@@ -249,6 +254,10 @@ int main(int argc, char **argv)
 			}
 			break;
 
+		case 'i':
+			ignore_errors = 1;
+			break;
+
 		case '?':
 			print_usage(basename(argv[0]));
 			goto out;
@@ -367,33 +376,39 @@ int main(int argc, char **argv)
 		}
 
 		if (FD_ISSET(s, &rdfs)) {
-			nbytes = read(s, buffer, 4096);
+			nbytes = read(s, buffer, PDU_BUF_SIZE);
 			if (nbytes < 0) {
 				perror("read socket s");
 				r = 1;
-				goto out;
+				if(!ignore_errors)
+					goto out;
 			}
-			if (nbytes > 4095) {
+			if (nbytes > (PDU_BUF_SIZE - 1)) {
 				r = 1;
+				fprintf(stderr, "PDU length %d longer than PDU buffer: %s\n", nbytes, strerror(errno));
 				goto out;
 			}
-			printbuf(buffer, nbytes, color?2:0, timestamp, format,
+			if(nbytes > 0)
-				 &tv, &last_tv, dst, s, if_name, head);
+				printbuf(buffer, nbytes, color?2:0, timestamp, format,
+					 &tv, &last_tv, dst, s, if_name, head);
 		}
 
 		if (FD_ISSET(t, &rdfs)) {
-			nbytes = read(t, buffer, 4096);
+			nbytes = read(t, buffer, PDU_BUF_SIZE);
 			if (nbytes < 0) {
 				perror("read socket t");
 				r = 1;
-				goto out;
+				if(!ignore_errors)
+					goto out;
 			}
-			if (nbytes > 4095) {
+			if (nbytes > (PDU_BUF_SIZE - 1)) {
 				r = 1;
+				fprintf(stderr, "PDU length %d longer than PDU buffer: %s\n", nbytes, strerror(errno));
 				goto out;
 			}
-			printbuf(buffer, nbytes, color?1:0, timestamp, format,
+			if(nbytes > 0)
-				 &tv, &last_tv, src, t, if_name, head);
+				printbuf(buffer, nbytes, color?1:0, timestamp, format,
+					 &tv, &last_tv, src, t, if_name, head);
 		}
 	}
 

lib.c

@@ -170,23 +170,21 @@ int parse_canframe(char *cs, union cfu *cu)
 
 	memset(cu, 0, sizeof(*cu)); /* init CAN CC/FD/XL frame, e.g. LEN = 0 */
 
-	if (len >= 4 && cs[3] == CANID_DELIM) { /* 3 digits SFF CAN ID */
+	if (len < 4)
+		return 0;
 
-		idx = 4; /* start of frame data */
+	if (cs[3] == CANID_DELIM) { /* 3 digits SFF */
 
-		/* get 3 digits SFF CAN ID value */
+		idx = 4;
 		for (i = 0; i < 3; i++) {
 			if ((tmp = asc2nibble(cs[i])) > 0x0F)
 				return 0;
 			cu->cc.can_id |= tmp << (2 - i) * 4;
 		}
 
-	} else if (len >= 21 && cs[5] == CANID_DELIM && cs[20] == CANID_DELIM) {
+	} else if (cs[5] == CANID_DELIM) { /* 5 digits CAN XL VCID/PRIO*/
-		/* 5 digits CAN XL VCID/PRIO - but also check for 2nd '#' here */
 
-		idx = 6; /* start of CAN XL frame extra content (AF, SDT, etc) */
+		idx = 6;
-
-		/* get 5 digits CAN XL VCID/PRIO */
 		for (i = 0; i < 5; i++) {
 			if ((tmp = asc2nibble(cs[i])) > 0x0F)
 				return 0;
@@ -198,11 +196,9 @@ int parse_canframe(char *cs, union cfu *cu)
 		cu->xl.prio &= CANXL_PRIO_MASK;
 		cu->xl.prio |= tmp;
 
-	} else if (len >= 9 && cs[8] == CANID_DELIM) { /* 8 digits EFF CAN ID */
+	} else if (cs[8] == CANID_DELIM) { /* 8 digits EFF */
 
-		idx = 9; /* start of frame data */
+		idx = 9;
-
-		/* get 8 digits EFF CAN ID value */
 		for (i = 0; i < 8; i++) {
 			if ((tmp = asc2nibble(cs[i])) > 0x0F)
 				return 0;
@@ -243,12 +239,11 @@ int parse_canframe(char *cs, union cfu *cu)
 		cu->fd.flags |= CANFD_FDF; /* dual-use */
 		idx += 2;
 
-	} else if (idx == 6) { /* CAN XL frame extra content '#80:00:11223344#' */
+	} else if (cs[idx + 14] == CANID_DELIM) { /* CAN XL frame '#80:00:11223344#' */
 		maxdlen = CANXL_MAX_DLEN;
 		mtu = CANXL_MTU;
-		data = cu->xl.data; /* overwrite pointer to CAN XL data */
+		data = cu->xl.data; /* fill CAN XL data */
 
-		/* get CAN XL frame extra content */
 		if ((cs[idx + 2] != XL_HDR_DELIM) || (cs[idx + 5] != XL_HDR_DELIM))
 			return 0;
 
@@ -282,7 +277,6 @@ int parse_canframe(char *cs, union cfu *cu)
 		idx++; /* skip CANID_DELIM */
 	}
 
-	/* copy CAN frame data content */
 	for (i = 0, dlen = 0; i < maxdlen; i++) {
 		if (cs[idx] == DATA_SEPERATOR) /* skip (optional) separator */
 			idx++;