canfdtest: adds support for custom CAN IDs

This change enables the use of this app alongside the traffic
typically found on a CAN bus.

canfdtest.c

@@ -4,6 +4,7 @@
  *
  * (C) 2009 by Vladislav Gribov, IXXAT Automation GmbH, <gribov@ixxat.de>
  * (C) 2009 Wolfgang Grandegger <wg@grandegger.com>
+ * (C) 2021 Jean Gressmann, IAV GmbH, <jean.steven.gressmann@iav.de>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the version 2 of the GNU General Public License
@@ -39,7 +40,8 @@
 #include <linux/can.h>
 #include <linux/can/raw.h>
 
-#define CAN_MSG_ID	0x77
+#define CAN_MSG_ID_PING	0x77
+#define CAN_MSG_ID_PONG	0x78
 #define CAN_MSG_LEN	8
 #define CAN_MSG_COUNT	50
 #define CAN_MSG_WAIT	27
@@ -50,6 +52,9 @@ static int sockfd;
 static int test_loops;
 static int exit_sig;
 static int inflight_count = CAN_MSG_COUNT;
+static canid_t can_id_ping = CAN_MSG_ID_PING;
+static canid_t can_id_pong = CAN_MSG_ID_PONG;
+static int has_pong_id = 0;
 
 static void print_usage(char *prg)
 {
@@ -60,9 +65,12 @@ static void print_usage(char *prg)
 		"Options:\n"
 		"         -f COUNT (number of frames in flight, default: %d)\n"
 		"         -g       (generate messages)\n"
+		"         -i ID    (CAN ID to use for frames to DUT (ping), default %x)\n"
 		"         -l COUNT (test loop count)\n"
+		"         -o ID    (CAN ID to use for frames to host (pong), default %x)\n"
 		"         -v       (low verbosity)\n"
 		"         -vv      (high verbosity)\n"
+		"         -x       (ignore other frames on bus)\n"
 		"\n"
 		"With the option '-g' CAN messages are generated and checked\n"
 		"on <can-interface>, otherwise all messages received on the\n"
@@ -74,52 +82,62 @@ static void print_usage(char *prg)
 		"%s -v can0\n"
 		"\ton Host:\n"
 		"%s -g -v can2\n",
-		prg, prg, CAN_MSG_COUNT, prg, prg);
+		prg, prg, CAN_MSG_COUNT, CAN_MSG_ID_PING, CAN_MSG_ID_PONG, prg, prg);
 
 	exit(1);
 }
 
