[PATCH v3 08/14] greybus: cpc: add and validate sequence numbers

["Damien Riégel" <damien.riegel@silabs.com>]

The first step in making the CPC header actually do something is to add
the sequence number to outgoing messages and validate that incoming
frames are received in order.

At this stage, the driver doesn't send standalone acks, so if a message
with Sequence X is received, the remote will not be acknowledged until a
message targeting that CPort comes from the Greybus layer. Only then the
driver will ack with acknowledgedment number of X + 1.

Signed-off-by: Damien Riégel <damien.riegel@silabs.com>
---
 drivers/greybus/cpc/Makefile   |  2 +-
 drivers/greybus/cpc/cpc.h      | 20 +++++++++++++++
 drivers/greybus/cpc/cport.c    | 25 ++++++++++++++++++
 drivers/greybus/cpc/header.c   | 17 ++++++++++++
 drivers/greybus/cpc/header.h   |  2 ++
 drivers/greybus/cpc/host.c     | 13 +++++++---
 drivers/greybus/cpc/protocol.c | 47 ++++++++++++++++++++++++++++++++++
 7 files changed, 121 insertions(+), 5 deletions(-)
 create mode 100644 drivers/greybus/cpc/header.c
 create mode 100644 drivers/greybus/cpc/protocol.c

diff --git a/drivers/greybus/cpc/Makefile b/drivers/greybus/cpc/Makefile
index 3d50f8c5473..c4b530d27a3 100644
--- a/drivers/greybus/cpc/Makefile
+++ b/drivers/greybus/cpc/Makefile
@@ -1,6 +1,6 @@
 # SPDX-License-Identifier: GPL-2.0
 
-gb-cpc-y := cport.o host.o
+gb-cpc-y := cport.o header.o host.o protocol.o
 
 # CPC core
 obj-$(CONFIG_GREYBUS_CPC)	+= gb-cpc.o
diff --git a/drivers/greybus/cpc/cpc.h b/drivers/greybus/cpc/cpc.h
index 62597957814..87b54a4fd34 100644
--- a/drivers/greybus/cpc/cpc.h
+++ b/drivers/greybus/cpc/cpc.h
@@ -8,17 +8,32 @@
 
 #include <linux/device.h>
 #include <linux/greybus.h>
+#include <linux/mutex.h>
 #include <linux/types.h>
 
+struct sk_buff;
+
 /**
  * struct cpc_cport - CPC cport
  * @id: cport ID
  * @cpc_hd: pointer to the CPC host device this cport belongs to
+ * @lock: mutex to synchronize accesses to tcb and other attributes
+ * @tcb: Transmission Control Block
  */
 struct cpc_cport {
 	u16 id;
 
 	struct cpc_host_device *cpc_hd;
+	struct mutex lock; /* Synchronize access to state variables */
+
+	/*
+	 * @ack: current acknowledge number
+	 * @seq: current sequence number
+	 */
+	struct {
+		u8 ack;
+		u8 seq;
+	} tcb;
 };
 
 struct cpc_cport *cpc_cport_alloc(u16 cport_id, gfp_t gfp_mask);
@@ -34,8 +49,13 @@ struct cpc_skb_cb {
 
 	/* Keep track of the GB message the skb originates from */
 	struct gb_message *gb_message;
+
+	u8 seq;
 };
 
 #define CPC_SKB_CB(__skb) ((struct cpc_skb_cb *)&((__skb)->cb[0]))
 
+void cpc_protocol_prepare_header(struct sk_buff *skb, u8 ack);
+void cpc_protocol_on_data(struct cpc_cport *cport, struct sk_buff *skb);
+
 #endif
diff --git a/drivers/greybus/cpc/cport.c b/drivers/greybus/cpc/cport.c
index 2c73d8e724e..7041a6a8a36 100644
--- a/drivers/greybus/cpc/cport.c
+++ b/drivers/greybus/cpc/cport.c
@@ -9,6 +9,16 @@
 #include "cpc.h"
 #include "host.h"
 
+/**
+ * cpc_cport_tcb_reset() - Reset cport's TCB to initial values.
+ * @cport: cport pointer
+ */
+static void cpc_cport_tcb_reset(struct cpc_cport *cport)
+{
+	cport->tcb.ack = 0;
+	cport->tcb.seq = 0;
+}
+
 /**
  * cpc_cport_alloc() - Allocate and initialize CPC cport.
  * @cport_id: cport ID.
@@ -25,6 +35,9 @@ struct cpc_cport *cpc_cport_alloc(u16 cport_id, gfp_t gfp_mask)
 		return NULL;
 
 	cport->id = cport_id;
+	cpc_cport_tcb_reset(cport);
+
+	mutex_init(&cport->lock);
 
 	return cport;
 }
@@ -69,10 +82,22 @@ int cpc_cport_transmit(struct cpc_cport *cport, struct sk_buff *skb)
 {
 	struct cpc_host_device *cpc_hd = cport->cpc_hd;
 	struct gb_operation_msg_hdr *gb_hdr;
+	u8 ack;
 
 	/* Inject cport ID in Greybus header */
 	gb_hdr = (struct gb_operation_msg_hdr *)skb->data;
 	cpc_cport_pack(gb_hdr, cport->id);
 
