Merge pull request #5207 from BernardXiong/riscv_smode

Fix risc-v s-mode issue.

4 files changed, 23 insertions, 12 deletions

diff --git a/libcpu/risc-v/virt64/interrupt.c b/libcpu/risc-v/virt64/interrupt.c
index eae0a55c96..f2869fc441 100644
--- a/libcpu/risc-v/virt64/interrupt.c
+++ b/libcpu/risc-v/virt64/interrupt.c
@@ -14,7 +14,6 @@
 #include "riscv.h"
 #include "interrupt.h"
 
-#define CPU_NUM         2
 #define MAX_HANDLERS    128
 
 static struct rt_irq_desc irq_desc[MAX_HANDLERS];
@@ -60,7 +59,6 @@ void rt_hw_interrupt_init(void)
     /* init exceptions table */
     for (idx = 0; idx < MAX_HANDLERS; idx++)
     {
-        //rt_hw_interrupt_mask(idx);
         irq_desc[idx].handler = (rt_isr_handler_t)rt_hw_interrupt_handle;
         irq_desc[idx].param = RT_NULL;
 #ifdef RT_USING_INTERRUPT_INFO
@@ -68,6 +66,8 @@ void rt_hw_interrupt_init(void)
         irq_desc[idx].counter = 0;
 #endif
     }
+
+    plic_set_threshold(0);
 }
 
 /**
@@ -86,7 +86,7 @@ void rt_hw_interrupt_mask(int vector)
 void rt_hw_interrupt_umask(int vector)
 {
     plic_set_priority(vector, 1);
-    plic_set_threshold(0);
+
     rt_hw_plic_irq_enable(vector);
 }
 
@@ -182,6 +182,7 @@ void handle_trap(rt_size_t xcause,rt_size_t xtval,rt_size_t xepc,struct rt_hw_st
 {
     int cause = (xcause & 0xFFFFFFFF);
     int plic_irq = 0;
+
     if (xcause & (1UL << 63))
     {
         switch (cause)
@@ -197,11 +198,13 @@ void handle_trap(rt_size_t xcause,rt_size_t xtval,rt_size_t xepc,struct rt_hw_st
             case IRQ_S_TIMER:
                 tick_isr();
                 break;
+
             case IRQ_S_EXT:
                 plic_irq = plic_claim();
                 plic_complete(plic_irq);
                 irq_desc[plic_irq].handler(plic_irq, irq_desc[plic_irq].param);
                 break;
+
             case IRQ_M_EXT:
                 plic_irq = plic_claim();
                 plic_complete(plic_irq);
diff --git a/libcpu/risc-v/virt64/riscv_io.h b/libcpu/risc-v/virt64/riscv_io.h
index 391d7f5a41..7dcc98f70a 100644
--- a/libcpu/risc-v/virt64/riscv_io.h
+++ b/libcpu/risc-v/virt64/riscv_io.h
@@ -10,15 +10,15 @@
 #ifndef __RISCV_IO_H__
 #define __RISCV_IO_H__
 
-// which hart (core) is this?
-static inline uint32_t  r_mhartid()
+static inline uint32_t  __raw_hartid(void)
 {
-#ifndef RISCV_S_MODE
+#ifdef RISCV_S_MODE
+    extern int boot_hartid;
+    return boot_hartid;
+#else
     uint32_t x;
     asm volatile("csrr %0, mhartid" : "=r" (x) );
     return x;
-#else
-    return 0;
 #endif
 }
 
diff --git a/libcpu/risc-v/virt64/startup_gcc.S b/libcpu/risc-v/virt64/startup_gcc.S
index ac980f6ed2..98fdc1d15b 100644
--- a/libcpu/risc-v/virt64/startup_gcc.S
+++ b/libcpu/risc-v/virt64/startup_gcc.S
@@ -14,10 +14,18 @@
 #define XSTATUS_PUM        (1 << 18) 
 #include <cpuport.h>
 
+boot_hartid: .int
+  .global      boot_hartid
+
   .global	_start
   .section ".start", "ax"
 _start:
-#ifndef RISCV_S_MODE
+#ifdef RISCV_S_MODE
+  # save hartid
+  la t0, boot_hartid                # global varible rt_boot_hartid
+  mv t1, a0                         # get hartid in S-mode frome a0 register
+  sw t1, (t0)                       # store t1 register low 4 bits in memory address which is stored in t0
+#else
   # setup stacks per hart
   csrr t0, mhartid                  # read current hart id
   slli t0, t0, 10                   # shift left the hart id by 1024
@@ -54,4 +62,4 @@ _start:
 
 park:
     wfi
-    j park
\ No newline at end of file
+    j park
diff --git a/libcpu/risc-v/virt64/tick.c b/libcpu/risc-v/virt64/tick.c
index d6970ec417..53f555da9d 100644
--- a/libcpu/risc-v/virt64/tick.c
+++ b/libcpu/risc-v/virt64/tick.c
@@ -36,7 +36,7 @@ int tick_isr(void)
 #ifdef RISCV_S_MODE
     sbi_set_timer(get_ticks() + tick_cycles);
 #else
-    *(uint64_t*)CLINT_MTIMECMP(r_mhartid()) = *(uint64_t*)CLINT_MTIME + tick_cycles;
+    *(uint64_t*)CLINT_MTIMECMP(__raw_hartid()) = *(uint64_t*)CLINT_MTIME + tick_cycles;
 #endif
 
     return 0;
@@ -62,7 +62,7 @@ int rt_hw_tick_init(void)
 #else
     clear_csr(mie, MIP_MTIP);
     clear_csr(mip, MIP_MTIP);
-    *(uint64_t*)CLINT_MTIMECMP(r_mhartid()) = *(uint64_t*)CLINT_MTIME + interval;
+    *(uint64_t*)CLINT_MTIMECMP(__raw_hartid()) = *(uint64_t*)CLINT_MTIME + interval;
     set_csr(mie, MIP_MTIP);
 #endif
     return 0;