From bd835f2ca2929077da25dfc771dbe1cc312ce70d Mon Sep 17 00:00:00 2001
From: Marc Kleine-Budde <mkl@pengutronix.de>
Date: Wed, 29 Nov 2023 18:06:25 +0100
Subject: [PATCH] cansequence: add support to send and receive CAN-FD

---
 cansequence.c | 66 ++++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 60 insertions(+), 6 deletions(-)

diff --git a/cansequence.c b/cansequence.c
index 4e933ec..4e97879 100644
--- a/cansequence.c
+++ b/cansequence.c
@@ -1,5 +1,5 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
-// Copyright (c) 2007, 2008, 2009, 2010, 2014, 2015, 2019 Pengutronix,
+// Copyright (c) 2007, 2008, 2009, 2010, 2014, 2015, 2019, 2023 Pengutronix,
 //		 Marc Kleine-Budde <kernel@pengutronix.de>
 // Copyright (c) 2005 Pengutronix,
 //		 Sascha Hauer <kernel@pengutronix.de>
@@ -36,6 +36,7 @@ static int s = -1;
 static bool running = true;
 static volatile sig_atomic_t signal_num;
 static bool infinite = true;
+static bool canfd = false;
 static unsigned int drop_until_quit;
 static unsigned int drop_count;
 static bool use_poll = false;
@@ -43,8 +44,8 @@ static bool use_poll = false;
 static unsigned int loopcount = 1;
 static int verbose;
 
-static struct can_frame frame = {
-	.can_dlc = 1,
+static struct canfd_frame frame = {
+	.len = 1,
 };
 static struct can_filter filter[] = {
 	{
@@ -64,6 +65,8 @@ static void print_usage(char *prg)
 		"\n"
 		"Options:\n"
 		" -e, --extended       send/receive extended frames\n"
+		" -f, --canfd          send/receive CAN-FD CAN frames\n"
+		" -b, --brs            send CAN-FD CAN frames with bitrate switch (BRS)\n"
 		" -i, --identifier=ID  CAN Identifier (default = %u)\n"
 		"     --loop=COUNT     send message COUNT times\n"
 		" -p, --poll           use poll(2) to wait for buffer space while sending\n"
@@ -100,6 +103,12 @@ static void do_receive()
 	uint32_t sequence = 0;
 	unsigned int overflow_old = 0;
 	can_err_mask_t err_mask = CAN_ERR_MASK;
+	size_t mtu;
+
+	if (canfd)
+		mtu = CANFD_MTU;
+	else
+		mtu = CAN_MTU;
 
 	if (setsockopt(s, SOL_SOCKET, SO_RXQ_OVFL,
 		       &dropmonitor_on, sizeof(dropmonitor_on)) < 0) {
@@ -121,7 +130,7 @@ static void do_receive()
 	while ((infinite || loopcount--) && running) {
 		ssize_t nbytes;
 
-		msg.msg_iov[0].iov_len = sizeof(frame);
+		msg.msg_iov[0].iov_len = mtu;
 		msg.msg_controllen = sizeof(ctrlmsg);
 		msg.msg_flags = 0;
 
@@ -197,6 +206,12 @@ static void do_send()
 {
 	unsigned int seq_wrap = 0;
 	uint8_t sequence = 0;
+	size_t mtu;
+
+	if (canfd)
+		mtu = CANFD_MTU;
+	else
+		mtu = CAN_MTU;
 
 	while ((infinite || loopcount--) && running) {
 		ssize_t len;
@@ -205,7 +220,7 @@ static void do_send()
 			printf("sending frame. sequence number: %d\n", sequence);
 
 again:
-		len = write(s, &frame, sizeof(frame));
+		len = write(s, &frame, mtu);
 		if (len == -1) {
 			switch (errno) {
 			case ENOBUFS: {
@@ -254,6 +269,7 @@ int main(int argc, char **argv)
 	};
 	char *interface = "can0";
 	bool extended = false;
+	bool brs = false;
 	bool receive = false;
 	int opt;
 
@@ -263,6 +279,8 @@ int main(int argc, char **argv)
 
 	struct option long_options[] = {
 		{ "extended", no_argument, 0, 'e' },
+		{ "canfd", no_argument, 0, 'f' },
+		{ "brs", no_argument, 0, 'b' },
 		{ "identifier", required_argument, 0, 'i' },
 		{ "loop", required_argument, 0, 'l' },
 		{ "poll", no_argument, 0, 'p' },
@@ -273,12 +291,21 @@ int main(int argc, char **argv)
 		{ 0, 0, 0, 0 },
 	};
 
-	while ((opt = getopt_long(argc, argv, "ei:pq::rvh?", long_options, NULL)) != -1) {
+	while ((opt = getopt_long(argc, argv, "efbi:pq::rvh?", long_options, NULL)) != -1) {
 		switch (opt) {
 		case 'e':
 			extended = true;
 			break;
 
+		case 'f':
+			canfd = true;
+			break;
+
+		case 'b':
+			brs = true; /* bitrate switch implies CAN-FD */
+			canfd = true;
+			break;
+
 		case 'i':
 			filter->can_id = strtoul(optarg, NULL, 0);
 			break;
@@ -360,6 +387,33 @@ int main(int argc, char **argv)
 		exit(EXIT_FAILURE);
 	}
 
+	if (canfd) {
+		const int enable_canfd = 1;
+		struct ifreq ifr;
+
+		strncpy(ifr.ifr_name, interface, sizeof(ifr.ifr_name));
+
+		/* check if the frame fits into the CAN netdevice */
+		if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
+			perror("SIOCGIFMTU");
+			exit(EXIT_FAILURE);
+		}
+
+		if (ifr.ifr_mtu != CANFD_MTU && ifr.ifr_mtu != CANXL_MTU) {
+			printf("CAN interface is only Classical CAN capable - sorry.\n");
+			exit(EXIT_FAILURE);
+		}
+
+		/* 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))) {
+			printf("error when enabling CAN FD support\n");
+			exit(EXIT_FAILURE);
+		}
+	}
+
+	if (brs)
+		frame.flags |= CANFD_BRS;
+
 	if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
 		perror("bind()");
 		exit(EXIT_FAILURE);