From f957224afd6631841a999c47794eb448b9a6d750 Mon Sep 17 00:00:00 2001
From: Chen Peng <pizer.chen@gmail.com>
Date: Fri, 22 May 2026 21:24:45 +0800
Subject: [PATCH] =?UTF-8?q?RISC=E4=B8=AD=E5=85=B3=E4=B8=AD=E6=96=AD?=
 =?UTF-8?q?=E5=BA=94=E8=AF=A5=E7=94=A8=E5=8E=9F=E5=AD=90=E6=93=8D=E4=BD=9C?=
 =?UTF-8?q?=EF=BC=8C=E9=81=BF=E5=85=8D=E5=A4=9A=E4=BB=BB=E5=8A=A1=E7=8E=AF?=
 =?UTF-8?q?=E5=A2=83=E9=80=A0=E6=88=90=E9=94=99=E8=AF=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 CPU/QingKeV3/os_port.c     | 4 +---
 CPU/QingKeV3/os_port_gnu.S | 4 +++-
 CPU/QingKeV3/riscv.c       | 9 +++++++++
 CPU/QingKeV3/riscv.h       | 1 +
 4 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/CPU/QingKeV3/os_port.c b/CPU/QingKeV3/os_port.c
index b8b498b..706fc10 100644
--- a/CPU/QingKeV3/os_port.c
+++ b/CPU/QingKeV3/os_port.c
@@ -124,9 +124,7 @@ void os_port_restore_irq(os_uint_t level){
 }
 
 os_uint_t os_port_save_disable_irq_in_isr(void){
-    os_uint_t level = RISCV_get_MSTATUS();
-    RISCV_set_MSTATUS(0x1800);
-    return level;
+    return RISCV_get_set_MSTATUS(0x1800);
 }
 
 void os_port_restore_irq_in_isr(os_uint_t level){
diff --git a/CPU/QingKeV3/os_port_gnu.S b/CPU/QingKeV3/os_port_gnu.S
index 2b4c104..f8affa1 100644
--- a/CPU/QingKeV3/os_port_gnu.S
+++ b/CPU/QingKeV3/os_port_gnu.S
@@ -132,7 +132,9 @@ os_port_context_restore:
     lw a0,   0 * 4(sp)
     csrw mepc, a0
 
-    lw  a0,  2*4(sp)        /* 恢复 MPIE */
+    li t0, 0x1800
+    csrs mstatus, t0
+    lw  a0,  2*4(sp)
     csrs  mstatus, a0
 
     lw x1,   1 * 4(sp)      /* 返回地址 */
diff --git a/CPU/QingKeV3/riscv.c b/CPU/QingKeV3/riscv.c
index d084634..ab3051b 100644
--- a/CPU/QingKeV3/riscv.c
+++ b/CPU/QingKeV3/riscv.c
@@ -20,6 +20,15 @@ uint32_t RISCV_get_MSTATUS(void)
     return (result);
 }
 
+uint32_t RISCV_get_set_MSTATUS(uint32_t value){
+    uint32_t result;
+    __ASM volatile("csrrw %0, mstatus, %1"
+        : "=r"(result)
+        :"r"(value)
+        :"memory");
+    return (result);
+}
+
 /*********************************************************************
  * @fn      RISCV_set_MSTATUS
  *
diff --git a/CPU/QingKeV3/riscv.h b/CPU/QingKeV3/riscv.h
index 55b0c9d..dfd1eaa 100644
--- a/CPU/QingKeV3/riscv.h
+++ b/CPU/QingKeV3/riscv.h
@@ -216,6 +216,7 @@ int32_t RISCV_AMOXOR_W(volatile int32_t *addr, int32_t value)
 /* ------------------------------------------------------------------------------------------------------------------ */
 /*  */
 
+extern uint32_t RISCV_get_set_MSTATUS(uint32_t value);
 extern uint32_t RISCV_get_MSTATUS(void);
 extern void RISCV_set_MSTATUS(uint32_t value);
 extern uint32_t RISCV_get_MISA(void);