diff --git a/src/apdu.c b/src/apdu.c
index 8ac596d..0bdc39a 100644
--- a/src/apdu.c
+++ b/src/apdu.c
@@ -190,7 +190,9 @@ void *apdu_thread(void *arg) {
         uint32_t m = 0;
         queue_remove_blocking(&usb_to_card_q, &m);
         uint32_t flag = m + 1;
-        queue_add_blocking(&card_to_usb_q, &flag);
+        if (m != EV_CMD_AVAILABLE) {
+            queue_add_blocking(&card_to_usb_q, &flag);
+        }
 
         if (m == EV_VERIFY_CMD_AVAILABLE || m == EV_MODIFY_CMD_AVAILABLE) {
             set_res_sw(0x6f, 0x00);
diff --git a/src/debug.h b/src/debug.h
index 0b99634..383f1e0 100644
--- a/src/debug.h
+++ b/src/debug.h
@@ -19,32 +19,53 @@
 #define _DEBUG_H_
 
 #if defined(DEBUG_APDU) && DEBUG_APDU == 1
-#define DEBUG_PAYLOAD(_p, _s) { \
-        printf("Payload %s (%zu bytes) [%s:%d]:\n", #_p, (size_t)(_s), __FILE__, __LINE__); \
-        for (size_t _i = 0; _i < (size_t)(_s); _i += 16) { \
-            printf("%" PRIxPTR "h : ", (uintptr_t) (_i + _p)); \
-            for (size_t _j = 0; _j < 16; _j++) { \
-                if (_j < (size_t)(_s) - _i) printf("%02X ", (_p)[_i + _j]); \
-                else printf("   "); \
-                if (_j == 7) printf(" "); \
-            } printf(":  "); \
-            for (size_t _j = 0; _j < 16; _j++) { \
-                if (_j < (size_t)(_s) - _i && (_p)[_i + _j] > 32 && (_p)[_i + _j] != 127 && (_p)[_i + _j] < 176) printf("%c", (_p)[_i + _j]); \
-                else printf(" "); \
-                if (_j == 7) printf(" "); \
-            } \
-            printf("\n"); \
-        } printf("\n"); \
-}
-#define DEBUG_DATA(_p, _s) {                                                    \
-        printf("Data %s (%zu bytes) [%s:%d]:\n", #_p, (size_t)(_s), __FILE__, __LINE__);      \
-        char *_tmp = (char *) calloc(2 * (_s) + 1, sizeof(char)); \
-        for (size_t _i = 0; _i < (size_t)(_s); _i++) {    \
-            sprintf(&_tmp[2 * _i], "%02X", (_p)[_i]);       \
-        }                                                \
-        printf("%s\n", _tmp);                             \
-        free(_tmp);                                       \
+#include <inttypes.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+
+static inline void debug_payload_impl(const char *name, const uint8_t *p, size_t s, const char *file, int line) {
+    printf("Payload %s (%zu bytes) [%s:%d]:\n", name, s, file, line);
+    for (size_t i = 0; i < s; i += 16) {
+        printf("%" PRIxPTR "h : ", (uintptr_t)(p + i));
+        for (size_t j = 0; j < 16; j++) {
+            if (j < s - i) {
+                printf("%02X ", p[i + j]);
+            }
+            else {
+                printf("   ");
+            }
+            if (j == 7) {
+                printf(" ");
+            }
+        }
+        printf(":  ");
+        for (size_t j = 0; j < 16; j++) {
+            if (j < s - i && p[i + j] > 32 && p[i + j] != 127 && p[i + j] < 176) {
+                printf("%c", p[i + j]);
+            }
+            else {
+                printf(" ");
+            }
+            if (j == 7) {
+                printf(" ");
+            }
+        }
+        printf("\n");
     }
+    printf("\n");
+}
+
+static inline void debug_data_impl(const char *name, const uint8_t *p, size_t s, const char *file, int line) {
+    printf("Data %s (%zu bytes) [%s:%d]:\n", name, s, file, line);
+    for (size_t i = 0; i < s; i++) {
+        printf("%02X", p[i]);
+    }
+    printf("\n");
+}
+
+#define DEBUG_PAYLOAD(_p, _s) debug_payload_impl(#_p, (const uint8_t *)(_p), (size_t)(_s), __FILE__, __LINE__)
+#define DEBUG_DATA(_p, _s) debug_data_impl(#_p, (const uint8_t *)(_p), (size_t)(_s), __FILE__, __LINE__)
 
 #else
 #define DEBUG_PAYLOAD(_p, _s)
diff --git a/src/usb/ccid/ccid.c b/src/usb/ccid/ccid.c
index fd16ae3..a2d2871 100644
--- a/src/usb/ccid/ccid.c
+++ b/src/usb/ccid/ccid.c
@@ -177,20 +177,20 @@ int driver_write_ccid(uint8_t itf, const uint8_t *tx_buffer, uint16_t buffer_siz
     if (*tx_buffer != 0x81) {
         DEBUG_PAYLOAD(tx_buffer, buffer_size);
     }
-    int r = tud_vendor_n_write(itf, tx_buffer, buffer_size);
-    if (r > 0) {
+    uint32_t written = tud_vendor_n_write(itf, tx_buffer, buffer_size);
+    if (written > 0) {
         tud_vendor_n_flush(itf);
 
-        ccid_tx[itf].r_ptr += (uint16_t)buffer_size;
+        ccid_tx[itf].r_ptr += (uint16_t)written;
         if (ccid_tx[itf].r_ptr >= ccid_tx[itf].w_ptr) {
             ccid_tx[itf].r_ptr = ccid_tx[itf].w_ptr = 0;
         }
 
     }
 #ifdef ENABLE_EMULATION
-    tud_vendor_tx_cb(itf, r);
+    tud_vendor_tx_cb(itf, written);
 #endif
-    return r;
+    return (int)written;
 }
 
 int ccid_write_fast(uint8_t itf, const uint8_t *buffer, uint16_t buffer_size) {
diff --git a/src/usb/usb.c b/src/usb/usb.c
index 9bdd08c..101fc00 100644
--- a/src/usb/usb.c
+++ b/src/usb/usb.c
@@ -178,8 +178,10 @@ void usb_send_event(uint32_t flag) {
     if (flag == EV_CMD_AVAILABLE) {
         timeout_start();
     }
-    uint32_t m;
-    queue_remove_blocking(&card_to_usb_q , &m);
+    if (flag != EV_CMD_AVAILABLE) {
+        uint32_t m;
+        queue_remove_blocking(&card_to_usb_q , &m);
+    }
 #ifndef ENABLE_EMULATION
     mutex_exit(&mutex);
 #endif
@@ -254,6 +256,9 @@ void usb_task() {
 
 int card_status(uint8_t itf) {
     if (card_locked_itf == itf) {
+        if (timeout == 0) {
+            return PICOKEY_ERR_FILE_NOT_FOUND;
+        }
         uint32_t m = 0x0;
 #ifndef ENABLE_EMULATION
         mutex_enter_blocking(&mutex);