queue: Support multiple queues

src/drivers/usb/class/hid/hww/hid_hww.c

@@ -65,7 +65,7 @@ static int32_t _read(void)
  * Sends the next frame, if the USB interface is ready.
  */
 static void _send_next(void) {
-    const uint8_t *data = queue_pull();
+    const uint8_t *data = queue_pull(queue_hww_queue());
     if (data != NULL) {
         hid_write(&_func_data, data, USB_HID_REPORT_OUT_SIZE);
     } else {

src/drivers/usb/class/hid/u2f/hid_u2f.c

@@ -65,7 +65,7 @@ static int32_t _read(void)
  * Sends the next data, if the USB interface is ready.
  */
 static void _send_next(void) {
-    const uint8_t *data = queue_pull();
+    const uint8_t *data = queue_pull(queue_hww_queue());
     if (data != NULL) {
         hid_write(&_func_data, data, USB_HID_REPORT_OUT_SIZE);
     } else {

src/queue.c

@@ -19,48 +19,63 @@
 // removed.
 #include "usb/usb_frame.h"
 
-static uint32_t _index_start = 0;
-static uint32_t _index_end = 0;
-// We use the queue to send and receive usb packets. Allocate enough space in
-// case all pulls come after all pushes.
-#define QUEUE_NUM_PACKETS ((USB_DATA_MAX_LEN / USB_REPORT_SIZE) * 2)
 // TODO: specify generic size
-static uint8_t _packets[QUEUE_NUM_PACKETS][USB_REPORT_SIZE];
+// The queue has enough room for a single maximum size packet
+#define QUEUE_NUM_REPORTS (USB_DATA_MAX_LEN / USB_REPORT_SIZE)
 
-void queue_clear(void)
+// `start` and `end` are indices into `items`
+struct queue {
+    uint32_t start;
+    uint32_t end;
+    uint8_t items[QUEUE_NUM_REPORTS][USB_REPORT_SIZE];
+};
+
+void queue_clear(struct queue* ctx)
 {
-    util_zero(_packets, sizeof(_packets));
-    _index_start = _index_end;
+    util_zero(ctx->items, sizeof(ctx->items));
+    ctx->start = ctx->end;
 }
 
-const uint8_t* queue_pull(void)
+const uint8_t* queue_pull(struct queue* ctx)
 {
-    uint32_t p = _index_start;
-    if (p == _index_end) {
+    uint32_t p = ctx->start;
+    if (p == ctx->end) {
         // queue is empty
         return NULL;
     }
-    _index_start = (p + 1) % QUEUE_NUM_PACKETS;
-    return _packets[p];
+    ctx->start = (p + 1) % QUEUE_NUM_REPORTS;
+    return ctx->items[p];
 }
 
-uint8_t queue_push(const uint8_t* data)
+int32_t queue_push(struct queue* ctx, const uint8_t* data)
 {
-    uint32_t next = (_index_end + 1) % QUEUE_NUM_PACKETS;
-    if (_index_start == next) {
-        return ERR_QUEUE_FULL; // Buffer full
+    uint32_t next = (ctx->end + 1) % QUEUE_NUM_REPORTS;
+    if (ctx->start == next) {
+        return QUEUE_ERR_FULL; // Buffer full
     }
-    memcpy(_packets[_index_end], data, USB_REPORT_SIZE);
-    _index_end = next;
-    return ERR_NONE;
+    memcpy(ctx->items[ctx->end], data, USB_REPORT_SIZE);
+    ctx->end = next;
+    return QUEUE_ERR_NONE;
 }
 
-const uint8_t* queue_peek(void)
+const uint8_t* queue_peek(struct queue* ctx)
 {
-    uint32_t p = _index_start;
-    if (p == _index_end) {
+    uint32_t p = ctx->start;
+    if (p == ctx->end) {
         // queue is empty
         return NULL;
     }
-    return _packets[p];
+    return ctx->items[p];
+}
+
+struct queue* queue_hww_queue(void)
+{
+    static struct queue queue;
+    return &queue;
+}
+
+struct queue* queue_u2f_queue(void)
+{
+    static struct queue queue;
+    return &queue;
 }

src/queue.h

@@ -18,25 +18,42 @@
 #include <stdint.h>
 #include <string.h>
 
-#define ERR_NONE 0
-#define ERR_QUEUE_FULL 7
+#define QUEUE_ERR_NONE 0
+#define QUEUE_ERR_FULL -1
+
+struct queue;
 
 /**
  * Append the given data to the queue.
- * Returns ERR_NONE if the data was added and ERR_QUEUE_FULL if the buffer was full.
+ * Returns QUEUE_ERR_NONE if the data was added and QUEUE_ERR_FULL if the buffer was full.
+ * data must be USB_REPORT_SIZE large
  */
-uint8_t queue_push(const uint8_t* data);
+int32_t queue_push(struct queue* ctx, const uint8_t* data);
 
 /**
  * Return the first data that was added to the queue.
+ * Returns NULL if empty
  */
-const uint8_t* queue_pull(void);
+const uint8_t* queue_pull(struct queue* ctx);
 
 /**
  * Clear the queue.
  */
-void queue_clear(void);
+void queue_clear(struct queue* ctx);
 
-const uint8_t* queue_peek(void);
+/**
+ * Peek at the tip of the queue. Returns NULL if queue is empty.
+ */
+const uint8_t* queue_peek(struct queue* ctx);
+
+/**
+ * Get a pointer to the hww queue
+ */
+struct queue* queue_hww_queue(void);
+
+/**
+ * Get a pointer ot the u2f queue
+ */
+struct queue* queue_u2f_queue(void);
 
 #endif

src/usb/usb_frame.c

@@ -118,7 +118,7 @@ uint8_t usb_frame_reply(
     const uint8_t* data,
     const uint32_t len,
     const uint32_t cid,
-    uint8_t(add_frame_callback)(const uint8_t*))
+    int32_t(add_frame_callback)(const uint8_t*))
 {
     USB_FRAME frame;
     uint32_t cnt = 0;
@@ -135,7 +135,7 @@ uint8_t usb_frame_reply(
     // Init frame
     psz = MIN(sizeof(frame.init.data), l);
     memcpy(frame.init.data, data, psz);
-    uint8_t err = add_frame_callback((const uint8_t*)&frame);
+    int32_t err = add_frame_callback((const uint8_t*)&frame);
     if (err != ERR_NONE) {
         return err;
     }
@@ -166,7 +166,7 @@ uint8_t usb_frame_reply(
 uint8_t usb_frame_prepare_err(
     uint8_t err,
     uint32_t cid,
-    uint8_t(add_frame_callback)(const uint8_t*))
+    int32_t(add_frame_callback)(const uint8_t*))
 {
     USB_FRAME frame;
 

src/usb/usb_frame.h

@@ -107,7 +107,7 @@ uint8_t usb_frame_reply(
     const uint8_t* data,
     uint32_t len,
     uint32_t cid,
-    uint8_t(add_frame_callback)(const uint8_t*));
+    int32_t(add_frame_callback)(const uint8_t*));
 
 /**
  * Takes data and a channel id and constructs USB frames that are added
@@ -131,7 +131,7 @@ void usb_frame_send_cmd(uint8_t cmd, const uint8_t* data, uint32_t len, uint8_t
 uint8_t usb_frame_prepare_err(
     uint8_t err,
     uint32_t cid,
-    uint8_t(add_frame_callback)(const uint8_t*));
+    int32_t(add_frame_callback)(const uint8_t*));
 
 /**
  * Processes usb frame requests.

src/usb/usb_packet.c

@@ -16,6 +16,7 @@
 #include "queue.h"
 #include "screen.h"
 #include "usb_processing.h"
+#include <err_codes.h>
 #include <stdbool.h>
 #include <stdlib.h>
 
@@ -59,11 +60,16 @@ static State _in_state;
  */
 static void _reset_state(void)
 {
-    queue_clear();
+    queue_clear(queue_hww_queue());
     _timeout_disable(_in_state.cid);
     memset(&_in_state, 0, sizeof(_in_state));
 }
 
+static int32_t _queue_push(const uint8_t* data)
+{
+    return queue_push(queue_hww_queue(), data);
+}
+
 /**
  * Responds with an error.
  * @param[in] err The error.
@@ -72,7 +78,7 @@ static void _reset_state(void)
  */
 static void _queue_err(const uint8_t err, uint32_t cid)
 {
-    usb_frame_prepare_err(err, cid, queue_push);
+    usb_frame_prepare_err(err, cid, _queue_push);
 }
 
 static bool _need_more_data(void)

src/usb/usb_processing.c

@@ -30,14 +30,26 @@ static Packet _in_packet = {0};
 static bool _in_packet_queued = false;
 static void (*_send)(void) = NULL;
 
+// TODO: remove this global in future refactoring
+static struct queue* _global_queue;
+
+static int32_t _queue_push(const uint8_t* data)
+{
+    if (_global_queue == NULL) {
+        Abort("usb_processing: Internal error");
+    }
+    return queue_push(_global_queue, data);
+}
+
 /**
  * Responds with data of a certain length.
  * @param[in] packet The packet to be sent.
  */
 static uint8_t _enqueue_frames(const Packet* out_packet)
 {
+    _global_queue = queue_hww_queue();
     return usb_frame_reply(
-        out_packet->cmd, out_packet->data_addr, out_packet->len, out_packet->cid, queue_push);
+        out_packet->cmd, out_packet->data_addr, out_packet->len, out_packet->cid, _queue_push);
 }
 
 /**
@@ -140,7 +152,8 @@ void usb_processing_process(void)
         // cmd in '_registered_cmds' if the U2F bit it not set (== U2F disabled).
         // TODO: figure out the consequences and either implement a solution or
         // inform U2F hijack vendors.
-        usb_frame_prepare_err(FRAME_ERR_INVALID_CMD, _in_packet.cid, queue_push);
+        _global_queue = queue_hww_queue();
+        usb_frame_prepare_err(FRAME_ERR_INVALID_CMD, _in_packet.cid, _queue_push);
     }
     _send();
     _in_packet_queued = false;

test/unit-test/framework/mock_hidapi.c

@@ -139,7 +139,7 @@ int hid_read_timeout(hid_device* dev, unsigned char* r, size_t r_len, int to)
         usb_processing_process();
     }
     usb_processing_process();
-    uint8_t* p = queue_pull();
+    uint8_t* p = queue_pull(queue_hww_queue());
     // printf("Queue: %p\n", p);
     if (p != NULL) {
         memcpy(r, p, MIN(r_len, BUFSIZE));
@@ -148,7 +148,7 @@ int hid_read_timeout(hid_device* dev, unsigned char* r, size_t r_len, int to)
         _delay(600);
         return -127;
     }
-    if (queue_peek() == NULL) {
+    if (queue_peek(queue_hww_queue()) == NULL) {
         _have_data = false;
     }
     _expect_more = false;