shift register i2s impl

5 files changed, 199 insertions, 293 deletions

diff --git a/code/fe310/eos/eos.c b/code/fe310/eos/eos.c
index ad80a47..3ad3b68 100644
--- a/code/fe310/eos/eos.c
+++ b/code/fe310/eos/eos.c
@@ -13,5 +13,5 @@ void eos_init(void) {
 }
 
 void eos_start(void) {
-    eos_net_start(511);
+    eos_net_start(15);
 }
\ No newline at end of file
diff --git a/code/fe310/eos/i2s.c b/code/fe310/eos/i2s.c
index 5d68f70..9ec5a71 100644
--- a/code/fe310/eos/i2s.c
+++ b/code/fe310/eos/i2s.c
@@ -18,7 +18,7 @@
 EOSABuf _eos_i2s_mic_buf;
 EOSABuf _eos_i2s_spk_buf;
 uint32_t _eos_i2s_ck_period = 0;
-uint32_t _eos_i2s_mic_volume = 2;
+uint32_t _eos_i2s_mic_volume = 3;
 uint32_t _eos_i2s_spk_volume = 3;
 static eos_evt_fptr_t evt_handler[I2S_MAX_HANDLER];
 uint32_t _eos_i2s_evt_enable[I2S_MAX_HANDLER];
@@ -104,6 +104,14 @@ void eos_i2s_init(uint32_t sample_rate) {
     GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << I2S_PIN_WS);
     GPIO_REG(GPIO_OUTPUT_XOR)   &= ~(1 << I2S_PIN_WS);
 
+    GPIO_REG(GPIO_INPUT_EN)     &= ~(1 << I2S_PIN_CKE);
+    GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << I2S_PIN_CKE);
+    GPIO_REG(GPIO_OUTPUT_XOR)   &= ~(1 << I2S_PIN_CKE);
+
+    GPIO_REG(GPIO_INPUT_EN)     &= ~(1 << I2S_PIN_CKF);
+    GPIO_REG(GPIO_OUTPUT_EN)    |=  (1 << I2S_PIN_CKF);
+    GPIO_REG(GPIO_OUTPUT_XOR)   &= ~(1 << I2S_PIN_CKF);
+
     GPIO_REG(GPIO_INPUT_EN)     |=  (1 << I2S_PIN_SD);
     GPIO_REG(GPIO_OUTPUT_EN)    &= ~(1 << I2S_PIN_SD);
     GPIO_REG(GPIO_PULLUP_EN)    &= ~(1 << I2S_PIN_SD);
