1 files changed, 22 insertions, 12 deletions

diff --git a/fw/fe310/eos/msgq.c b/fw/fe310/eos/msgq.c
index a483a58..2af3b14 100644
--- a/fw/fe310/eos/msgq.c
+++ b/fw/fe310/eos/msgq.c
@@ -17,7 +17,15 @@ void eos_msgq_init(EOSMsgQ *msgq, EOSMsgItem *array, uint8_t size) {
     msgq->array = array;
 }
 
+uint8_t eos_msgq_len(EOSMsgQ *msgq) {
+    return (uint8_t)(msgq->idx_w - msgq->idx_r);
+}
+
 int eos_msgq_push(EOSMsgQ *msgq, unsigned char type, unsigned char *buffer, uint16_t len) {
+    return eos_msgq_push_widx(msgq, type, buffer, len, NULL);
+}
+
+int eos_msgq_push_widx(EOSMsgQ *msgq, unsigned char type, unsigned char *buffer, uint16_t len, uint8_t *_idx) {
     if ((uint8_t)(msgq->idx_w - msgq->idx_r) == msgq->size) return EOS_ERR_FULL;
 
     uint8_t idx = IDX_MASK(msgq->idx_w, msgq->size);
@@ -25,10 +33,15 @@ int eos_msgq_push(EOSMsgQ *msgq, unsigned char type, unsigned char *buffer, uint
     msgq->array[idx].buffer = buffer;
     msgq->array[idx].len = len;
     msgq->idx_w++;
+    if (_idx) *_idx = idx;
     return EOS_OK;
 }
 
 void eos_msgq_pop(EOSMsgQ *msgq, unsigned char *type, unsigned char **buffer, uint16_t *len) {
+    eos_msgq_pop_widx(msgq, type, buffer, len, NULL);
+}
+
+void eos_msgq_pop_widx(EOSMsgQ *msgq, unsigned char *type, unsigned char **buffer, uint16_t *len, uint8_t *_idx) {
     if (msgq->idx_r == msgq->idx_w) {
         *type = 0;
         *buffer = NULL;
@@ -39,6 +52,7 @@ void eos_msgq_pop(EOSMsgQ *msgq, unsigned char *type, unsigned char **buffer, ui
         *buffer = msgq->array[idx].buffer;
         *len = msgq->array[idx].len;
         msgq->idx_r++;
+        if (_idx) *_idx = idx;
     }
 }
 
@@ -52,7 +66,7 @@ int eos_msgq_find(EOSMsgQ *msgq, unsigned char type, unsigned char *selector, ui
             *buffer = NULL;
             *len = 0;
         }
-        return 0;
+        return EOS_ERR_NOTFOUND;
     }
 
     idx = IDX_MASK(msgq->idx_r, msgq->size);
@@ -65,7 +79,7 @@ int eos_msgq_find(EOSMsgQ *msgq, unsigned char type, unsigned char *selector, ui
                 *buffer = _buffer;
                 *len = _len;
             }
-            return 1;
+            return EOS_OK;
         }
     }
     for (i = msgq->idx_r + 1; IDX_LT(i, msgq->idx_w); i++) {
@@ -82,7 +96,7 @@ int eos_msgq_find(EOSMsgQ *msgq, unsigned char type, unsigned char *selector, ui
                     *buffer = _buffer;
                     *len = _len;
                 }
-                return 1;
+                return EOS_OK;
             }
         }
     }
@@ -90,11 +104,7 @@ int eos_msgq_find(EOSMsgQ *msgq, unsigned char type, unsigned char *selector, ui
         *buffer = NULL;
         *len = 0;
     }
-    return 0;
-}
-
-uint8_t eos_msgq_len(EOSMsgQ *msgq) {
-    return (uint8_t)(msgq->idx_w - msgq->idx_r);
+    return EOS_ERR_NOTFOUND;
 }
 
 void eos_bufq_init(EOSBufQ *bufq, unsigned char **array, uint8_t size) {
@@ -104,6 +114,10 @@ void eos_bufq_init(EOSBufQ *bufq, unsigned char **array, uint8_t size) {
     bufq->array = array;
 }
 
+uint8_t eos_bufq_len(EOSBufQ *bufq) {
+    return (uint8_t)(bufq->idx_w - bufq->idx_r);
+}
+
 int eos_bufq_push(EOSBufQ *bufq, unsigned char *buffer) {
     if ((uint8_t)(bufq->idx_w - bufq->idx_r) == bufq->size) return EOS_ERR_FULL;
 
@@ -116,7 +130,3 @@ unsigned char *eos_bufq_pop(EOSBufQ *bufq) {
 
     return bufq->array[IDX_MASK(bufq->idx_r++, bufq->size)];
 }
-
-uint8_t eos_bufq_len(EOSBufQ *bufq) {
-    return (uint8_t)(bufq->idx_w - bufq->idx_r);
-}