cangen: add initial CAN XL support

TODO:
- Test all features
- make CAN XL content variable

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

cangen.c

@@ -177,9 +177,10 @@ static void print_usage(char *prg)
 	fprintf(stderr, "         -f            (generate CAN FD CAN frames)\n");
 	fprintf(stderr, "         -b            (generate CAN FD CAN frames with bitrate switch (BRS))\n");
 	fprintf(stderr, "         -E            (generate CAN FD CAN frames with error state (ESI))\n");
+	fprintf(stderr, "         -X            (generate CAN XL CAN frames)\n");
 	fprintf(stderr, "         -R            (generate RTR frames)\n");
 	fprintf(stderr, "         -8            (allow DLC values greater then 8 for Classic CAN frames)\n");
-	fprintf(stderr, "         -m            (mix -e -f -b -E -R frames)\n");
+	fprintf(stderr, "         -m            (mix -e -f -b -E -R -X frames)\n");
 	fprintf(stderr, "         -I <mode>     (CAN ID generation mode - see below)\n");
 	fprintf(stderr, "         -L <mode>     (CAN data length code (dlc) generation mode - see below)\n");
 	fprintf(stderr, "         -D <mode>     (CAN data (payload) generation mode - see below)\n");
@@ -298,7 +299,6 @@ static int do_send_one(int fd, cu_t *cu, size_t len, int timeout)
 	uint8_t control[CMSG_SPACE(sizeof(uint64_t))] = { 0 };
 	struct iovec iov = {
 		.iov_base = cu,
-		.iov_len = len,
 	};
 	struct msghdr msg = {
 		.msg_iov = &iov,
@@ -307,6 +307,12 @@ static int do_send_one(int fd, cu_t *cu, size_t len, int timeout)
 	ssize_t nbytes;
 	int ret;
 
+	/* CAN XL frames need real frame length for sending */
+	if (len == CANXL_MTU)
+		len = CANXL_HDR_SIZE + cu->xl.len;
+
+	iov.iov_len = len;
+
 	if (use_so_txtime) {
 		struct cmsghdr *cm;
 		uint64_t tdeliver;
@@ -443,6 +449,7 @@ int main(int argc, char **argv)
 	unsigned long polltimeout = 0;
 	unsigned char extended = 0;
 	unsigned char canfd = 0;
+	unsigned char canxl = 0;
 	unsigned char brs = 0;
 	unsigned char esi = 0;
 	unsigned char mix = 0;
@@ -458,6 +465,7 @@ int main(int argc, char **argv)
 	unsigned long burst_sent_count = 0;
 	int mtu, maxdlen;
 	uint64_t incdata = 0;
+	__u8 *data; /* base pointer for CC/FD or XL data */
 	int incdlc = 0;
 	unsigned long rnd;
 	unsigned char fixdata[CANFD_MAX_DLEN];
@@ -467,9 +475,13 @@ int main(int argc, char **argv)
 	int s; /* socket */
 
 	struct sockaddr_can addr = { 0 };
+	struct can_raw_vcid_options vcid_opts = {
+		.flags = CAN_RAW_XL_VCID_TX_PASS,
+	};
 	static cu_t cu;
 	int i;
-	struct ifreq ifr;
+	static struct ifreq ifr;
+	const int enable_canfx = 1;
 
 	struct timeval now;
 	int ret;
@@ -488,7 +500,7 @@ int main(int argc, char **argv)
 		{ 0,		0,			0, 0 },
 	};
 
-	while ((opt = getopt_long(argc, argv, "g:atefbER8mI:L:D:p:n:ixc:vh?", long_options, NULL)) != -1) {
+	while ((opt = getopt_long(argc, argv, "g:atefbEXR8mI:L:D:p:n:ixc:vh?", long_options, NULL)) != -1) {
 		switch (opt) {
 		case 'g':
 			gap = strtod(optarg, NULL);
@@ -533,6 +545,10 @@ int main(int argc, char **argv)
 			canfd = 1;
 			break;
 
+		case 'X':
+			canxl = 1;
+			break;
+
 		case 'R':
 			rtr_frame = 1;
 			break;
@@ -676,8 +692,7 @@ int main(int argc, char **argv)
 			   &loopback, sizeof(loopback));
 	}
 
-	if (canfd) {
+	if (canfd || canxl) {
-		const int enable_canfd = 1;
 
 		/* check if the frame fits into the CAN netdevice */
 		if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
@@ -685,19 +700,49 @@ int main(int argc, char **argv)
 			return 1;
 		}
 
-		if (ifr.ifr_mtu != CANFD_MTU && ifr.ifr_mtu != CANXL_MTU) {
+		if (canfd) {
-			printf("CAN interface is only Classical CAN capable - sorry.\n");
+			/* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */
+			cu.fd.len = can_fd_dlc2len(can_fd_len2dlc(cu.fd.len));
+		} else {
+			/* limit fixed CAN XL data length to 64 */
+			if (cu.fd.len > CANFD_MAX_DLEN)
+				cu.fd.len = CANFD_MAX_DLEN;
+		}
+
+		if (canxl && (ifr.ifr_mtu < CANXL_MIN_MTU)) {
+			printf("CAN interface not CAN XL capable - sorry.\n");
 			return 1;
 		}
 
+		if (canfd && (ifr.ifr_mtu < CANFD_MTU)) {
+			printf("CAN interface not CAN FD capable - sorry.\n");
+			return 1;
+		}
+
+		if (ifr.ifr_mtu == CANFD_MTU) {
 			/* interface is ok - try to switch the socket into CAN FD mode */
-		if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd))) {
+			if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES,
+				       &enable_canfx, sizeof(enable_canfx))){
 				printf("error when enabling CAN FD support\n");
 				return 1;
 			}
+		}
+
+		if (ifr.ifr_mtu >= CANXL_MIN_MTU) {
+			/* interface is ok - try to switch the socket into CAN XL mode */
+			if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_FRAMES,
+				       &enable_canfx, sizeof(enable_canfx))){
+				printf("error when enabling CAN XL support\n");
+				return 1;
+			}
+			/* try to enable the CAN XL VCID pass through mode */
+			if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_VCID_OPTS,
+				       &vcid_opts, sizeof(vcid_opts))) {
+				printf("error when enabling CAN XL VCID pass through\n");
+				return 1;
+			}
+		}
 
