From c90de6983cc09ab0c991afc689ce859331367518 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?K=C3=A9vin=20Redon?= <kredon@sysmocom.de>
Date: Mon, 18 Nov 2019 22:32:58 +0100
Subject: [PATCH] free USB buffer when allocation failed

when the reader sends APDU headers (e.g. after multiple reset),
messages are queued for USB transmission.
but if no host software is connected to SIMtrace in card emulation
mode, the USB message queue is not emptied, leading to the memory
getting full and preventing allocation for newer messages (e.g.
more recent APDU).
in this case the oldest queued message is now dropped to free some
memory.

Change-Id: Ie9ebdd2ff966f67c9afd1ed760f106558f0091ad
---
 firmware/libcommon/source/card_emu.c | 31 ++++++++++++++++++++++++----
 1 file changed, 27 insertions(+), 4 deletions(-)

diff --git a/firmware/libcommon/source/card_emu.c b/firmware/libcommon/source/card_emu.c
index 5298ffcf..497b963e 100644
--- a/firmware/libcommon/source/card_emu.c
+++ b/firmware/libcommon/source/card_emu.c
@@ -254,12 +254,35 @@ void usb_buf_upd_len_and_submit(struct msgb *msg)
 /* Allocate USB buffer and push + initialize simtrace_msg_hdr */
 struct msgb *usb_buf_alloc_st(uint8_t ep, uint8_t msg_class, uint8_t msg_type)
 {
-	struct msgb *msg;
+	struct msgb *msg = NULL;
 	struct simtrace_msg_hdr *sh;
 
-	msg = usb_buf_alloc(ep);
-	if (!msg)
-		return NULL;
+	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) {
+				TRACE_ERROR("ep %u: %s queue does not exist\n\r",
+				            ep, __func__);
+				return NULL;
+			}
+			if (llist_empty(queue)) {
+				TRACE_ERROR("ep %u: %s EOMEM (queue already empty)\n\r",
+				            ep, __func__);
+				return NULL;
+			}
+			msg = msgb_dequeue(queue);
+			if (!msg) {
+				TRACE_ERROR("ep %u: %s no msg in non-empty queue\n\r",
+				            ep, __func__);
+				return NULL;
+			}
+			usb_buf_free(msg);
+			msg = NULL;
+			TRACE_DEBUG("ep %u: %s queue msg dropped\n\r",
+			            ep, __func__);
+		}
+	}
 
 	msg->l1h = msgb_put(msg, sizeof(*sh));
 	sh = (struct simtrace_msg_hdr *) msg->l1h;