[PATCH v3 04/14] greybus: cpc: pack cport ID in Greybus header

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

Take advantage of the padding bytes present in the Greybus header to
store the CPort ID and minimize overhead. This technique is already used
by the es2 driver.

Signed-off-by: Damien Riégel <damien.riegel@silabs.com>
---
Changes in v2:
  - clear pad bytes when unpacking CPort ID

 drivers/greybus/cpc/cpc.h   |  3 +++
 drivers/greybus/cpc/cport.c | 34 ++++++++++++++++++++++++++++++++++
 drivers/greybus/cpc/host.c  | 13 ++++++++++++-
 drivers/greybus/cpc/host.h  |  2 +-
 4 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/drivers/greybus/cpc/cpc.h b/drivers/greybus/cpc/cpc.h
index d9f8f60913a..62597957814 100644
--- a/drivers/greybus/cpc/cpc.h
+++ b/drivers/greybus/cpc/cpc.h
@@ -24,6 +24,9 @@ struct cpc_cport {
 struct cpc_cport *cpc_cport_alloc(u16 cport_id, gfp_t gfp_mask);
 void cpc_cport_release(struct cpc_cport *cport);
 
+void cpc_cport_pack(struct gb_operation_msg_hdr *gb_hdr, u16 cport_id);
+u16 cpc_cport_unpack(struct gb_operation_msg_hdr *gb_hdr);
+
 int cpc_cport_transmit(struct cpc_cport *cport, struct sk_buff *skb);
 
 struct cpc_skb_cb {
diff --git a/drivers/greybus/cpc/cport.c b/drivers/greybus/cpc/cport.c
index ed0b8e8b0d7..2c73d8e724e 100644
--- a/drivers/greybus/cpc/cport.c
+++ b/drivers/greybus/cpc/cport.c
@@ -3,6 +3,9 @@
  * Copyright (c) 2025, Silicon Laboratories, Inc.
  */
 
+#include <linux/unaligned.h>
+#include <linux/skbuff.h>
+
 #include "cpc.h"
 #include "host.h"
 
@@ -31,6 +34,32 @@ void cpc_cport_release(struct cpc_cport *cport)
 	kfree(cport);
 }
 
+/**
+ * cpc_cport_pack() - Pack CPort ID into Greybus Operation Message header.
+ * @gb_hdr: Greybus operation message header.
+ * @cport_id: CPort ID to pack.
+ */
+void cpc_cport_pack(struct gb_operation_msg_hdr *gb_hdr, u16 cport_id)
+{
+	put_unaligned_le16(cport_id, gb_hdr->pad);
+}
+
+/**
+ * cpc_cport_unpack() - Unpack CPort ID from Greybus Operation Message header.
+ * @gb_hdr: Greybus operation message header.
+ *
+ * Return: CPort ID packed in the header.
+ */
+u16 cpc_cport_unpack(struct gb_operation_msg_hdr *gb_hdr)
+{
+	u16 cport_id = get_unaligned_le16(gb_hdr->pad);
+
+	// Clear padding bytes
+	put_unaligned_le16(0, gb_hdr->pad);
+
+	return cport_id;
+}
+
 /**
  * cpc_cport_transmit() - Transmit skb over cport.
  * @cport: cport.
@@ -39,6 +68,11 @@ void cpc_cport_release(struct cpc_cport *cport)
 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;
+
+	/* Inject cport ID in Greybus header */
+	gb_hdr = (struct gb_operation_msg_hdr *)skb->data;
+	cpc_cport_pack(gb_hdr, cport->id);
 
 	return cpc_hd_send_skb(cpc_hd, skb);
 }
diff --git a/drivers/greybus/cpc/host.c b/drivers/greybus/cpc/host.c
index a3acfc9bfca..10b0529dc4e 100644
--- a/drivers/greybus/cpc/host.c
+++ b/drivers/greybus/cpc/host.c
@@ -209,8 +209,19 @@ void cpc_hd_message_sent(struct sk_buff *skb, int status)
 }
 EXPORT_SYMBOL_GPL(cpc_hd_message_sent);
 
-void cpc_hd_rcvd(struct cpc_host_device *cpc_hd, u16 cport_id, u8 *data, size_t length)
+void cpc_hd_rcvd(struct cpc_host_device *cpc_hd, u8 *data, size_t length)
 {
+	struct gb_operation_msg_hdr *gb_hdr;
+	u16 cport_id;
+
+	/* Prevent an out-of-bound access if called with non-sensical parameters. */
+	if (!data || length < sizeof(*gb_hdr))
+		return;
+
+	/* Retrieve cport ID that was packed in Greybus header */
+	gb_hdr = (struct gb_operation_msg_hdr *)data;
+	cport_id = cpc_cport_unpack(gb_hdr);
+
 	greybus_data_rcvd(cpc_hd->gb_hd, cport_id, data, length);
 }
 EXPORT_SYMBOL_GPL(cpc_hd_rcvd);
diff --git a/drivers/greybus/cpc/host.h b/drivers/greybus/cpc/host.h
index 191b5e394a6..2e568bac44e 100644
--- a/drivers/greybus/cpc/host.h
+++ b/drivers/greybus/cpc/host.h
@@ -45,7 +45,7 @@ struct cpc_host_device *cpc_hd_create(struct cpc_hd_driver *driver, struct devic
 int cpc_hd_add(struct cpc_host_device *cpc_hd);
 void cpc_hd_put(struct cpc_host_device *cpc_hd);
 void cpc_hd_del(struct cpc_host_device *cpc_hd);
-void cpc_hd_rcvd(struct cpc_host_device *cpc_hd, u16 cport_id, u8 *data, size_t length);
+void cpc_hd_rcvd(struct cpc_host_device *cpc_hd, u8 *data, size_t length);
 void cpc_hd_message_sent(struct sk_buff *skb, int status);
 
 int cpc_hd_send_skb(struct cpc_host_device *cpc_hd, struct sk_buff *skb);
-- 
2.52.0