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

1 files changed, 27 insertions, 4 deletions

diff --git a/firmware/libcommon/source/card_emu.c b/firmware/libcommon/source/card_emu.c
index 5298ffc..497b963 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;