usb_buf: count number of elements in queue

This is in preparation for limiting the maximum queue length

Change-Id: I7cb184d7a1ccb519010a2f3e3295cc3a5fbf8052
Related: OS#4251

6 files changed, 58 insertions, 13 deletions

diff --git a/firmware/libcommon/include/usb_buf.h b/firmware/libcommon/include/usb_buf.h
index bd6947b..3a4eda2 100644
--- a/firmware/libcommon/include/usb_buf.h
+++ b/firmware/libcommon/include/usb_buf.h
@@ -29,6 +29,8 @@ struct usb_buffered_ep {
 	volatile uint32_t in_progress;
 	/* Tx queue (IN) / Rx queue (OUT) */
 	struct llist_head queue;
+	/* current length of queue */
+	unsigned int queue_len;
 };
 
 struct msgb *usb_buf_alloc(uint8_t ep);
diff --git a/firmware/libcommon/source/card_emu.c b/firmware/libcommon/source/card_emu.c
index 923226a..f95f5d1 100644
--- a/firmware/libcommon/source/card_emu.c
+++ b/firmware/libcommon/source/card_emu.c
@@ -260,18 +260,18 @@ struct msgb *usb_buf_alloc_st(uint8_t ep, uint8_t msg_class, uint8_t msg_type)
 	while (!msg) {
 		msg = usb_buf_alloc(ep); // try to allocate some memory
 		if (!msg) { // allocation failed, we might be out of memory
-			struct llist_head *queue = usb_get_queue(ep);
-			if (!queue) {
+			struct usb_buffered_ep *bep = usb_get_buf_ep(ep);
+			if (!bep) {
 				TRACE_ERROR("ep %u: %s queue does not exist\n\r",
 				            ep, __func__);
 				return NULL;
 			}
-			if (llist_empty(queue)) {
+			if (llist_empty(&bep->queue)) {
 				TRACE_ERROR("ep %u: %s EOMEM (queue already empty)\n\r",
 				            ep, __func__);
 				return NULL;
 			}
-			msg = msgb_dequeue(queue);
+			msg = msgb_dequeue_count(&bep->queue, &bep->queue_len);
 			if (!msg) {
 				TRACE_ERROR("ep %u: %s no msg in non-empty queue\n\r",
 				            ep, __func__);
diff --git a/firmware/libcommon/source/host_communication.c b/firmware/libcommon/source/host_communication.c
index 1e15ada..0e496c5 100644
--- a/firmware/libcommon/source/host_communication.c
+++ b/firmware/libcommon/source/host_communication.c
@@ -76,7 +76,7 @@ int usb_refill_to_host(uint8_t ep)
 
 	bep->in_progress++;
 
-	msg = msgb_dequeue(&bep->queue);
+	msg = msgb_dequeue_count(&bep->queue, &bep->queue_len);
 
 	local_irq_restore(x);
 
@@ -180,7 +180,7 @@ int usb_drain_queue(uint8_t ep)
 	}
 
 	/* free all queued msgbs */
-	while ((msg = msgb_dequeue(&bep->queue))) {
+	while ((msg = msgb_dequeue_count(&bep->queue, &bep->queue_len))) {
 		usb_buf_free(msg);
 		ret++;
 	}
diff --git a/firmware/libcommon/source/usb_buf.c b/firmware/libcommon/source/usb_buf.c
index 418569e..8ad5a0e 100644
--- a/firmware/libcommon/source/usb_buf.c
+++ b/firmware/libcommon/source/usb_buf.c
@@ -78,7 +78,7 @@ int usb_buf_submit(struct msgb *msg)
 
 	/* no need for irqsafe operation, as the usb_tx_queue is
 	 * processed only by the main loop context */
-	msgb_enqueue(&ep->queue, msg);
+	msgb_enqueue_count(&ep->queue, msg, &ep->queue_len);
 	return 0;
 }
 
diff --git a/firmware/libosmocore/include/osmocom/core/msgb.h b/firmware/libosmocore/include/osmocom/core/msgb.h
index afb887c..7c4420d 100644
--- a/firmware/libosmocore/include/osmocom/core/msgb.h
+++ b/firmware/libosmocore/include/osmocom/core/msgb.h
@@ -80,6 +80,49 @@ extern int msgb_resize_area(struct msgb *msg, uint8_t *area,
 extern struct msgb *msgb_copy(const struct msgb *msg, const char *name);
 static int msgb_test_invariant(const struct msgb *msg) __attribute__((pure));
 
+/*! Free all msgbs from a queue built with msgb_enqueue().
+ * \param[in] queue  list head of a msgb queue.
+ */
+static inline void msgb_queue_free(struct llist_head *queue)
+{
+	struct msgb *msg;
+	while ((msg = msgb_dequeue(queue))) msgb_free(msg);
+}
+
+/*! Enqueue message buffer to tail of a queue and increment queue size counter
+ * \param[in] queue linked list header of queue
+ * \param[in] msg message buffer to be added to the queue
+ * \param[in] count pointer to variable holding size of the queue
+ *
+ * The function will append the specified message buffer \a msg to the queue
+ * implemented by \ref llist_head \a queue using function \ref msgb_enqueue_count,
+ * then increment \a count
+ */
+static inline void msgb_enqueue_count(struct llist_head *queue, struct msgb *msg,
+					unsigned int *count)
+{
+	msgb_enqueue(queue, msg);
+	(*count)++;
+}
+
+/*! Dequeue message buffer from head of queue and decrement queue size counter
+ * \param[in] queue linked list header of queue
+ * \param[in] count pointer to variable holding size of the queue
+ * \returns message buffer (if any) or NULL if queue empty
+ *
+ * The function will remove the first message buffer from the queue
+ * implemented by \ref llist_head \a queue using function \ref msgb_enqueue_count,
+ * and decrement \a count, all if queue is not empty.
+ */
+static inline struct msgb *msgb_dequeue_count(struct llist_head *queue,
+						unsigned int *count)
+{
+	struct msgb *msg = msgb_dequeue(queue);
+	if (msg)
+		(*count)--;
+	return msg;
+}
+
 #ifdef MSGB_DEBUG
 #include <osmocom/core/panic.h>
 #define MSGB_ABORT(msg, fmt, args ...) do {		\
diff --git a/firmware/test/card_emu_tests.c b/firmware/test/card_emu_tests.c
index a5ba62e..2a1d682 100644
--- a/firmware/test/card_emu_tests.c
+++ b/firmware/test/card_emu_tests.c
@@ -177,14 +177,14 @@ static void dump_rctx(struct msgb *msg)
 
 static void get_and_verify_rctx(uint8_t ep, const uint8_t *data, unsigned int len)
 {
-	struct llist_head *queue = usb_get_queue(ep);
+	struct usb_buffered_ep *bep = usb_get_buf_ep(ep);
 	struct msgb *msg;
 	struct cardemu_usb_msg_tx_data *td;
 	struct cardemu_usb_msg_rx_data *rd;
 	struct simtrace_msg_hdr *mh;
 
-	assert(queue);
-	msg = msgb_dequeue(queue);
+	assert(bep);
+	msg = msgb_dequeue_count(&bep->queue, &bep->queue_len);
 	assert(msg);
 	dump_rctx(msg);
 	assert(msg->l1h);
@@ -214,13 +214,13 @@ static void get_and_verify_rctx(uint8_t ep, const uint8_t *data, unsigned int le
 
 static void get_and_verify_rctx_pps(const uint8_t *data, unsigned int len)
 {
-	struct llist_head *queue = usb_get_queue(PHONE_DATAIN);
+	struct usb_buffered_ep *bep = usb_get_buf_ep(PHONE_DATAIN);
 	struct msgb *msg;
 	struct simtrace_msg_hdr *mh;
 	struct cardemu_usb_msg_pts_info *ptsi;
 
-	assert(queue);
-	msg = msgb_dequeue(queue);
+	assert(bep);
+	msg = msgb_dequeue_count(&bep->queue, &bep->queue_len);
 	assert(msg);
 	dump_rctx(msg);
 	assert(msg->l1h);