@@ -114,23 +122,23 @@ void eos_i2s_init(uint32_t sample_rate) {
     I2S_PWM_REG_CK(PWM_COUNT)   = 0;
     I2S_PWM_REG_CK(PWM_CMP0)    = _ck_period_scaled;
     I2S_PWM_REG_CK(PWM_CMP1)    = _ck_period_scaled / 2;
-    I2S_PWM_REG_CK(PWM_CMP2)    = _ck_period_scaled / 8;
+    I2S_PWM_REG_CK(PWM_CMP2)    = 0;
+    I2S_PWM_REG_CK(PWM_CMP3)    = 0;
 
     I2S_PWM_REG_WS(PWM_CFG)     = 0;
     I2S_PWM_REG_WS(PWM_COUNT)   = 0;
     I2S_PWM_REG_WS(PWM_CMP0)    = (_eos_i2s_ck_period + 1) * 64 - 1;
     I2S_PWM_REG_WS(PWM_CMP1)    = (_eos_i2s_ck_period + 1) * 32;
-    I2S_PWM_REG_WS(PWM_CMP2)    = (_eos_i2s_ck_period + 1) * 64 - 512;
+    I2S_PWM_REG_WS(PWM_CMP2)    = (_eos_i2s_ck_period + 1) * _eos_i2s_mic_volume;
+    I2S_PWM_REG_WS(PWM_CMP3)    = I2S_PWM_REG_WS(PWM_CMP2) + (_eos_i2s_ck_period + 1) * 16;
 
     _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_intr_set(I2S_IRQ_SD_ID, 0, NULL);
-    eos_intr_set(I2S_IRQ_CK_ID, 0, NULL);
     eos_intr_set(I2S_IRQ_WS_ID, 0, NULL);
-    eos_intr_set(I2S_IRQ_CI_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;
     }
@@ -141,22 +149,24 @@ void eos_i2s_start(void) {
     _eos_i2s_evt_enable[I2S_EVT_MIC] = 1;
     _eos_i2s_evt_enable[I2S_EVT_SPK] = 1;
     
-    eos_intr_set_priority(I2S_IRQ_SD_ID, I2S_IRQ_SD_PRIORITY);
-    eos_intr_set_priority(I2S_IRQ_CK_ID, I2S_IRQ_CK_PRIORITY);
     eos_intr_set_priority(I2S_IRQ_WS_ID, I2S_IRQ_WS_PRIORITY);
-    eos_intr_set_priority(I2S_IRQ_CI_ID, I2S_IRQ_CI_PRIORITY);
 
     I2S_PWM_REG_CK(PWM_COUNT)   = 0;
     I2S_PWM_REG_WS(PWM_COUNT)   = 0;
 
     I2S_PWM_REG_CK(PWM_CFG)     = PWM_CFG_ENALWAYS | PWM_CFG_ZEROCMP | I2S_PWM_SCALE_CK;
-    I2S_PWM_REG_WS(PWM_CFG)     = PWM_CFG_ENALWAYS | PWM_CFG_ZEROCMP;
+    I2S_PWM_REG_WS(PWM_CFG)     = PWM_CFG_ENALWAYS | PWM_CFG_ZEROCMP | PWM_CFG_CMP2GANG;
 
     GPIO_REG(GPIO_IOF_SEL)      |=  (1 << I2S_PIN_CK);
     GPIO_REG(GPIO_IOF_EN)       |=  (1 << I2S_PIN_CK);
     
     GPIO_REG(GPIO_IOF_SEL)      |=  (1 << I2S_PIN_WS);
     GPIO_REG(GPIO_IOF_EN)       |=  (1 << I2S_PIN_WS);
+
+    GPIO_REG(GPIO_IOF_SEL)      |=  (1 << I2S_PIN_CKE);
+    GPIO_REG(GPIO_IOF_EN)       |=  (1 << I2S_PIN_CKE);
+
+    GPIO_REG(GPIO_OUTPUT_VAL)   |= (1 << I2S_PIN_CKF);
 }
 
 void eos_i2s_stop(void) {
@@ -165,10 +175,8 @@ void eos_i2s_stop(void) {
     I2S_PWM_REG_CK(PWM_COUNT)   = 0;
     I2S_PWM_REG_WS(PWM_COUNT)   = 0;
 
-    eos_intr_set_priority(I2S_IRQ_SD_ID, 0);
-    eos_intr_set_priority(I2S_IRQ_CK_ID, 0);
     eos_intr_set_priority(I2S_IRQ_WS_ID, 0);
-    eos_intr_set_priority(I2S_IRQ_CI_ID, 0);
+    eos_intr_set_priority(I2S_IRQ_SD_ID, 0);
     eos_intr_mask(0);
     eos_i2s_mic_init(NULL, 0);
     eos_i2s_spk_init(NULL, 0);
@@ -179,7 +187,11 @@ void eos_i2s_stop(void) {
     GPIO_REG(GPIO_IOF_EN)       &= ~(1 << I2S_PIN_WS);
     GPIO_REG(GPIO_IOF_SEL)      &= ~(1 << I2S_PIN_WS);
     
-    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~((1 << I2S_PIN_CK) | (1 << I2S_PIN_WS));
+    GPIO_REG(GPIO_IOF_EN)       &= ~(1 << I2S_PIN_CKE);
+    GPIO_REG(GPIO_IOF_SEL)      &= ~(1 << I2S_PIN_CKE);
+
+    GPIO_REG(GPIO_OUTPUT_VAL)   &= ~((1 << I2S_PIN_CK) | (1 << I2S_PIN_WS) | (1 << I2S_PIN_CKE));
+    GPIO_REG(GPIO_OUTPUT_VAL)   |= (1 << I2S_PIN_CKF);
 }
 
 void eos_i2s_mic_init(uint8_t *mic_arr, uint16_t mic_arr_size) {
diff --git a/code/fe310/eos/i2s_def.h b/code/fe310/eos/i2s_def.h
index 4a7fd85..2b9d852 100644
--- a/code/fe310/eos/i2s_def.h
+++ b/code/fe310/eos/i2s_def.h
@@ -2,26 +2,22 @@
 #define I2S_EVT_SPK             0x1
 #define I2S_MAX_HANDLER         2
 
-#define I2S_PIN_SD              12  // pin 18
-// #define I2S_PIN_CK              19  // pin 3
 #define I2S_PIN_CK              1   // pin 9
+#define I2S_PIN_CKE             12  // pin 18
+#define I2S_PIN_CKF             18  // pin 2
+#define I2S_PIN_SD              20  // pin 4
 #define I2S_PIN_WS              11  // pin 17
 
 #define I2S_PIN_LR              13  // pin 19
 
-#define I2S_IRQ_SD_ID           (INT_PWM0_BASE + 2)
-#define I2S_IRQ_CK_ID           (INT_PWM0_BASE + 0)
 #define I2S_IRQ_WS_ID           (INT_PWM2_BASE + 0)
-#define I2S_IRQ_CI_ID           (INT_PWM2_BASE + 2)
+#define I2S_IRQ_SD_ID           (INT_PWM2_BASE + 3)
 
-#define I2S_IRQ_SD_PRIORITY     7
-#define I2S_IRQ_CK_PRIORITY     6
 #define I2S_IRQ_WS_PRIORITY     6
-#define I2S_IRQ_CI_PRIORITY     7
-#define I2S_IRQ_MASK            5
+#define I2S_IRQ_SD_PRIORITY     7
 
 #define I2S_SMPL_BITS           13
-#define I2S_SMPL_BITS_S         5
+#define I2S_IDLE_CYCLES         8
 
 #define I2S_PWM_SCALE_CK        2
 #define I2S_PWM_SCALE_CK_MASK   0x0003
diff --git a/code/fe310/eos/interrupt.c b/code/fe310/eos/interrupt.c
index 28bd8d1..706d96b 100644
--- a/code/fe310/eos/interrupt.c
+++ b/code/fe310/eos/interrupt.c
@@ -15,14 +15,12 @@ static plic_instance_t plic;
 static eos_intr_fptr_t ext_interrupt_handler[PLIC_NUM_INTERRUPTS];
 
 uintptr_t eos_intr_handle(uintptr_t int_num) {
-    // plic_source int_num  = PLIC_claim_interrupt(&plic);
     if ((int_num >=1) && (int_num < PLIC_NUM_INTERRUPTS) && (ext_interrupt_handler[int_num])) {
         ext_interrupt_handler[int_num]();
     } else {
         write(1, "error\n", 6);
         exit(int_num);
     }
-    // PLIC_complete_interrupt(&plic, int_num);
     return int_num;
 }
 
diff --git a/code/fe310/eos/trap_entry.S b/code/fe310/eos/trap_entry.S
index 0c5278b..cda534e 100644
--- a/code/fe310/eos/trap_entry.S
+++ b/code/fe310/eos/trap_entry.S
@@ -17,6 +17,7 @@
 #define GPIO_CTRL_ADDR    0x10012000
 #include "sifive/devices/gpio.h"
 
+#define SPI0_CTRL_ADDR    0x10014000
 #define SPI1_CTRL_ADDR    0x10024000
 #include "sifive/devices/spi.h"
 
@@ -42,51 +43,27 @@
   .section      .data.entry
   .align 2
 
-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)
-
-  sub x18, x19, x18
-  beq x18, x8, 1f
-
-  addi x8, x8, -1
-  and x8, x8, x19
-  li x18, MSGQ_ITEM_SIZE
-  mul x8, x8, x18
-  lw x18, MSGQ_OFF_ARRAY(x9)
-  add x8, x8, x18
-
-  addi x19, x19, 1
-  sb x19, MSGQ_OFF_IDXW(x9)
-  jalr x0, x20
-
-1:  
-  mv x8, x0
-  jalr x0, x20
-
 .global eos_trap_entry
 eos_trap_entry:
-  addi sp, sp, -4*REGBYTES
+  addi sp, sp, -8*REGBYTES
   STORE x8, 0*REGBYTES(sp)
   STORE x9, 1*REGBYTES(sp)
   STORE x18, 2*REGBYTES(sp)
   STORE x19, 3*REGBYTES(sp)
+  STORE x20, 4*REGBYTES(sp)
+  STORE x21, 5*REGBYTES(sp)
+  STORE x22, 6*REGBYTES(sp)
+  STORE x23, 7*REGBYTES(sp)
 
   csrr x8, mcause
   li x18, MCAUSE_EXT
   bne x8, x18, handler
   li x18, PLIC_CLAIM
   lw x9, 0(x18)
-  li x18, I2S_IRQ_SD_ID
-  beq x9, x18, i2s_handler_sd
-  li x18, I2S_IRQ_CK_ID
-  beq x9, x18, i2s_handler_ck
   li x18, I2S_IRQ_WS_ID
   beq x9, x18, i2s_handler_ws
-  li x18, I2S_IRQ_CI_ID
-  beq x9, x18, i2s_handler_ci
+  li x18, I2S_IRQ_SD_ID
+  beq x9, x18, i2s_handler_sd
 #ifdef EOS_WITH_SPI_ASM
   li x18, INT_SPI1_BASE
   beq x9, x18, spi_handler_xchg
@@ -97,125 +74,112 @@ eos_trap_entry:
 #endif
   j handler
 
-i2s_handler_sd:
-  # exit if too early
-  li x18, I2S_PWM_CTRL_ADDR_CK
-  lw x8, PWM_COUNT(x18)
-  lw x9, PWM_CMP2(x18)
-  srli x9, x9, I2S_PWM_SCALE_CK
-  bltu x8, x9, i2s_handler_sd_exit_
+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)
 
-  # read mic value -> x8
-  li x18, GPIO_CTRL_ADDR
-  lw x8, GPIO_INPUT_VAL(x18)
+  sub x18, x19, x18
+  beq x18, x8, 0f
+
+  addi x8, x8, -1
+  and x8, x8, x19
+  li x18, MSGQ_ITEM_SIZE
+  mul x8, x8, x18
+  lw x18, MSGQ_OFF_ARRAY(x9)
+  add x8, x8, x18
 
-  # set bit as ws pwm counter/(ck period + 1) - sd volume -> x9
-  li x18, I2S_PWM_CTRL_ADDR_WS
-  lw x9, PWM_COUNT(x18)
-  la x18, _eos_i2s_ck_period
-  lw x19, 0(x18)
   addi x19, x19, 1
-  divu x9, x9, x19
-  la x18, _eos_i2s_mic_volume
-  lw x19, 0(x18)
-  sub x9, x9, x19
-  
-  # disable sd irq
-  li x18, PLIC_PRIORITY
-  sw x0, 4*I2S_IRQ_SD_ID(x18)
-  
-  # store x20
-  addi sp, sp, -1*REGBYTES
-  STORE x20, 0*REGBYTES(sp)
+  sb x19, MSGQ_OFF_IDXW(x9)
+  jalr x0, x20
 
-  # bit <= 0 or bit > 32 -> exit
-  li x18, 32
-  bge x0, x9, i2s_handler_sd_exit
-  bge x9, x18, i2s_handler_sd_exit
+0:
+  mv x8, x0
+  jalr x0, x20
 
-  # sample |= sd gpio value << (32 - bit) -> x8
-  # sample_mask |= 1 << (32 - bit) -> x20
-  li x18, (0x1 << I2S_PIN_SD)
-  and x8, x8, x18
-  srli x8, x8, I2S_PIN_SD
+i2s_handler_sd:
+  # exit if too early
+  li x18, I2S_PWM_CTRL_ADDR_WS
+  lw x8, PWM_COUNT(x18)
+  lw x9, PWM_CMP3(x18)
+  bltu x8, x9, i2s_handler_sd_exit
+
+  # read shift reg value -> x8
+  mv x8, x0
+  li x23, GPIO_CTRL_ADDR
+  lw x22, GPIO_OUTPUT_VAL(x23)
+  li x9, (0x1 << I2S_PIN_CKF)
+  or x22, x22, x9
+  not x9, x9
+  and x21, x22, x9
+  li x20, (0x1 << I2S_PIN_SD)
+  li x19, (32 - I2S_SMPL_BITS)
   li x18, 32
-  sub x19, x18, x9
-  sll x8, x8, x19
-  li x20, 1
-  sll x20, x20, x19
-  la x18, sd_sample_mask
-  lw x19, 0(x18)
-  or x19, x19, x20
-  sw x19, 0(x18)
-  mv x20, x19
-  la x18, sd_sample
-  lw x19, 0(x18)
-  or x19, x19, x8
-  sw x19, 0(x18)
-  mv x8, x19
-
-  # bit < I2S_SMPL_BITS -> exit
-  li x18, I2S_SMPL_BITS
-  blt x9, x18, i2s_handler_sd_exit
-
-  # disable ck irq
-  li x18, PLIC_PRIORITY
-  sw x0, 4*I2S_IRQ_CK_ID(x18)
+  
+0:
+  addi x18, x18, -1
+  # ck high
+  sw x21, GPIO_OUTPUT_VAL(x23)
 
-  # reset irq mask
-  li x18, PLIC_THRESHOLD
-  sw x0, 0(x18)
+  # idle
+  li x9, I2S_IDLE_CYCLES
+1:
+  addi x9, x9, -1
+  bnez x9, 1b
+  
+  lw x9, GPIO_INPUT_VAL(x23)
+  # ck low
+  sw x22, GPIO_OUTPUT_VAL(x23)
+
+  and x9, x9, x20
+  srli x9, x9, I2S_PIN_SD
+  sll x9, x9, x18
+  or x8, x8, x9
+  beq x19, x18, 3f
 
-  # reset sample / sample_mask
-  la x18, sd_sample
-  sw x0, 0(x18)
-  la x18, sd_sample_mask
-  sw x0, 0(x18)
+  # idle
+  li x9, I2S_IDLE_CYCLES
+2:
+  addi x9, x9, -1
+  bnez x9, 2b
   
-  # correct for missing bits
-  # la x18, sd_sample_save
-  li x19, 0x80000000
-  srai x19, x19, I2S_SMPL_BITS_S-1
-  and x20, x20, x19
-  beq x19, x20, 0f
-  # 1. drop missing
-  # j i2s_handler_sd_exit
-  # 2. zero missing sample
-  # mv x8, x0
-  # 3. put saved sample to missing
-  # lw x8, 0(x18)
-0:
-  # sw x8, 0(x18)
-  srai x8, x8, 32-I2S_SMPL_BITS
+  j 0b
 
+3:
+  srai x8, x8, (32 - I2S_SMPL_BITS)
+  
+  # disable sd irq
+  li x18, PLIC_PRIORITY
+  sw x0, 4*I2S_IRQ_SD_ID(x18)
+  
   # aLaw encode -> x8
   li x18, 0x800
   li x19, 7
-  bgez x8, 1f
+  bgez x8, 0f
   neg x8, x8
   lui x9, 0x80000
   or x8, x8, x9
-1:
+0:
   and x9, x8, x18
-  beq x9, x18, 2f
-  beqz x19, 2f
+  beq x9, x18, 1f
+  beqz x19, 1f
   srli x18, x18, 1
   addi x19, x19, -1
-  j 1b
-2:
+  j 0b
+1:
   mv x9, x19
-  bnez x9, 3f
+  bnez x9, 2f
   addi x9, x9, 1
-3:
+2:
   sra x8, x8, x9
   li x9, 0x8000000f
   and x8, x8, x9
   slli x19, x19, 4
   or x8, x8, x19
-  bgez x8, 4f
-  ori x8, x8, 0x80
-  
-4:
+  bgez x8, 3f
+  ori x8, x8, 0x80 
+3:
   xori x8, x8, 0x55
   andi x8, x8, 0xff
 
@@ -226,7 +190,7 @@ i2s_handler_sd:
   lhu x20, I2S_ABUF_OFF_SIZE(x9)
 
   sub x18, x19, x18
-  beq x18, x20, 5f
+  beq x18, x20, 0f
 
   addi x19, x19, 1
   sh x19, I2S_ABUF_OFF_IDXW(x9)
@@ -239,88 +203,30 @@ i2s_handler_sd:
   sb x8, 0(x20)
   addi x18, x18, 1
   
-5:
+0:
   # check for push to event queue
   la x9, _eos_i2s_wm
   lw x20, 0(x9)
-  beqz x20, i2s_handler_sd_exit
-  bltu x18, x20, i2s_handler_sd_exit
+  beqz x20, i2s_handler_sd_out
+  bltu x18, x20, i2s_handler_sd_out
 
   la x9, _eos_i2s_evt_enable
   lw x18, 0(x9)
-  beqz x18, i2s_handler_sd_exit
+  beqz x18, i2s_handler_sd_out
   sw x0, 0(x9)
 
   # push to event queue
   jal x20, evtq_push
-  beqz x8, i2s_handler_sd_exit
+  beqz x8, i2s_handler_sd_out
   li x18, (EOS_EVT_AUDIO | I2S_EVT_MIC)
   sb x18, MSGQ_ITEM_OFF_CMD(x8)
-  
-i2s_handler_sd_exit:
-  LOAD x20, 0*REGBYTES(sp)
-  addi sp, sp, 1*REGBYTES
-
-i2s_handler_sd_exit_:
-  # complete
-  li x18, I2S_IRQ_SD_ID
-  li x19, PLIC_CLAIM
-  sw x18, 0(x19)
-
-  # exit
-  j trap_exit_data
-  
-i2s_handler_ck:
-  # enable sd irq
-  li x18, PLIC_PRIORITY
-  li x19, I2S_IRQ_SD_PRIORITY
-  sw x19, 4*I2S_IRQ_SD_ID(x18)
-
-  # complete
-  li x18, I2S_IRQ_CK_ID
-  li x19, PLIC_CLAIM
-  sw x18, 0(x19)
-
-  # exit
-  j trap_exit_data
-  
-i2s_handler_ws:
-  # enable ci irq
-  li x18, PLIC_PRIORITY
-  li x19, I2S_IRQ_CI_PRIORITY
-  sw x19, 4*I2S_IRQ_CI_ID(x18)
-
-  # complete
-  li x18, I2S_IRQ_WS_ID
-  li x19, PLIC_CLAIM
-  sw x18, 0(x19)
-
-  # exit
-  j trap_exit_data
-
-i2s_handler_ci:
-  # exit if too early
-  li x18, I2S_PWM_CTRL_ADDR_WS
-  lw x8, PWM_COUNT(x18)
-  lw x9, PWM_CMP2(x18)
-  bltu x8, x9, i2s_handler_ci_exit
-
-  # disable ci/enable ck irq
-  li x18, PLIC_PRIORITY
-  sw x0, 4*I2S_IRQ_CI_ID(x18)
-  li x19, I2S_IRQ_CK_PRIORITY
-  sw x19, 4*I2S_IRQ_CK_ID(x18)
-
-  # set irq mask
-  li x18, PLIC_THRESHOLD
-  li x19, I2S_IRQ_MASK
-  sw x19, 0(x18)
 
+i2s_handler_sd_out:  
   # pop from spk buf -> x8
   la x9, _eos_i2s_spk_buf
   lhu x18, I2S_ABUF_OFF_IDXR(x9)
   lhu x19, I2S_ABUF_OFF_IDXW(x9)
-  beq x18, x19, i2s_handler_ci_exit
+  beq x18, x19, i2s_handler_sd_exit
   lhu x19, I2S_ABUF_OFF_SIZE(x9)
 
   addi x19, x19, -1
@@ -334,9 +240,10 @@ i2s_handler_ci:
 
 #define UART0_CTRL_ADDR 0x10013000
 #define UART_REG_TXFIFO 0x00
+  # END
   # li x18, UART0_CTRL_ADDR
   # sw x8, UART_REG_TXFIFO(x18)
-  
+
   # aLaw decode -> x8
   xori x8, x8, 0x55
   andi x9, x8, 0x80
@@ -344,7 +251,7 @@ i2s_handler_ci:
   li x9, 1
   slli x9, x9, 7
   not x9, x9
-  and x9, x8, x9
+  and x8, x8, x9
   li x9, -1
 0:
   andi x18, x8, 0xf0
@@ -374,25 +281,51 @@ i2s_handler_ci:
   beqz x9, 3f
   mul x8, x8, x9
 3:
+#define UART0_CTRL_ADDR 0x10013000
+#define UART_REG_TXFIFO 0x00
   # END
-  # li x18, UART0_CTRL_ADDR
-  # mv x9, x8
-  # srai x9, x9, 8
-  # andi x9, x9, 0xff
-  # sw x9, UART_REG_TXFIFO(x18)
-  # mv x9, x8
-  # andi x9, x9, 0xff
-  # sw x9, UART_REG_TXFIFO(x18)
+  li x18, UART0_CTRL_ADDR
+  mv x9, x8
+  srai x9, x9, 8
+  andi x9, x9, 0xff
+  sw x9, UART_REG_TXFIFO(x18)
+  mv x9, x8
+  andi x9, x9, 0xff
+  sw x9, UART_REG_TXFIFO(x18)
+
+i2s_handler_sd_exit:
+  # complete
+  li x18, I2S_IRQ_SD_ID
+  li x19, PLIC_CLAIM
+  sw x18, 0(x19)
+
+  # exit
+  j trap_exit_data
   
-i2s_handler_ci_exit:
+i2s_handler_ws:
+  # enable sd irq
+  li x18, PLIC_PRIORITY
+  li x19, I2S_IRQ_SD_PRIORITY
+  sw x19, 4*I2S_IRQ_SD_ID(x18)
+
   # complete
-  li x18, I2S_IRQ_CI_ID
+  li x18, I2S_IRQ_WS_ID
   li x19, PLIC_CLAIM
   sw x18, 0(x19)
 
   # exit
   j trap_exit_data
 
+.global eos_flash_set
+eos_flash_set:
+  li a3, SPI0_CTRL_ADDR
+  sw x0, SPI_REG_FCTRL(a3)
+  sw a0, SPI_REG_SCKDIV(a3)
+  sw a1, SPI_REG_FFMT(a3)
+  li a0, 1
+  sw a0, SPI_REG_FCTRL(a3)
+  ret
+  
 #ifdef EOS_WITH_SPI_ASM
 
 # x9 - cmd, x18 - buffer, x19 - len, x20 - &flags, x21 - flags
@@ -447,12 +380,6 @@ spi_xchg_start:
   jalr x0, x8  
   
 spi_handler_xchg:
-  addi sp, sp, -4*REGBYTES
-  STORE x20, 0*REGBYTES(sp)
-  STORE x21, 1*REGBYTES(sp)
-  STORE x22, 2*REGBYTES(sp)
-  STORE x23, 3*REGBYTES(sp)
-
   li x19, SPI1_CTRL_ADDR
   la x18, _eos_spi_state_flags
   lbu x20, 0(x18)
@@ -644,12 +571,6 @@ spi_handler_xchg:
   sw x9, SPI_REG_IE(x19)
   
 spi_handler_xchg_exit:
-  LOAD x20, 0*REGBYTES(sp)
-  LOAD x21, 1*REGBYTES(sp)
-  LOAD x22, 2*REGBYTES(sp)
-  LOAD x23, 3*REGBYTES(sp)
-  addi sp, sp, 4*REGBYTES
-
   # complete
   li x18, INT_SPI1_BASE
   li x19, PLIC_CLAIM
@@ -659,12 +580,6 @@ spi_handler_xchg_exit:
   j trap_exit_data
 
 spi_handler_cts:
-  addi sp, sp, -4*REGBYTES
-  STORE x20, 0*REGBYTES(sp)
-  STORE x21, 1*REGBYTES(sp)
-  STORE x22, 2*REGBYTES(sp)
-  STORE x23, 3*REGBYTES(sp)
-
   li x8, 1
   slli x8, x8, SPI_PIN_CTS
   li x19, GPIO_CTRL_ADDR
@@ -733,12 +648,6 @@ spi_handler_cts:
   sw x8, GPIO_IOF_EN(x19)
 
 spi_handler_cts_exit:
-  LOAD x20, 0*REGBYTES(sp)
-  LOAD x21, 1*REGBYTES(sp)
-  LOAD x22, 2*REGBYTES(sp)
-  LOAD x23, 3*REGBYTES(sp)
-  addi sp, sp, 4*REGBYTES
-
   # complete
   li x18, INT_GPIO_BASE + SPI_PIN_CTS
   li x19, PLIC_CLAIM
@@ -811,7 +720,11 @@ trap_exit_data:
   LOAD x9, 1*REGBYTES(sp)
   LOAD x18, 2*REGBYTES(sp)
   LOAD x19, 3*REGBYTES(sp)
-  addi sp, sp, 4*REGBYTES
+  LOAD x20, 4*REGBYTES(sp)
+  LOAD x21, 5*REGBYTES(sp)
+  LOAD x22, 6*REGBYTES(sp)
+  LOAD x23, 7*REGBYTES(sp)
+  addi sp, sp, 8*REGBYTES
 
   mret
 
@@ -820,20 +733,11 @@ handler:
   addi x18, x18, %lo(trap_entry_text)
   jalr x0, x18
 
-  .align 4
-
-sd_sample:
-  .word 0
-sd_sample_save:
-  .word 0
-sd_sample_mask:
-  .word 0
-
   .section      .text.entry
   .align 2
   
 trap_entry_text:
-  addi sp, sp, -28*REGBYTES
+  addi sp, sp, -24*REGBYTES
 
   STORE x1, 0*REGBYTES(sp)
   STORE x2, 1*REGBYTES(sp)
@@ -850,18 +754,14 @@ trap_entry_text:
   STORE x15, 12*REGBYTES(sp)
   STORE x16, 13*REGBYTES(sp)
   STORE x17, 14*REGBYTES(sp)
-  STORE x20, 15*REGBYTES(sp)
-  STORE x21, 16*REGBYTES(sp)
-  STORE x22, 17*REGBYTES(sp)
-  STORE x23, 18*REGBYTES(sp)
-  STORE x24, 19*REGBYTES(sp)
-  STORE x25, 20*REGBYTES(sp)
-  STORE x26, 21*REGBYTES(sp)
-  STORE x27, 22*REGBYTES(sp)
-  STORE x28, 23*REGBYTES(sp)
-  STORE x29, 24*REGBYTES(sp)
-  STORE x30, 25*REGBYTES(sp)
-  STORE x31, 26*REGBYTES(sp)
+  STORE x24, 15*REGBYTES(sp)
+  STORE x25, 16*REGBYTES(sp)
+  STORE x26, 17*REGBYTES(sp)
+  STORE x27, 18*REGBYTES(sp)
+  STORE x28, 19*REGBYTES(sp)
+  STORE x29, 20*REGBYTES(sp)
+  STORE x30, 21*REGBYTES(sp)
+  STORE x31, 22*REGBYTES(sp)
 
   li x18, MCAUSE_TIMER
   beq x8, x18, handle_timer
@@ -901,23 +801,23 @@ trap_exit_text:
   LOAD x15, 12*REGBYTES(sp)
   LOAD x16, 13*REGBYTES(sp)
   LOAD x17, 14*REGBYTES(sp)
-  LOAD x20, 15*REGBYTES(sp)
-  LOAD x21, 16*REGBYTES(sp)
-  LOAD x22, 17*REGBYTES(sp)
-  LOAD x23, 18*REGBYTES(sp)
-  LOAD x24, 19*REGBYTES(sp)
-  LOAD x25, 20*REGBYTES(sp)
-  LOAD x26, 21*REGBYTES(sp)
-  LOAD x27, 22*REGBYTES(sp)
-  LOAD x28, 23*REGBYTES(sp)
-  LOAD x29, 24*REGBYTES(sp)
-  LOAD x30, 25*REGBYTES(sp)
-  LOAD x31, 26*REGBYTES(sp)
-
-  LOAD x8, 28*REGBYTES(sp)
-  LOAD x9, 29*REGBYTES(sp)
-  LOAD x18, 30*REGBYTES(sp)
-  LOAD x19, 31*REGBYTES(sp)
+  LOAD x24, 15*REGBYTES(sp)
+  LOAD x25, 16*REGBYTES(sp)
+  LOAD x26, 17*REGBYTES(sp)
+  LOAD x27, 18*REGBYTES(sp)
+  LOAD x28, 19*REGBYTES(sp)
+  LOAD x29, 20*REGBYTES(sp)
+  LOAD x30, 21*REGBYTES(sp)
+  LOAD x31, 22*REGBYTES(sp)
+
+  LOAD x8, 24*REGBYTES(sp)
+  LOAD x9, 25*REGBYTES(sp)
+  LOAD x18, 26*REGBYTES(sp)
+  LOAD x19, 27*REGBYTES(sp)
+  LOAD x20, 28*REGBYTES(sp)
+  LOAD x21, 29*REGBYTES(sp)
+  LOAD x22, 30*REGBYTES(sp)
+  LOAD x23, 31*REGBYTES(sp)
 
   addi sp, sp, 32*REGBYTES
   mret