-		/* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */
-		cu.fd.len = can_fd_dlc2len(can_fd_len2dlc(cu.fd.len));
 	} else {
 		/* sanitize Classical CAN 2.0 frame length */
 		if (len8_dlc) {
@@ -733,10 +778,16 @@ int main(int argc, char **argv)
 		if (count && (--count == 0))
 			running = 0;
 
-		if (canfd) {
+		if (canxl) {
+			mtu = CANXL_MTU;
+			maxdlen = CANFD_MAX_DLEN; /* generate up to 64 byte */
+			extended = 0; /* prio has only 11 bit ID content */
+			data = cu.xl.data; /* fill CAN XL data */
+		} else if (canfd) {
 			mtu = CANFD_MTU;
 			maxdlen = CANFD_MAX_DLEN;
-			cu.fd.flags |= CANFD_FDF;
+			data = cu.fd.data; /* fill CAN CC/FD data */
+			cu.fd.flags = CANFD_FDF;
 			if (brs)
 				cu.fd.flags |= CANFD_BRS;
 			if (esi)
@@ -744,6 +795,7 @@ int main(int argc, char **argv)
 		} else {
 			mtu = CAN_MTU;
 			maxdlen = CAN_MAX_DLEN;
+			data = cu.cc.data; /* fill CAN CC/FD data */
 		}
 
 		if (id_mode == MODE_RANDOM)
@@ -759,11 +811,13 @@ int main(int argc, char **argv)
 		} else
 			cu.fd.can_id &= CAN_SFF_MASK;
 
-		if (rtr_frame && !canfd)
+		if (rtr_frame && !canfd && !canxl)
 			cu.fd.can_id |= CAN_RTR_FLAG;
 
 		if (dlc_mode == MODE_RANDOM) {
-			if (canfd)
+			if (canxl)
+				cu.fd.len = CANXL_MIN_DLEN + (random() & 0x3F);
+			else if (canfd)
 				cu.fd.len = can_fd_dlc2len(random() & 0xF);
 			else {
 				cu.cc.len = random() & 0xF;
@@ -785,40 +839,40 @@ int main(int argc, char **argv)
 
 		if (data_mode == MODE_RANDOM) {
 			rnd = random();
-			memcpy(&cu.cc.data[0], &rnd, 4);
+			memcpy(&data[0], &rnd, 4);
 			rnd = random();
-			memcpy(&cu.cc.data[4], &rnd, 4);
+			memcpy(&data[4], &rnd, 4);
 
 			/* omit extra random number generation for CAN FD */
-			if (canfd && cu.fd.len > 8) {
+			if ((canfd || canxl) && cu.fd.len > 8) {
-				memcpy(&cu.fd.data[8], &cu.fd.data[0], 8);
+				memcpy(&data[8], &data[0], 8);
-				memcpy(&cu.fd.data[16], &cu.fd.data[0], 16);
+				memcpy(&data[16], &data[0], 16);
-				memcpy(&cu.fd.data[32], &cu.fd.data[0], 32);
+				memcpy(&data[32], &data[0], 32);
 			}
 		}
 
 		if (data_mode == MODE_RANDOM_FIX) {
 			int i;
 
-			memcpy(cu.fd.data, fixdata, CANFD_MAX_DLEN);
+			memcpy(data, fixdata, CANFD_MAX_DLEN);
 
 			for (i = 0; i < cu.fd.len; i++) {
 				if (rand_position[i] == (NIBBLE_H | NIBBLE_L)) {
-					cu.fd.data[i] = random();
+					data[i] = random();
 				} else if (rand_position[i] == NIBBLE_H) {
-					cu.fd.data[i] = (cu.fd.data[i] & 0x0f) | (random() & 0xf0);
+					data[i] = (data[i] & 0x0f) | (random() & 0xf0);
 				} else if (rand_position[i] == NIBBLE_L) {
-					cu.fd.data[i] = (cu.fd.data[i] & 0xf0) | (random() & 0x0f);
+					data[i] = (data[i] & 0xf0) | (random() & 0x0f);
 				}
 			}
 		}
 
 		if (data_mode == MODE_FIX)
-			memcpy(cu.fd.data, fixdata, CANFD_MAX_DLEN);
+			memcpy(data, fixdata, CANFD_MAX_DLEN);
 
 		/* set unused payload data to zero like the CAN driver does it on rx */
 		if (cu.fd.len < maxdlen)
-			memset(&cu.fd.data[cu.fd.len], 0, maxdlen - cu.fd.len);
+			memset(&data[cu.fd.len], 0, maxdlen - cu.fd.len);
 
 		if (!use_so_txtime &&
 		    (ts.tv_sec || ts.tv_nsec) &&
@@ -833,6 +887,21 @@ int main(int argc, char **argv)
 			}
 		}
 
+		if (canxl) {
+			/* convert some CAN FD frame content into a CAN XL frame */
+			if (cu.fd.len < CANXL_MIN_DLEN) {
+				cu.fd.len = CANXL_MIN_DLEN;
+				data[0] = 0xCC; /* default filler */
+			}
+			cu.xl.len = cu.fd.len;
+			cu.xl.flags = CANXL_XLF;
+
+			/* static values for now */
+			cu.xl.sdt = 0x22;
+			cu.xl.af = 0x12345678;
+			cu.xl.prio |= (0x33 << CANXL_VCID_OFFSET);
+		}
+
 		if (verbose) {
 			static char afrbuf[AFRSZ]; /* ASCII CAN frame buffer size */
 
@@ -854,6 +923,11 @@ int main(int argc, char **argv)
 			burst_sent_count = 0;
 		burst_sent_count++;
 
+		if (canxl) {
+			/* restore some CAN FD frame content from CAN XL frame */
+			cu.fd.len = cu.xl.len;
+		}
+
 		if (id_mode == MODE_INCREMENT)
 			cu.cc.can_id++;
 
@@ -861,7 +935,7 @@ int main(int argc, char **argv)
 			incdlc++;
 			incdlc %= CAN_MAX_RAW_DLC + 1;
 
-			if (canfd && !mix)
+			if ((canfd || canxl) && !mix)
 				cu.fd.len = can_fd_dlc2len(incdlc);
 			else if (len8_dlc) {
 				if (incdlc > CAN_MAX_DLEN) {
@@ -881,7 +955,7 @@ int main(int argc, char **argv)
 			incdata++;
 
 			for (i = 0; i < 8; i++)
-				cu.cc.data[i] = incdata >> i * 8;
+				data[i] = incdata >> i * 8;
 		}
 
 		if (mix) {
@@ -892,6 +966,10 @@ int main(int argc, char **argv)
 				brs = i & 4;
 				esi = i & 8;
 			}
+			/* generate CAN XL traffic if the interface is capable */
+			if (ifr.ifr_mtu >= CANXL_MIN_MTU)
+				canxl = ((i & 96) == 96);
+
 			rtr_frame = ((i & 24) == 24); /* reduce RTR frames to 1/4 */
 		}
 	}