+	mutex_lock(&cport->lock);
+
+	CPC_SKB_CB(skb)->seq = cport->tcb.seq;
+
+	cport->tcb.seq++;
+	ack = cport->tcb.ack;
+
+	mutex_unlock(&cport->lock);
+
+	cpc_protocol_prepare_header(skb, ack);
+
 	return cpc_hd_send_skb(cpc_hd, skb);
 }
diff --git a/drivers/greybus/cpc/header.c b/drivers/greybus/cpc/header.c
new file mode 100644
index 00000000000..62946d6077e
--- /dev/null
+++ b/drivers/greybus/cpc/header.c
@@ -0,0 +1,17 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2025, Silicon Laboratories, Inc.
+ */
+
+#include "header.h"
+
+/**
+ * cpc_header_get_seq() - Get the sequence number.
+ * @hdr: CPC header.
+ *
+ * Return: Sequence number.
+ */
+u8 cpc_header_get_seq(const struct cpc_header *hdr)
+{
+	return hdr->seq;
+}
diff --git a/drivers/greybus/cpc/header.h b/drivers/greybus/cpc/header.h
index f65a608a650..5196422380e 100644
--- a/drivers/greybus/cpc/header.h
+++ b/drivers/greybus/cpc/header.h
@@ -38,4 +38,6 @@ struct cpc_header {
 	__u8 ack;
 } __packed;
 
+u8 cpc_header_get_seq(const struct cpc_header *hdr);
+
 #endif
diff --git a/drivers/greybus/cpc/host.c b/drivers/greybus/cpc/host.c
index 759322759bd..2c1b5d02ec2 100644
--- a/drivers/greybus/cpc/host.c
+++ b/drivers/greybus/cpc/host.c
@@ -215,19 +215,24 @@ EXPORT_SYMBOL_GPL(cpc_hd_message_sent);
 void cpc_hd_rcvd(struct cpc_host_device *cpc_hd, struct sk_buff *skb)
 {
 	struct gb_operation_msg_hdr *gb_hdr;
+	struct cpc_cport *cport;
 	u16 cport_id;
 
 	/* Prevent an out-of-bound access if called with non-sensical parameters. */
 	if (skb->len < (sizeof(*gb_hdr) + sizeof(struct cpc_header)))
 		goto free_skb;
 
-	skb_pull(skb, sizeof(struct cpc_header));
-
 	/* Retrieve cport ID that was packed in Greybus header */
-	gb_hdr = (struct gb_operation_msg_hdr *)skb->data;
+	gb_hdr = (struct gb_operation_msg_hdr *)(skb->data + sizeof(struct cpc_header));
 	cport_id = cpc_cport_unpack(gb_hdr);
 
-	greybus_data_rcvd(cpc_hd->gb_hd, cport_id, skb->data, skb->len);
+	cport = cpc_hd_get_cport(cpc_hd, cport_id);
+	if (!cport) {
+		dev_warn(cpc_hd_dev(cpc_hd), "cport %u not allocated\n", cport_id);
+		goto free_skb;
+	}
+
+	cpc_protocol_on_data(cport, skb);
 
 free_skb:
 	kfree_skb(skb);
diff --git a/drivers/greybus/cpc/protocol.c b/drivers/greybus/cpc/protocol.c
new file mode 100644
index 00000000000..8f0ac6dfa11
--- /dev/null
+++ b/drivers/greybus/cpc/protocol.c
@@ -0,0 +1,47 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2025, Silicon Laboratories, Inc.
+ */
+
+#include <linux/skbuff.h>
+
+#include "cpc.h"
+#include "header.h"
+#include "host.h"
+
+void cpc_protocol_prepare_header(struct sk_buff *skb, u8 ack)
+{
+	struct cpc_header *hdr;
+
+	skb_push(skb, sizeof(*hdr));
+
+	hdr = (struct cpc_header *)skb->data;
+	memset(hdr, 0, sizeof(*hdr));
+
+	hdr->ack = ack;
+	hdr->seq = CPC_SKB_CB(skb)->seq;
+}
+
+void cpc_protocol_on_data(struct cpc_cport *cport, struct sk_buff *skb)
+{
+	struct cpc_header *cpc_hdr = (struct cpc_header *)skb->data;
+	u8 seq = cpc_header_get_seq(cpc_hdr);
+	bool expected_seq = false;
+
+	mutex_lock(&cport->lock);
+
+	expected_seq = seq == cport->tcb.ack;
+	if (expected_seq)
+		cport->tcb.ack++;
+	else
+		dev_warn_ratelimited(cpc_hd_dev(cport->cpc_hd),
+				     "unexpected seq: %u, expected seq: %u\n", seq, cport->tcb.ack);
+
+	mutex_unlock(&cport->lock);
+
+	if (expected_seq) {
+		skb_pull(skb, sizeof(*cpc_hdr));
+
+		greybus_data_rcvd(cport->cpc_hd->gb_hd, cport->id, skb->data, skb->len);
+	}
+}
-- 
2.52.0