1 files changed, 33 insertions, 11 deletions
diff --git a/code/fe310/eos/trap_entry.S b/code/fe310/eos/trap_entry.S
index 650c45b..c3b7104 100644
--- a/code/fe310/eos/trap_entry.S
+++ b/code/fe310/eos/trap_entry.S
@@ -76,7 +76,7 @@ evtq_push:
   lbu x18, MSGQ_OFF_IDXR(x9)
   lbu x19, MSGQ_OFF_IDXW(x9)
   lbu x20, MSGQ_OFF_SIZE(x9)
-  
+
   sub x18, x19, x18
   andi x18, x18, 0xff
   beq x18, x20, 0f
@@ -107,8 +107,11 @@ i2s_handler_sd:
   li x18, PLIC_PRIORITY
   sw x0, 4*I2S_IRQ_SD_ID(x18)
 
+  la x9, _eos_i2s_fmt
+  lw x23, 0(x9)
+
+i2s_abuf_pop:
   # pop from spk buf -> x8
-  mv x8, x0
   la x9, _eos_i2s_spk_buf
   lhu x18, I2S_ABUF_OFF_IDXR(x9)
   lhu x19, I2S_ABUF_OFF_IDXW(x9)
@@ -121,14 +124,20 @@ i2s_handler_sd:
   lw x21, I2S_ABUF_OFF_ARRAY(x9)
   add x21, x21, x20
   lbu x8, 0(x21)
-
   addi x18, x18, 1
+  beqz x23, 0f
+  slli x8, x8, 8
+  lbu x22, 1(x21)
+  or x8, x8, x22
+  addi x18, x18, 1
+
+0:
   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)
@@ -145,7 +154,9 @@ i2s_handler_sd:
   beqz x21, i2s_decode
   li x18, (EOS_EVT_AUDIO | I2S_ETYPE_SPK)
   sb x18, MSGQ_ITEM_OFF_TYPE(x21)
- 
+
+  bnez x23, i2s_handler_sd_xchg
+
 i2s_decode:
   # aLaw decode -> x8
   xori x8, x8, 0x55
@@ -255,6 +266,10 @@ i2s_handler_sd_xchg:
   and x9, x9, x20
   sw x9, GPIO_OUTPUT_EN(x18)
 
+  la x9, _eos_i2s_fmt
+  lw x23, 0(x9)
+  bnez x23, i2s_abuf_push
+
 i2s_encode:
   # aLaw encode -> x8
   li x18, 0x800
@@ -286,6 +301,7 @@ i2s_encode:
   xori x8, x8, 0x55
   andi x8, x8, 0xff
 
+i2s_abuf_push:
   # push to mic buf
   la x9, _eos_i2s_mic_buf
   lhu x18, I2S_ABUF_OFF_IDXR(x9)
@@ -300,13 +316,19 @@ i2s_encode:
   addi x20, x20, -1
   and x20, x20, x19
   lw x21, I2S_ABUF_OFF_ARRAY(x9)
-  add x20, x21, x20
-
+  add x21, x21, x20
+  add x21, x21, x23
+  sb x8, 0(x21)
   addi x19, x19, 1
-  sh x19, I2S_ABUF_OFF_IDXW(x9)
-
-  sb x8, 0(x20)
   addi x18, x18, 1
+  beqz x23, 0f
+  srli x8, x8, 8
+  sb x8, -1(x21)
+  addi x19, x19, 1
+  addi x18, x18, 1
+
+0:
+  sh x19, I2S_ABUF_OFF_IDXW(x9)
 
   # check for push to event queue
   la x9, _eos_i2s_mic_wm