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;