11 files changed, 264 insertions, 165 deletions

diff --git a/code/fe310/eos/ecp.c b/code/fe310/eos/ecp.c
index 44d9a47..85539f5 100644
--- a/code/fe310/eos/ecp.c
+++ b/code/fe310/eos/ecp.c
@@ -13,11 +13,11 @@
 
 static ECPSocket *_sock = NULL;
 
-static void timer_handler(unsigned char type, unsigned char *buffer, uint16_t len) {
+static void timer_handler(unsigned char type) {
     ecp_cts_t next = ecp_timer_exe(_sock);
     if (next) {
         uint32_t tick = next * (uint64_t)RTC_FREQ / 1000;
-        eos_timer_set(tick, 1);
+        eos_timer_set(tick, EOS_TIMER_ETYPE_ECP);
     }
 }
 
@@ -59,8 +59,8 @@ int ecp_init(ECPContext *ctx) {
     rv = ecp_ctx_create_vconn(ctx);
     if (rv) return rv;
     
+    eos_timer_set_handler(EOS_TIMER_ETYPE_ECP, timer_handler, EOS_EVT_FLAG_NET_BUF_ACQ);
     /* XXX */
-    eos_evtq_set_handler(EOS_EVT_TIMER, timer_handler, EOS_EVT_FLAG_NET_BUF_ACQ);
     // eos_net_set_handler(EOS_NET_DATA_PKT, packet_handler, 0);
     return ECP_OK;
 }
diff --git a/code/fe310/eos/event.c b/code/fe310/eos/event.c
index 66b1f4a..9e3fb90 100644
--- a/code/fe310/eos/event.c
+++ b/code/fe310/eos/event.c
@@ -13,8 +13,8 @@ EOSMsgQ _eos_event_q;
 static EOSMsgItem event_q_array[EOS_EVT_SIZE_Q];
 
 static eos_evt_fptr_t evt_handler[EOS_EVT_MAX_EVT];
-static uint16_t evt_handler_wrapper_acq = 0;
-static uint16_t evt_handler_flags_buf_acq = 0;
+static uint16_t evt_handler_wrapper_acq[EOS_EVT_MAX_EVT];
+static uint16_t evt_handler_flags_buf_acq[EOS_EVT_MAX_EVT];
 
 void eos_evtq_init(void) {
     int i;
@@ -42,38 +42,48 @@ void eos_evtq_bad_handler(unsigned char type, unsigned char *buffer, uint16_t le
     write(1, "error\n", 6);
 }
 
-static void evtq_handler_wrapper(unsigned char type, unsigned char *buffer, uint16_t len, uint16_t *flags_acq, uint16_t flag, eos_evt_fptr_t f) {
-    int ok = eos_net_acquire(*flags_acq & flag);
+static void evtq_handler_wrapper(unsigned char type, unsigned char *buffer, uint16_t len) {
+    unsigned char idx = ((type & EOS_EVT_MASK) >> 4) - 1;
+    uint16_t flag = (uint16_t)1 << ((type & ~EOS_EVT_MASK) - 1);
+    int ok;
+    
+    ok = eos_net_acquire(evt_handler_wrapper_acq[idx] & flag);
     if (ok) {
-        f(type, buffer, len);
+        evt_handler[idx](type, buffer, len);
         eos_net_release();
-        *flags_acq &= ~flag;
+        evt_handler_wrapper_acq[idx] &= ~flag;
     } else {
-        *flags_acq |= flag;
+        evt_handler_wrapper_acq[idx] |= flag;
         eos_evtq_push(type, buffer, len);
     }
 }
 
 static void evtq_handler(unsigned char type, unsigned char *buffer, uint16_t len) {
-    if (((type & EOS_EVT_MASK) >> 4) > EOS_EVT_MAX_EVT) {
+    unsigned char idx = ((type & EOS_EVT_MASK) >> 4) - 1;
+    uint16_t flag = (uint16_t)1 << ((type & ~EOS_EVT_MASK) - 1);
+    
+    if (idx >= EOS_EVT_MAX_EVT) {
         eos_evtq_bad_handler(type, buffer, len);
+        return;
+    }
+    if (flag & evt_handler_flags_buf_acq[idx]) {
+        evtq_handler_wrapper(type, buffer, len);
     } else {
-        unsigned char idx = ((type & EOS_EVT_MASK) >> 4) - 1;
-        uint16_t flag = (uint16_t)1 << idx;
-        if (flag & evt_handler_flags_buf_acq) {
-            evtq_handler_wrapper(type, buffer, len, &evt_handler_wrapper_acq, flag, evt_handler[idx]);
-        } else {
-            evt_handler[idx](type, buffer, len);
-        }
+        evt_handler[idx](type, buffer, len);
     }
 }
 
-void eos_evtq_set_handler(unsigned char type, eos_evt_fptr_t handler, uint8_t flags) {
-    if (flags) {
-        uint16_t flag = (uint16_t)1 << (((type & EOS_EVT_MASK) >> 4) - 1);
-        if (flags & EOS_EVT_FLAG_NET_BUF_ACQ) evt_handler_flags_buf_acq |= flag;
-    }
-    evt_handler[((type & EOS_EVT_MASK) >> 4) - 1] = handler;
+void eos_evtq_set_handler(unsigned char type, eos_evt_fptr_t handler) {
+    unsigned char idx = ((type & EOS_EVT_MASK) >> 4) - 1;
+    
+    if (idx < EOS_EVT_MAX_EVT) evt_handler[idx] = handler;
+}
+
+void eos_evtq_set_flags(unsigned char type, uint8_t flags) {
+    unsigned char idx = ((type & EOS_EVT_MASK) >> 4) - 1;
+    uint16_t flag = type & ~EOS_EVT_MASK ? (uint16_t)1 << ((type & ~EOS_EVT_MASK) - 1) : 0xFFFF;
+
+    if ((idx < EOS_EVT_MAX_EVT) && (flags & EOS_EVT_FLAG_NET_BUF_ACQ)) evt_handler_flags_buf_acq[idx] |= flag;
 }
 
 void eos_evtq_loop(void) {
diff --git a/code/fe310/eos/event.h b/code/fe310/eos/event.h
index 653d5c4..a9a98ef 100644
--- a/code/fe310/eos/event.h
+++ b/code/fe310/eos/event.h
@@ -8,5 +8,6 @@ void eos_evtq_init(void);
 int eos_evtq_push(unsigned char type, unsigned char *buffer, uint16_t len);
 void eos_evtq_pop(unsigned char *type, unsigned char **buffer, uint16_t *len);
 void eos_evtq_bad_handler(unsigned char type, unsigned char *buffer, uint16_t len);
-void eos_evtq_set_handler(unsigned char type, eos_evt_fptr_t handler, uint8_t flags);
+void eos_evtq_set_handler(unsigned char type, eos_evt_fptr_t handler);
+void eos_evtq_set_flags(unsigned char type, uint8_t flags);
 void eos_evtq_loop(void);
diff --git a/code/fe310/eos/i2s.c b/code/fe310/eos/i2s.c
index cb25493..d171a45 100644
--- a/code/fe310/eos/i2s.c
+++ b/code/fe310/eos/i2s.c
@@ -22,9 +22,13 @@ EOSABuf _eos_i2s_spk_buf;
 uint32_t _eos_i2s_ck_period = 0;
 uint32_t _eos_i2s_mic_volume = 0;
 uint32_t _eos_i2s_spk_volume = 0;
-static eos_evt_fptr_t evt_handler[I2S_MAX_HANDLER];
-uint32_t _eos_i2s_evt_enable[I2S_MAX_HANDLER];
-uint32_t _eos_i2s_wm[I2S_MAX_HANDLER];
+uint32_t _eos_i2s_mic_wm;
+uint32_t _eos_i2s_spk_wm;
+uint32_t _eos_i2s_mic_evt_enable;
+uint32_t _eos_i2s_spk_evt_enable;
+
+static eos_evt_fptr_t spk_evt_handler;
+static eos_evt_fptr_t mic_evt_handler;
 
 static void _abuf_init(EOSABuf *buf, uint8_t *array, uint16_t size) {
     buf->idx_r = 0;
@@ -77,14 +81,23 @@ static uint16_t _abuf_len(EOSABuf *buf) {
     return buf->idx_w - buf->idx_r;
 }
 
-static void audio_handler(unsigned char type, unsigned char *buffer, uint16_t len) {
-    type = type & ~EOS_EVT_MASK;
-    
-    if (type < I2S_MAX_HANDLER) {
-        evt_handler[type](type, buffer, len);
-        _eos_i2s_evt_enable[type] = 1;
-    } else {
-        eos_evtq_bad_handler(type, buffer, len);
+static void i2s_handler_evt(unsigned char type, unsigned char *buffer, uint16_t len) {
+    switch(type & ~EOS_EVT_MASK) {
+        case I2S_ETYPE_MIC:
+            mic_evt_handler(type, buffer, len);
+            clear_csr(mstatus, MSTATUS_MIE);
+            _eos_i2s_mic_evt_enable = 1;
+            set_csr(mstatus, MSTATUS_MIE);
+            break;
+        case I2S_ETYPE_SPK:
+            spk_evt_handler(type, buffer, len);
+            clear_csr(mstatus, MSTATUS_MIE);
+            _eos_i2s_spk_evt_enable = 1;
+            set_csr(mstatus, MSTATUS_MIE);
+            break;
+        default:
+            eos_evtq_bad_handler(type, buffer, len);
+            break;
     }
 }
 
@@ -144,24 +157,22 @@ void eos_i2s_init(uint32_t sample_rate) {
     I2S_PWM_REG_WS_SPK(PWM_CMP1)    = (_eos_i2s_ck_period + 1) * 32;
     I2S_PWM_REG_WS_SPK(PWM_CMP2)    = (_eos_i2s_ck_period + 1) * 33;
 
-    _eos_i2s_evt_enable[I2S_EVT_MIC] = 0;
-    _eos_i2s_evt_enable[I2S_EVT_SPK] = 0;
-    evt_handler[I2S_EVT_MIC] = NULL;
-    evt_handler[I2S_EVT_SPK] = NULL;
+    _eos_i2s_mic_evt_enable = 0;
+    _eos_i2s_spk_evt_enable = 0;
+    mic_evt_handler = eos_evtq_bad_handler;
+    spk_evt_handler = eos_evtq_bad_handler;
     
     eos_intr_set(I2S_IRQ_WS_ID, 0, NULL);
     eos_intr_set(I2S_IRQ_SD_ID, 0, NULL);
-    for (i=0; i<I2S_MAX_HANDLER; i++) {
-        evt_handler[i] = eos_evtq_bad_handler;
-    }
-    eos_evtq_set_handler(EOS_EVT_AUDIO, audio_handler, EOS_EVT_FLAG_NET_BUF_ACQ);
+    eos_evtq_set_handler(EOS_EVT_AUDIO, i2s_handler_evt);
+    eos_evtq_set_flags(EOS_EVT_AUDIO | I2S_ETYPE_MIC, EOS_EVT_FLAG_NET_BUF_ACQ);
 }
 
 extern void _eos_set_pwm(void);
 
 void eos_i2s_start(void) {
-    _eos_i2s_evt_enable[I2S_EVT_MIC] = 1;
-    _eos_i2s_evt_enable[I2S_EVT_SPK] = 1;
+    _eos_i2s_mic_evt_enable = 1;
+    _eos_i2s_spk_evt_enable = 1;
     
     eos_intr_set_priority(I2S_IRQ_WS_ID, I2S_IRQ_WS_PRIORITY);
 
@@ -229,13 +240,13 @@ void eos_i2s_mic_init(uint8_t *mic_arr, uint16_t mic_arr_size) {
 
 void eos_i2s_mic_set_handler(eos_evt_fptr_t wm_handler) {
     clear_csr(mstatus, MSTATUS_MIE);
-    evt_handler[I2S_EVT_MIC] = wm_handler;
+    mic_evt_handler = wm_handler;
     set_csr(mstatus, MSTATUS_MIE);
 }
 
 void eos_i2s_mic_set_wm(uint16_t wm) {
     clear_csr(mstatus, MSTATUS_MIE);
-    _eos_i2s_wm[I2S_EVT_MIC] = wm;
+    _eos_i2s_mic_wm = wm;
     set_csr(mstatus, MSTATUS_MIE);
     
 }
@@ -281,13 +292,13 @@ void eos_i2s_spk_init(uint8_t *spk_arr, uint16_t spk_arr_size) {
 
 void eos_i2s_spk_set_handler(eos_evt_fptr_t wm_handler) {
     clear_csr(mstatus, MSTATUS_MIE);
-    evt_handler[I2S_EVT_SPK] = wm_handler;
+    spk_evt_handler = wm_handler;
     set_csr(mstatus, MSTATUS_MIE);
 }
 
 void eos_i2s_spk_set_wm(uint16_t wm) {
     clear_csr(mstatus, MSTATUS_MIE);
-    _eos_i2s_wm[I2S_EVT_SPK] = wm;
+    _eos_i2s_spk_wm = wm;
     set_csr(mstatus, MSTATUS_MIE);
 }
 
diff --git a/code/fe310/eos/i2s_def.h b/code/fe310/eos/i2s_def.h
index 06fef72..1f7687a 100644
--- a/code/fe310/eos/i2s_def.h
+++ b/code/fe310/eos/i2s_def.h
@@ -1,6 +1,5 @@
-#define I2S_EVT_MIC             0x0
-#define I2S_EVT_SPK             0x1
-#define I2S_MAX_HANDLER         2
+#define I2S_ETYPE_MIC           1
+#define I2S_ETYPE_SPK           2
 
 #define I2S_PIN_CK              1       /* PWM 0.1 */
 #define I2S_PIN_CK_SW           21      /* PWM 1.2 */
diff --git a/code/fe310/eos/msgq.c b/code/fe310/eos/msgq.c
index 867bf94..0c5d8f1 100644
--- a/code/fe310/eos/msgq.c
+++ b/code/fe310/eos/msgq.c
@@ -1,4 +1,5 @@
 #include <stddef.h>
+#include <string.h>
 
 #include "eos.h"
 #include "msgq.h"
@@ -41,36 +42,38 @@ void eos_msgq_pop(EOSMsgQ *msgq, unsigned char *type, unsigned char **buffer, ui
     }
 }
 
-void eos_msgq_get(EOSMsgQ *msgq, unsigned char type, unsigned char **buffer, uint16_t *len) {
+void eos_msgq_get(EOSMsgQ *msgq, unsigned char type, unsigned char *selector, uint16_t sel_len, unsigned char **buffer, uint16_t *len) {
+    uint8_t i, j, idx;
+    
     if (msgq->idx_r == msgq->idx_w) {
         *buffer = NULL;
         *len = 0;
         return;
     }
-    
-    uint8_t idx = EOS_MSGQ_IDX_MASK(msgq->idx_r, msgq->size);
-    unsigned char _type = msgq->array[idx].type;
-    EOSMsgItem *tmp_item = &msgq->array[idx];
-    
-    *buffer = msgq->array[idx].buffer;
-    *len = msgq->array[idx].len;
-    if (_type == type) {
-        msgq->idx_r++;
-        return;
-    }
-    for (idx = msgq->idx_r + 1; EOS_MSGQ_IDX_LT(idx, msgq->idx_w); idx++) {
-        *tmp_item = msgq->array[EOS_MSGQ_IDX_MASK(idx, msgq->size)];
-        msgq->array[EOS_MSGQ_IDX_MASK(idx, msgq->size)].type = _type;
-        msgq->array[EOS_MSGQ_IDX_MASK(idx, msgq->size)].buffer = *buffer;
-        msgq->array[EOS_MSGQ_IDX_MASK(idx, msgq->size)].len = *len;
-        _type = tmp_item->type;
-        *buffer = tmp_item->buffer;
-        *len = tmp_item->len;
-        if (_type == type) {
+
+    idx = EOS_MSGQ_IDX_MASK(msgq->idx_r, msgq->size);
+    if (type == msgq->array[idx].type) {
+        *buffer = msgq->array[idx].buffer;
+        *len = msgq->array[idx].len;
+        if ((selector == NULL) || (sel_len == 0) || ((sel_len <= *len) && (memcmp(selector, *buffer, sel_len) == 0))) {
             msgq->idx_r++;
             return;
         }
     }
+    for (i = msgq->idx_r + 1; EOS_MSGQ_IDX_LT(i, msgq->idx_w); i++) {
+        idx = EOS_MSGQ_IDX_MASK(i, msgq->size);
+        if (type== msgq->array[idx].type) {
+            *buffer = msgq->array[idx].buffer;
+            *len = msgq->array[idx].len;
+            if ((selector == NULL) || (sel_len == 0) || ((sel_len <= *len) && (memcmp(selector, *buffer, sel_len) == 0))) {
+                for (j = i + 1; EOS_MSGQ_IDX_LT(j, msgq->idx_w); j++) {
+                    msgq->array[EOS_MSGQ_IDX_MASK(j - 1, msgq->size)] = msgq->array[EOS_MSGQ_IDX_MASK(j, msgq->size)];
+                }
+                msgq->idx_w--;
+                return;
+            }
+        }
+    }
     *buffer = NULL;
     *len = 0;
 }
diff --git a/code/fe310/eos/msgq.h b/code/fe310/eos/msgq.h
index 5a61a79..a280357 100644
--- a/code/fe310/eos/msgq.h
+++ b/code/fe310/eos/msgq.h
@@ -16,4 +16,4 @@ typedef struct EOSMsgQ {
 void eos_msgq_init(EOSMsgQ *msgq, EOSMsgItem *array, uint8_t size);
 int eos_msgq_push(EOSMsgQ *msgq, unsigned char type, unsigned char *buffer, uint16_t len);
 void eos_msgq_pop(EOSMsgQ *msgq, unsigned char *type, unsigned char **buffer, uint16_t *len);
-void eos_msgq_get(EOSMsgQ *msgq, unsigned char type, unsigned char **buffer, uint16_t *len);
\ No newline at end of file
+void eos_msgq_get(EOSMsgQ *msgq, unsigned char type, unsigned char *selector, uint16_t sel_len, unsigned char **buffer, uint16_t *len);
\ No newline at end of file
diff --git a/code/fe310/eos/net.c b/code/fe310/eos/net.c
index 0cbe9f6..b8a5056 100644
--- a/code/fe310/eos/net.c
+++ b/code/fe310/eos/net.c
@@ -217,32 +217,38 @@ static void net_handler_rts(void) {
 }
 
 static void net_handler_evt(unsigned char type, unsigned char *buffer, uint16_t len) {
-    if ((type & ~EOS_EVT_MASK) > EOS_NET_MAX_MTYPE) {
+    unsigned char idx = (type & ~EOS_EVT_MASK) - 1;
+    uint16_t buf_free = ((uint16_t)1 << idx) & evt_handler_flags_buf_free;
+    uint16_t buf_acq = ((uint16_t)1 << idx) & evt_handler_flags_buf_acq;
+
+    if (idx >= EOS_NET_MAX_MTYPE) {
         eos_evtq_bad_handler(type, buffer, len);
-    } else {
-        unsigned char idx = (type & ~EOS_EVT_MASK) - 1;
-        uint16_t buf_free = ((uint16_t)1 << idx) & evt_handler_flags_buf_free;
-        uint16_t buf_acq = ((uint16_t)1 << idx) & evt_handler_flags_buf_acq;
-
-        if (buf_free) {
-            eos_net_free(buffer, buf_acq);
-            buffer = NULL;
-            len = 0;
-        }
+        eos_net_free(buffer, 0);
+        return;
+    }
+    if (buf_free) {
+        eos_net_free(buffer, buf_acq);
+        buffer = NULL;
+        len = 0;
+    }
 
-        evt_handler[idx](type, buffer, len);
+    evt_handler[idx](type, buffer, len);
 
-        if (buf_free && buf_acq) eos_net_release();
-    }
+    if (buf_free && buf_acq) eos_net_release();
 }
 
-void eos_net_set_handler(unsigned char type, eos_evt_fptr_t handler, uint8_t flags) {
+void eos_net_set_handler(unsigned char mtype, eos_evt_fptr_t handler, uint8_t flags) {
+    if (mtype && (mtype <= EOS_NET_MAX_MTYPE)) {
+        mtype--;
+    } else {
+        return;
+    }
     if (flags) {
-        uint16_t flag = (uint16_t)1 << ((type & ~EOS_EVT_MASK) - 1);
+        uint16_t flag = (uint16_t)1 << mtype;
         if (flags & EOS_NET_FLAG_BUF_FREE) evt_handler_flags_buf_free |= flag;
         if (flags & EOS_NET_FLAG_BUF_ACQ) evt_handler_flags_buf_acq |= flag;
     }
-    evt_handler[(type & ~EOS_EVT_MASK) - 1] = handler;
+    evt_handler[mtype] = handler;
 }
 
 void eos_net_init(void) {
@@ -275,7 +281,7 @@ void eos_net_init(void) {
     for (i=0; i<EOS_NET_MAX_MTYPE; i++) {
         evt_handler[i] = eos_evtq_bad_handler;
     }
-    eos_evtq_set_handler(EOS_EVT_NET, net_handler_evt, 0);
+    eos_evtq_set_handler(EOS_EVT_NET, net_handler_evt);
 }
 
 void eos_net_start(uint32_t sckdiv) {
diff --git a/code/fe310/eos/timer.c b/code/fe310/eos/timer.c
index b287317..2bbeec8 100644
--- a/code/fe310/eos/timer.c
+++ b/code/fe310/eos/timer.c
@@ -11,30 +11,40 @@
 #define MIN(X, Y)               (((X) < (Y)) ? (X) : (Y))
 #define MAX(X, Y)               (((X) > (Y)) ? (X) : (Y))
 
-static eos_timer_fptr_t timer_ext_handler = NULL;
-volatile uint64_t timer_next = 0;
-volatile uint64_t timer_next_evt = 0;
+static eos_timer_fptr_t timer_handler[EOS_TIMER_MAX_ETYPE + 1];
+static uint64_t timer_next[EOS_TIMER_MAX_ETYPE + 1];
 
 extern EOSMsgQ _eos_event_q;
 
+static void timer_handler_evt(unsigned char type, unsigned char *buffer, uint16_t len) {
+    unsigned char idx = (type & ~EOS_EVT_MASK) - 1;
+
+    if ((idx < EOS_TIMER_MAX_ETYPE) && timer_handler[idx]) {
+        timer_handler[idx](type);
+    } else {
+        eos_evtq_bad_handler(type, buffer, len);
+    }
+}
+
 void eos_timer_handle(void) {
+    int i;
     volatile uint64_t *mtime = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIME);
-    volatile uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
+    uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
     uint64_t now = *mtime;
-
-    if (timer_next && (timer_next <= now)) {
-        uint32_t next = timer_ext_handler();
-        if (next) {
-            timer_next = now + next;
-        } else {
-            timer_next = 0;
+    uint64_t next = 0;
+
+    for (i = 0; i <= EOS_TIMER_MAX_ETYPE; i++) {
+        if (timer_next[i] && (timer_next[i] <= now)) {
+            timer_next[i] = 0;
+            if (i == EOS_TIMER_MAX_ETYPE) {
+                timer_handler[EOS_TIMER_MAX_ETYPE](0);
+            } else {
+                eos_msgq_push(&_eos_event_q, EOS_EVT_TIMER | i + 1, NULL, 0);
+            }
         }
+        next = next && timer_next[i] ? MIN(next, timer_next[i]) : (next ? next : timer_next[i]);
     }
-    if (timer_next_evt && (timer_next_evt <= now)) {
-        eos_msgq_push(&_eos_event_q, EOS_EVT_TIMER, NULL, 0);
-        timer_next_evt = 0;
-    }
-    *mtimecmp = (timer_next && timer_next_evt) ? MIN(timer_next, timer_next_evt) : (timer_next ? timer_next : timer_next_evt);
+    *mtimecmp = next;
     if (*mtimecmp == 0) clear_csr(mie, MIP_MTIP);
 }
 
@@ -43,51 +53,81 @@ void handle_m_time_interrupt(void) {
 }
 
 void eos_timer_init(void) {
+    int i;
     volatile uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
-    *mtimecmp = 0;
+
     clear_csr(mie, MIP_MTIP);
+    *mtimecmp = 0;
+    for (i = 0; i <= EOS_TIMER_MAX_ETYPE; i++) {
+        timer_next[i] = 0;
+        timer_handler[i] = NULL;
+    }
+    eos_evtq_set_handler(EOS_EVT_TIMER, timer_handler_evt);
 }
 
-void eos_timer_set(uint32_t tick, unsigned char is_evt) {
+void eos_timer_set(uint32_t tick, unsigned char evt) {
+    int i;
     volatile uint64_t *mtime = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIME);
-    volatile uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
-
+    uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
+    uint64_t next = 0;
+    uint64_t now;
+    
     clear_csr(mie, MIP_MTIP);
 
-    uint64_t now = *mtime;
-    uint64_t next = now + tick;
-    if (is_evt) {
-        if ((timer_next_evt == 0) || (next < timer_next_evt)) timer_next_evt = next;
-        next = timer_next ? MIN(timer_next, timer_next_evt) : timer_next_evt;
-    } else if (timer_ext_handler) {
-        if ((timer_next == 0) || (next < timer_next)) timer_next = next;
-        next = timer_next_evt ? MIN(timer_next, timer_next_evt) : timer_next;
+    now = *mtime;
+    if (evt && (evt <= EOS_TIMER_MAX_ETYPE)) {
+        evt--;
+    } else if (evt == 0) {
+        evt = EOS_TIMER_MAX_ETYPE;
+    } else {
+        return;
+    }
+    if ((timer_next[evt] == 0) || (now + tick < timer_next[evt])) timer_next[evt] = now + tick;
+    for (i = 0; i <= EOS_TIMER_MAX_ETYPE; i++) {
+        next = next && timer_next[i] ? MIN(next, timer_next[i]) : (next ? next : timer_next[i]);
     }
-    if ((*mtimecmp == 0) || (next < *mtimecmp)) *mtimecmp = next;
+    *mtimecmp = next;
 
     if (*mtimecmp != 0) set_csr(mie, MIP_MTIP);
 }
 
-void eos_timer_clear(unsigned char is_evt) {
-    volatile uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
+void eos_timer_clear(unsigned char evt) {
+    int i;
+    uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
+    uint64_t next = 0;
 
     clear_csr(mie, MIP_MTIP);
 
-    if (is_evt) {
-        timer_next_evt = 0;
-        *mtimecmp = timer_next;
+    if (evt && (evt <= EOS_TIMER_MAX_ETYPE)) {
+        evt--;
+    } else if (evt == 0) {
+        evt = EOS_TIMER_MAX_ETYPE;
     } else {
-        timer_next = 0;
-        *mtimecmp = timer_next_evt;
+        return;
     }
+    timer_next[evt] = 0;
+    for (i = 0; i <= EOS_TIMER_MAX_ETYPE; i++) {
+        next = next && timer_next[i] ? MIN(next, timer_next[i]) : (next ? next : timer_next[i]);
+    }
+    *mtimecmp = next;
 
     if (*mtimecmp != 0) set_csr(mie, MIP_MTIP);
 }
 
-void eos_timer_set_handler(eos_timer_fptr_t handler) {
-    volatile uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
+void eos_timer_set_handler(unsigned char evt, eos_timer_fptr_t handler, uint8_t flags) {
+    uint64_t *mtimecmp = (uint64_t *) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
 
     clear_csr(mie, MIP_MTIP);
-    timer_ext_handler = handler;
+    
+    if (evt && (evt <= EOS_TIMER_MAX_ETYPE)) {
+        evt--;
+    } else if (evt == 0) {
+        evt = EOS_TIMER_MAX_ETYPE;
+    } else {
+        return;
+    }
+    timer_handler[evt] = handler;
+    if (flags) eos_evtq_set_flags(EOS_EVT_TIMER | evt, flags);
+
     if (*mtimecmp != 0) set_csr(mie, MIP_MTIP);
 }
diff --git a/code/fe310/eos/timer.h b/code/fe310/eos/timer.h
index 929283b..34512fa 100644
--- a/code/fe310/eos/timer.h
+++ b/code/fe310/eos/timer.h
@@ -1,7 +1,12 @@
 #include <stdint.h>
 
-typedef uint32_t (*eos_timer_fptr_t) (void);
+#define EOS_TIMER_ETYPE_ECP     1
+#define EOS_TIMER_ETYPE_USER    2
+
+#define EOS_TIMER_MAX_ETYPE     4
+
+typedef void (*eos_timer_fptr_t) (unsigned char);
 
 void eos_timer_init(void);
-void eos_timer_set(uint32_t tick, unsigned char is_evt);
-void eos_timer_set_handler(eos_timer_fptr_t handler);
+void eos_timer_set(uint32_t tick, unsigned char evt);
+void eos_timer_set_handler(unsigned char evt, eos_timer_fptr_t handler, uint8_t flags);
diff --git a/code/fe310/eos/trap_entry.S b/code/fe310/eos/trap_entry.S
index 432e84a..650c45b 100644
--- a/code/fe310/eos/trap_entry.S
+++ b/code/fe310/eos/trap_entry.S
@@ -75,25 +75,26 @@ evtq_push:
   la x9, _eos_event_q
   lbu x18, MSGQ_OFF_IDXR(x9)
   lbu x19, MSGQ_OFF_IDXW(x9)
-  lbu x8, MSGQ_OFF_SIZE(x9)
-
+  lbu x20, MSGQ_OFF_SIZE(x9)
+  
   sub x18, x19, x18
-  beq x18, x8, 0f
+  andi x18, x18, 0xff
+  beq x18, x20, 0f
 
-  addi x8, x8, -1
-  and x8, x8, x19
+  addi x20, x20, -1
+  and x20, x20, x19
   li x18, MSGQ_ITEM_SIZE
-  mul x8, x8, x18
-  lw x18, MSGQ_OFF_ARRAY(x9)
-  add x8, x8, x18
+  mul x20, x20, x18
+  lw x21, MSGQ_OFF_ARRAY(x9)
+  add x21, x21, x20
 
   addi x19, x19, 1
   sb x19, MSGQ_OFF_IDXW(x9)
-  jalr x0, x20
+  jalr x0, x22
 
 0:
-  mv x8, x0
-  jalr x0, x20
+  mv x20, x0
+  jalr x0, x21
 
 i2s_handler_sd:
   # exit if too early
@@ -111,18 +112,41 @@ i2s_handler_sd:
   la x9, _eos_i2s_spk_buf
   lhu x18, I2S_ABUF_OFF_IDXR(x9)
   lhu x19, I2S_ABUF_OFF_IDXW(x9)
+  lhu x20, I2S_ABUF_OFF_SIZE(x9)
+
   beq x18, x19, i2s_handler_sd_xchg
-  lhu x19, I2S_ABUF_OFF_SIZE(x9)
 
-  addi x19, x19, -1
-  and x19, x19, x18
-  lw x8, I2S_ABUF_OFF_ARRAY(x9)
-  add x19, x19, x8
-  lbu x8, 0(x19)
+  addi x20, x20, -1
+  and x20, x20, x18
+  lw x21, I2S_ABUF_OFF_ARRAY(x9)
+  add x21, x21, x20
+  lbu x8, 0(x21)
 
   addi x18, x18, 1
   sh x18, I2S_ABUF_OFF_IDXR(x9)
+  
+  li x21, 0xffff
+  sub x18, x19, x18
+  and x18, x18, x21
+  
+  # check for push to event queue
+  la x9, _eos_i2s_spk_wm
+  lw x20, 0(x9)
+  beqz x20, i2s_decode
+  bgtu x18, x20, i2s_decode
+
+  la x9, _eos_i2s_spk_evt_enable
+  lw x18, 0(x9)
+  beqz x18, i2s_decode
+  sw x0, 0(x9)
 
+  # push to event queue
+  jal x22, evtq_push
+  beqz x21, i2s_decode
+  li x18, (EOS_EVT_AUDIO | I2S_ETYPE_SPK)
+  sb x18, MSGQ_ITEM_OFF_TYPE(x21)
+ 
+i2s_decode:
   # aLaw decode -> x8
   xori x8, x8, 0x55
   andi x9, x8, 0x80
@@ -231,6 +255,7 @@ i2s_handler_sd_xchg:
   and x9, x9, x20
   sw x9, GPIO_OUTPUT_EN(x18)
 
+i2s_encode:
   # aLaw encode -> x8
   li x18, 0x800
   li x19, 7
@@ -270,36 +295,35 @@ i2s_handler_sd_xchg:
 
   sub x18, x19, x18
   and x18, x18, x21
-  beq x18, x20, 0f
-
-  addi x19, x19, 1
-  sh x19, I2S_ABUF_OFF_IDXW(x9)
+  beq x18, x20, i2s_handler_sd_exit
 
   addi x20, x20, -1
   and x20, x20, x19
-  lw x19, I2S_ABUF_OFF_ARRAY(x9)
-  add x20, x19, x20
+  lw x21, I2S_ABUF_OFF_ARRAY(x9)
+  add x20, x21, x20
+
+  addi x19, x19, 1
+  sh x19, I2S_ABUF_OFF_IDXW(x9)
 
   sb x8, 0(x20)
   addi x18, x18, 1
 
-0:
   # check for push to event queue
-  la x9, _eos_i2s_wm
+  la x9, _eos_i2s_mic_wm
   lw x20, 0(x9)
   beqz x20, i2s_handler_sd_exit
   bltu x18, x20, i2s_handler_sd_exit
 
-  la x9, _eos_i2s_evt_enable
+  la x9, _eos_i2s_mic_evt_enable
   lw x18, 0(x9)
   beqz x18, i2s_handler_sd_exit
   sw x0, 0(x9)
 
   # push to event queue
-  jal x20, evtq_push
-  beqz x8, i2s_handler_sd_exit
-  li x18, (EOS_EVT_AUDIO | I2S_EVT_MIC)
-  sb x18, MSGQ_ITEM_OFF_TYPE(x8)
+  jal x22, evtq_push
+  beqz x21, i2s_handler_sd_exit
+  li x18, (EOS_EVT_AUDIO | I2S_ETYPE_MIC)
+  sb x18, MSGQ_ITEM_OFF_TYPE(x21)
 
 i2s_handler_sd_exit:
   # complete