[PATCH v2 10/14] greybus: cpc: use holding queue instead of sending out immediately

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

As the first step to handle remote's RX window, store the skb in a
sk_buff_head list, instead of sending a message immediately when pushed
by Greybus.

skbs are still sent out straight away, but now there is a place to store
away if the remote's RX window is too small.

Signed-off-by: Damien Riégel <damien.riegel@silabs.com>
---
 drivers/greybus/cpc/cpc.h      | 10 ++++++----
 drivers/greybus/cpc/cport.c    | 21 ++++++++++++---------
 drivers/greybus/cpc/host.c     |  4 +++-
 drivers/greybus/cpc/protocol.c | 25 ++++++++++++++++++++++++-
 4 files changed, 45 insertions(+), 15 deletions(-)

diff --git a/drivers/greybus/cpc/cpc.h b/drivers/greybus/cpc/cpc.h
index 725fd7f4afc..f1669585c45 100644
--- a/drivers/greybus/cpc/cpc.h
+++ b/drivers/greybus/cpc/cpc.h
@@ -9,15 +9,15 @@
 #include <linux/device.h>
 #include <linux/greybus.h>
 #include <linux/mutex.h>
+#include <linux/skbuff.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
+ * @holding_queue: list of frames queued to be sent
  * @tcb: Transmission Control Block
  */
 struct cpc_cport {
@@ -26,6 +26,8 @@ struct cpc_cport {
 	struct cpc_host_device *cpc_hd;
 	struct mutex lock; /* Synchronize access to state variables */
 
+	struct sk_buff_head holding_queue;
+
 	/*
 	 * @ack: current acknowledge number
 	 * @seq: current sequence number
@@ -42,7 +44,7 @@ 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);
+void cpc_cport_transmit(struct cpc_cport *cport, struct sk_buff *skb);
 
 struct cpc_skb_cb {
 	struct cpc_cport *cport;
@@ -58,7 +60,7 @@ struct cpc_skb_cb {
 
 #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);
+void __cpc_protocol_write_head(struct cpc_cport *cport);
 
 #endif
diff --git a/drivers/greybus/cpc/cport.c b/drivers/greybus/cpc/cport.c
index 847cc8ebe41..91c39856e22 100644
--- a/drivers/greybus/cpc/cport.c
+++ b/drivers/greybus/cpc/cport.c
@@ -7,7 +7,6 @@
 #include <linux/skbuff.h>
 
 #include "cpc.h"
-#include "host.h"
 
 /**
  * cpc_cport_tcb_reset() - Reset cport's TCB to initial values.
@@ -38,15 +37,23 @@ struct cpc_cport *cpc_cport_alloc(u16 cport_id, gfp_t gfp_mask)
 	cpc_cport_tcb_reset(cport);
 
 	mutex_init(&cport->lock);
+	skb_queue_head_init(&cport->holding_queue);
 
 	return cport;
 }
 
 void cpc_cport_release(struct cpc_cport *cport)
 {
+	skb_queue_purge(&cport->holding_queue);
 	kfree(cport);
 }
 
+static void cpc_cport_queue_skb(struct cpc_cport *cport, struct sk_buff *skb)
+{
+	__skb_header_release(skb);
+	__skb_queue_tail(&cport->holding_queue, skb);
+}
+
 /**
  * cpc_cport_pack() - Pack CPort ID into Greybus Operation Message header.
  * @gb_hdr: Greybus operation message header.
@@ -78,11 +85,9 @@ u16 cpc_cport_unpack(struct gb_operation_msg_hdr *gb_hdr)
  * @cport: cport.
  * @skb: skb to be transmitted.
  */
-int cpc_cport_transmit(struct cpc_cport *cport, struct sk_buff *skb)
+void 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;
@@ -94,11 +99,9 @@ int cpc_cport_transmit(struct cpc_cport *cport, struct sk_buff *skb)
 	CPC_SKB_CB(skb)->cpc_flags = CPC_SKB_FLAG_REQ_ACK;
 
 	cport->tcb.seq++;
-	ack = cport->tcb.ack;
+
+	cpc_cport_queue_skb(cport, skb);
+	__cpc_protocol_write_head(cport);
 
 	mutex_unlock(&cport->lock);
-
-	cpc_protocol_prepare_header(skb, ack);
-
-	return cpc_hd_send_skb(cpc_hd, skb);
 }
diff --git a/drivers/greybus/cpc/host.c b/drivers/greybus/cpc/host.c
index 7f0579fde26..ec43d33dfc6 100644
--- a/drivers/greybus/cpc/host.c
+++ b/drivers/greybus/cpc/host.c
@@ -62,7 +62,9 @@ static int cpc_hd_message_send(struct cpc_host_device *cpc_hd, u16 cport_id,
 	CPC_SKB_CB(skb)->cport = cport;
 	CPC_SKB_CB(skb)->gb_message = message;
 
-	return cpc_cport_transmit(cport, skb);
+	cpc_cport_transmit(cport, skb);
+
+	return 0;
 }
 
 static int cpc_hd_cport_allocate(struct cpc_host_device *cpc_hd, int cport_id, unsigned long flags)
diff --git a/drivers/greybus/cpc/protocol.c b/drivers/greybus/cpc/protocol.c
index ad97e903b92..72276818f17 100644
--- a/drivers/greybus/cpc/protocol.c
+++ b/drivers/greybus/cpc/protocol.c
@@ -14,7 +14,7 @@ static bool cpc_skb_is_sequenced(struct sk_buff *skb)
 	return CPC_SKB_CB(skb)->cpc_flags & CPC_SKB_FLAG_REQ_ACK;
 }
 
-void cpc_protocol_prepare_header(struct sk_buff *skb, u8 ack)
+static void cpc_protocol_prepare_header(struct sk_buff *skb, u8 ack)
 {
 	struct cpc_header *hdr;
 
@@ -85,3 +85,26 @@ void cpc_protocol_on_data(struct cpc_cport *cport, struct sk_buff *skb)
 		greybus_data_rcvd(cport->cpc_hd->gb_hd, cport->id, skb->data, skb->len);
 	}
 }
+
+static void __cpc_protocol_write_skb(struct cpc_cport *cport, struct sk_buff *skb, u8 ack)
+{
+	cpc_protocol_prepare_header(skb, ack);
+
+	cpc_hd_send_skb(cport->cpc_hd, skb);
+}
+
+/* Write skbs at the head of holding queue */
+void __cpc_protocol_write_head(struct cpc_cport *cport)
+{
+	struct sk_buff *skb;
+	u8 ack;
+
+	ack = cport->tcb.ack;
+
+	/* For each SKB in the holding queue, clone it and pass it to lower layer */
+	while ((skb = skb_peek(&cport->holding_queue))) {
+		skb_unlink(skb, &cport->holding_queue);
+
+		__cpc_protocol_write_skb(cport, skb, ack);
+	}
+}
-- 
2.52.0