From: Oliver Hartkopp <socketcan@hartkopp.net>
Date: Sun, 16 Nov 2014 21:24:04 +0000 (+0100)
Subject: isotp: add tool support for ISO 15765-2:2015 with CAN FD
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/can-utils.git/commitdiff_plain/8150e21a1129c4b822d73186cd63e0e9fbf4c32d

isotp: add tool support for ISO 15765-2:2015 with CAN FD

Signed-off-by: Oliver Hartkopp <socketcan@hartkopp.net>
---

diff --git a/include/linux/can/isotp.h b/include/linux/can/isotp.h
index 0381506..f074329 100644
--- a/include/linux/can/isotp.h
+++ b/include/linux/can/isotp.h
@@ -66,6 +66,8 @@
 					/* ignore received CF frames which */
 					/* timestamps differ less than val */
 
+#define CAN_ISOTP_LL_OPTS	5	/* pass struct can_isotp_ll_options */
+
 struct can_isotp_options {
 
 	__u32 flags;		/* set flags for isotp behaviour.	*/
@@ -103,6 +105,24 @@ struct can_isotp_fc_options {
 				/* __u8 value : 0 = omit FC N_PDU WT	*/
 };
 
+struct can_isotp_ll_options {
+
+	__u8  mtu;		/* generated & accepted CAN frame type	*/
+				/* __u8 value :				*/
+				/* CAN_MTU   (16) -> standard CAN 2.0	*/
+				/* CANFD_MTU (72) -> CAN FD frame	*/
+
+	__u8  tx_dl;		/* tx link layer data length in bytes	*/
+				/* (configured maximum payload length)	*/
+				/* __u8 value : 8,12,16,20,24,32,48,64	*/
+				/* => rx path supports all LL_DL values */
+
+	__u8  tx_flags;		/* set into struct canfd_frame.flags	*/
+				/* at frame creation: e.g. CANFD_BRS	*/
+				/* Obsolete when the BRS flag is fixed	*/
+				/* by the CAN netdriver configuration	*/
+};
+
 
 /* flags for isotp behaviour */
 
@@ -122,13 +142,16 @@ struct can_isotp_fc_options {
 
 #define CAN_ISOTP_DEFAULT_FLAGS		0
 #define CAN_ISOTP_DEFAULT_EXT_ADDRESS	0x00
-#define CAN_ISOTP_DEFAULT_RXPAD_CONTENT	0x00
-#define CAN_ISOTP_DEFAULT_TXPAD_CONTENT	0x00
+#define CAN_ISOTP_DEFAULT_PAD_CONTENT	0xCC /* prevent bit-stuffing */
 #define CAN_ISOTP_DEFAULT_FRAME_TXTIME	0
 #define CAN_ISOTP_DEFAULT_RECV_BS	0
 #define CAN_ISOTP_DEFAULT_RECV_STMIN	0x00
 #define CAN_ISOTP_DEFAULT_RECV_WFTMAX	0
 
+#define CAN_ISOTP_DEFAULT_LL_MTU	CAN_MTU
+#define CAN_ISOTP_DEFAULT_LL_TX_DL	CAN_MAX_DLEN
+#define CAN_ISOTP_DEFAULT_LL_TX_FLAGS	0
+
 /*
  * Remark on CAN_ISOTP_DEFAULT_RECV_* values:
  *
diff --git a/isotpdump.c b/isotpdump.c
index 85be399..c5e9113 100644
--- a/isotpdump.c
+++ b/isotpdump.c
@@ -60,6 +60,7 @@
 #define NO_CAN_ID 0xFFFFFFFFU
 
 const char fc_info [4][9] = { "CTS", "WT", "OVFLW", "reserved" };
+const int canfd_on = 1;
 
 void print_usage(char *prg)
 {
@@ -80,7 +81,7 @@ int main(int argc, char **argv)
 	int s;
 	struct sockaddr_can addr;
 	struct can_filter rfilter[2];
-	struct can_frame frame;
+	struct canfd_frame frame;
 	int nbytes, i;
 	canid_t src = NO_CAN_ID;
 	canid_t dst = NO_CAN_ID;
@@ -180,6 +181,8 @@ int main(int argc, char **argv)
 		return 1;
 	}
 
+	/* try to switch the socket into CAN FD mode */
+	setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on));
 
 	if (src & CAN_EFF_FLAG) {
 		rfilter[0].can_id   = src & (CAN_EFF_MASK | CAN_EFF_FLAG);
@@ -212,12 +215,12 @@ int main(int argc, char **argv)
 	}
 
 	while (1) {
-
-		if ((nbytes = read(s, &frame, sizeof(struct can_frame))) < 0) {
+		nbytes = read(s, &frame, sizeof(frame));
+		if (nbytes < 0) {
 			perror("read");
 			return 1;
-		} else if (nbytes < sizeof(struct can_frame)) {
-			fprintf(stderr, "read: incomplete CAN frame\n");
+		} else if (nbytes != CAN_MTU && nbytes != CANFD_MTU) {
+			fprintf(stderr, "read: incomplete CAN frame %lu %d\n", sizeof(frame), nbytes);
 			return 1;
 		} else {
 
@@ -284,15 +287,23 @@ int main(int argc, char **argv)
 			if (ext)
 				printf("{%02X}", frame.data[0]);
 
-			printf("  [%d]  ", frame.can_dlc);
+			if (nbytes == CAN_MTU)
+				printf("  [%d]  ", frame.len);
+			else
+				printf(" [%02d]  ", frame.len);
 
 			datidx = 0;
 			n_pci = frame.data[ext];
 	    
 			switch (n_pci & 0xF0) {
 			case 0x00:
-				printf("[SF] ln: %-4d data:", n_pci & 0x0F);
-				datidx = ext+1;
+				if (n_pci & 0xF) {
+					printf("[SF] ln: %-4d data:", n_pci & 0xF);
+					datidx = ext+1;
+				} else {
+					printf("[SF] ln: %-4d data:", frame.data[ext + 1]);
+					datidx = ext+2;
+				}
 				break;
 
 			case 0x10:
@@ -341,16 +352,16 @@ int main(int argc, char **argv)
 				printf("[??]");
 			}
 
-			if (datidx && frame.can_dlc > datidx) {
+			if (datidx && frame.len > datidx) {
 				printf(" ");
-				for (i = datidx; i < frame.can_dlc; i++) {
+				for (i = datidx; i < frame.len; i++) {
 					printf("%02X ", frame.data[i]);
 				}
 
 				if (asc) {
-					printf("%*s", ((7-ext) - (frame.can_dlc-datidx))*3 + 5 ,
+					printf("%*s", ((7-ext) - (frame.len-datidx))*3 + 5 ,
 					       "-  '");
-					for (i = datidx; i < frame.can_dlc; i++) {
+					for (i = datidx; i < frame.len; i++) {
 						printf("%c",((frame.data[i] > 0x1F) &&
 							     (frame.data[i] < 0x7F))?
 						       frame.data[i] : '.');
diff --git a/isotprecv.c b/isotprecv.c
index a0934ba..f4b8610 100644
--- a/isotprecv.c
+++ b/isotprecv.c
@@ -72,6 +72,7 @@ void print_usage(char *prg)
 	fprintf(stderr, "         -f <time ns> (force rx stmin value in nanosecs)\n");
 	fprintf(stderr, "         -w <num>     (max. wait frame transmissions.)\n");
 	fprintf(stderr, "         -l           (loop: do not exit after pdu receiption.)\n");
+	fprintf(stderr, "         -L <mtu>:<tx_dl>:<tx_flags> (link layer options for CAN FD)\n");
 	fprintf(stderr, "\nCAN IDs and addresses are given and expected in hexadecimal values.\n");
 	fprintf(stderr, "The pdu data is written on STDOUT in space separated ASCII hex values.\n");
 	fprintf(stderr, "\n");
@@ -84,6 +85,7 @@ int main(int argc, char **argv)
     struct ifreq ifr;
     static struct can_isotp_options opts;
     static struct can_isotp_fc_options fcopts;
+    static struct can_isotp_ll_options llopts;
     int opt, i;
     extern int optind, opterr, optopt;
     __u32 force_rx_stmin = 0;
@@ -94,7 +96,7 @@ int main(int argc, char **argv)
 
     addr.can_addr.tp.tx_id = addr.can_addr.tp.rx_id = NO_CAN_ID;
 
-    while ((opt = getopt(argc, argv, "s:d:x:X:p:P:b:m:w:f:l?")) != -1) {
+    while ((opt = getopt(argc, argv, "s:d:x:X:p:P:b:m:w:f:lL:?")) != -1) {
 	    switch (opt) {
 	    case 's':
 		    addr.can_addr.tp.tx_id = strtoul(optarg, (char **)NULL, 16);
@@ -158,6 +160,17 @@ int main(int argc, char **argv)
 		    loop = 1;
 		    break;
 
+	    case 'L':
+		    if (sscanf(optarg, "%hhu:%hhu:%hhu",
+			       &llopts.mtu,
+			       &llopts.tx_dl,
+			       &llopts.tx_flags) != 3) {
+			    printf("unknown link layer options '%s'.\n", optarg);
+			    print_usage(basename(argv[0]));
+			    exit(0);
+		    }
+		    break;
+
 	    case '?':
 		    print_usage(basename(argv[0]));
 		    exit(0);
@@ -191,6 +204,13 @@ int main(int argc, char **argv)
     setsockopt(s, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
     setsockopt(s, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts));
 
+    if (llopts.tx_dl) {
+	if (setsockopt(s, SOL_CAN_ISOTP, CAN_ISOTP_LL_OPTS, &llopts, sizeof(llopts)) < 0) {
+	    perror("link layer sockopt");
+	    exit(1);
+	}
+    }
+
     if (opts.flags & CAN_ISOTP_FORCE_RXSTMIN)
 	    setsockopt(s, SOL_CAN_ISOTP, CAN_ISOTP_RX_STMIN, &force_rx_stmin, sizeof(force_rx_stmin));
 
diff --git a/isotpsend.c b/isotpsend.c
index 7b48ec2..2585bec 100644
--- a/isotpsend.c
+++ b/isotpsend.c
@@ -70,6 +70,7 @@ void print_usage(char *prg)
 	fprintf(stderr, "         -t <time ns> (frame transmit time (N_As) in nanosecs)\n");
 	fprintf(stderr, "         -f <time ns> (ignore FC and force local tx stmin value in nanosecs)\n");
 	fprintf(stderr, "         -D <len>     (send a fixed PDU with len bytes - no STDIN data)\n");
+	fprintf(stderr, "         -L <mtu>:<tx_dl>:<tx_flags> (link layer options for CAN FD)\n");
 	fprintf(stderr, "\nCAN IDs and addresses are given and expected in hexadecimal values.\n");
 	fprintf(stderr, "The pdu data is expected on STDIN in space separated ASCII hex values.\n");
 	fprintf(stderr, "\n");
@@ -81,6 +82,7 @@ int main(int argc, char **argv)
     struct sockaddr_can addr;
     struct ifreq ifr;
     static struct can_isotp_options opts;
+    static struct can_isotp_ll_options llopts;
     int opt;
     extern int optind, opterr, optopt;
     __u32 force_tx_stmin = 0;
@@ -91,7 +93,7 @@ int main(int argc, char **argv)
 
     addr.can_addr.tp.tx_id = addr.can_addr.tp.rx_id = NO_CAN_ID;
 
-    while ((opt = getopt(argc, argv, "s:d:x:X:p:P:t:f:D:?")) != -1) {
+    while ((opt = getopt(argc, argv, "s:d:x:X:p:P:t:f:D:L:?")) != -1) {
 	    switch (opt) {
 	    case 's':
 		    addr.can_addr.tp.tx_id = strtoul(optarg, (char **)NULL, 16);
@@ -151,6 +153,17 @@ int main(int argc, char **argv)
 		    }
 		    break;
 
+	    case 'L':
+		    if (sscanf(optarg, "%hhu:%hhu:%hhu",
+			       &llopts.mtu,
+			       &llopts.tx_dl,
+			       &llopts.tx_flags) != 3) {
+			    printf("unknown link layer options '%s'.\n", optarg);
+			    print_usage(basename(argv[0]));
+			    exit(0);
+		    }
+		    break;
+
 	    case '?':
 		    print_usage(basename(argv[0]));
 		    exit(0);
@@ -183,6 +196,13 @@ int main(int argc, char **argv)
 
     setsockopt(s, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
 
+    if (llopts.tx_dl) {
+	if (setsockopt(s, SOL_CAN_ISOTP, CAN_ISOTP_LL_OPTS, &llopts, sizeof(llopts)) < 0) {
+	    perror("link layer sockopt");
+	    exit(1);
+	}
+    }
+
     if (opts.flags & CAN_ISOTP_FORCE_TXSTMIN)
 	    setsockopt(s, SOL_CAN_ISOTP, CAN_ISOTP_TX_STMIN, &force_tx_stmin, sizeof(force_tx_stmin));