-static void print_frame(const struct can_frame *frame, int inc)
+static void print_frame(canid_t id, const uint8_t *data, int dlc, int inc_data)
 {
 	int i;
 
-	printf("%04x: ", frame->can_id + inc);
+	printf("%04x: ", id);
-	if (frame->can_id & CAN_RTR_FLAG) {
+	if (id & CAN_RTR_FLAG) {
 		printf("remote request");
 	} else {
-		printf("[%d]", frame->can_dlc);
+		printf("[%d]", dlc);
-		for (i = 0; i < frame->can_dlc; i++)
+		for (i = 0; i < dlc; i++)
-			printf(" %02x", (uint8_t)(frame->data[i] + inc));
+			printf(" %02x", (uint8_t)(data[i] + inc_data));
 	}
 	printf("\n");
 }
 
-static void print_compare(const struct can_frame *exp, const struct can_frame *rec, int inc)
+static void print_compare(
+	canid_t exp_id,
+	const uint8_t *exp_data,
+	uint8_t exp_dlc,
+	canid_t rec_id,
+	const uint8_t *rec_data,
+	uint8_t rec_dlc,
+	int inc)
 {
 	printf("expected: ");
-	print_frame(exp, inc);
+	print_frame(exp_id, exp_data, exp_dlc, inc);
 	printf("received: ");
-	print_frame(rec, 0);
+	print_frame(rec_id, rec_data, rec_dlc, 0);
 }
 
-static void compare_frame(struct can_frame *exp, struct can_frame *rec, int inc)
+static void compare_frame(const struct can_frame *exp, const struct can_frame *rec, int inc)
 {
 	int i;
+	const canid_t expected_can_id = inc ? can_id_pong : can_id_ping;
 
-	if (rec->can_id != exp->can_id + inc) {
+	if (rec->can_id != expected_can_id) {
 		printf("Message ID mismatch!\n");
-		print_compare(exp, rec, inc);
+		print_compare(expected_can_id, exp->data, exp->can_dlc,
+		              rec->can_id, rec->data, rec->can_dlc, inc);
 		running = 0;
 	} else if (rec->can_dlc != exp->can_dlc) {
 		printf("Message length mismatch!\n");
-		print_compare(exp, rec, inc);
+		print_compare(expected_can_id, exp->data, exp->can_dlc,
+		              rec->can_id, rec->data, rec->can_dlc, inc);
 		running = 0;
 	} else {
 		for (i = 0; i < rec->can_dlc; i++) {
 			if (rec->data[i] != (uint8_t)(exp->data[i] + inc)) {
 				printf("Databyte %x mismatch!\n", i);
-				print_compare(exp,
+				print_compare(expected_can_id, exp->data, exp->can_dlc,
-					      rec, inc);
+				              rec->can_id, rec->data, rec->can_dlc, inc);
 				running = 0;
 			}
 		}
@@ -202,20 +220,20 @@ static int check_frame(const struct can_frame *frame)
 	int err = 0;
 	int i;
 
-	if (frame->can_id != CAN_MSG_ID) {
+	if (frame->can_id != can_id_ping) {
-		printf("unexpected Message ID 0x%04x!\n", frame->can_id);
+		printf("Unexpected Message ID 0x%04x!\n", frame->can_id);
 		err = -1;
 	}
 
 	if (frame->can_dlc != CAN_MSG_LEN) {
-		printf("unexpected Message length %d!\n", frame->can_dlc);
+		printf("Unexpected Message length %d!\n", frame->can_dlc);
 		err = -1;
 	}
 
 	for (i = 1; i < frame->can_dlc; i++) {
 		if (frame->data[i] != (uint8_t)(frame->data[i-1] + 1)) {
 			printf("Frame inconsistent!\n");
-			print_frame(frame, 0);
+			print_frame(frame->can_id, frame->data, frame->can_dlc, 0);
 			err = -1;
 			goto out;
 		}
@@ -229,7 +247,11 @@ static void inc_frame(struct can_frame *frame)
 {
 	int i;
 
-	frame->can_id++;
+	if (has_pong_id)
+		frame->can_id = can_id_pong;
+	else
+		frame->can_id++;
+
 	for (i = 0; i < frame->can_dlc; i++)
 		frame->data[i]++;
 }
@@ -246,7 +268,7 @@ static int can_echo_dut(void)
 		if (verbose == 1) {
 			echo_progress(frame.data[0]);
 		} else if (verbose > 1) {
-			print_frame(&frame, 0);
+			print_frame(frame.can_id, frame.data, frame.can_dlc, 0);
 		}
 
 		check_frame(&frame);
@@ -291,7 +313,7 @@ static int can_echo_gen(void)
 		if (unprocessed < inflight_count) {
 			/* still send messages */
 			tx_frames[send_pos].can_dlc = CAN_MSG_LEN;
-			tx_frames[send_pos].can_id = CAN_MSG_ID;
+			tx_frames[send_pos].can_id = can_id_ping;
 			recv_tx[send_pos] = 0;
 
 			for (i = 0; i < CAN_MSG_LEN; i++)
@@ -320,10 +342,10 @@ static int can_echo_gen(void)
 			}
 
 			if (verbose > 1)
-				print_frame(&rx_frame, 0);
+				print_frame(rx_frame.can_id, rx_frame.data, rx_frame.can_dlc, 0);
 
 			/* own frame */
-			if (rx_frame.can_id == CAN_MSG_ID) {
+			if (rx_frame.can_id == can_id_ping) {
 				compare_frame(&tx_frames[recv_tx_pos], &rx_frame, 0);
 				recv_tx[recv_tx_pos] = 1;
 				recv_tx_pos++;
@@ -334,7 +356,7 @@ static int can_echo_gen(void)
 
 			if (!recv_tx[recv_rx_pos]) {
 				printf("RX before TX!\n");
-				print_frame(&rx_frame, 0);
+				print_frame(rx_frame.can_id, rx_frame.data, rx_frame.can_dlc, 0);
 				running = 0;
 			}
 			/* compare with expected */
@@ -369,12 +391,13 @@ int main(int argc, char *argv[])
 	int echo_gen = 0;
 	int opt, err;
 	int recv_own_msgs = 1;
+	int filter = 0;
 
 	signal(SIGTERM, signal_handler);
 	signal(SIGHUP, signal_handler);
 	signal(SIGINT, signal_handler);
 
-	while ((opt = getopt(argc, argv, "f:gl:v?")) != -1) {
+	while ((opt = getopt(argc, argv, "f:gi:l:o:vx?")) != -1) {
 		switch (opt) {
 		case 'f':
 			inflight_count = atoi(optarg);
@@ -384,14 +407,27 @@ int main(int argc, char *argv[])
 			echo_gen = 1;
 			break;
 
+		case 'i':
+			can_id_ping = strtoul(optarg, NULL, 16) & CAN_SFF_MASK;
+			break;
+
 		case 'l':
 			test_loops = atoi(optarg);
 			break;
 
+		case 'o':
+			can_id_pong = strtoul(optarg, NULL, 16) & CAN_SFF_MASK;
+			has_pong_id = 1;
+			break;
+
 		case 'v':
 			verbose++;
 			break;
 
+		case 'x':
+			filter = 1;
+			break;
+
 		case '?':
 		default:
 			print_usage(basename(argv[0]));
@@ -433,6 +469,29 @@ int main(int argc, char *argv[])
 		return 1;
 	}
 
+	if (!has_pong_id)
+		can_id_pong = can_id_ping + 1;
+
+	if (filter) {
+		const struct can_filter filters[] = {
+			{
+				.can_id = can_id_ping,
+				.can_mask = CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_SFF_MASK,
+			},
+			{
+				.can_id = can_id_pong,
+				.can_mask = CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_SFF_MASK,
+			},
+		};
+
+		if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, filters,
+					   sizeof(struct can_filter) * (1 + echo_gen))) {
+			perror("setsockopt()");
+			close(sockfd);
+			return 1;
+		}
+	}
+
 	if (echo_gen)
 		err = can_echo_gen();
 	else