riscv: enable s-mode in CPU port

Updates #3337

bsps/riscv/riscv/start/bspsmp.c

@@ -87,3 +87,10 @@ void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)
   *cpu->cpu_per_cpu.clint_msip = 0x1;
 #endif
 }
+
+#ifdef RISCV_USE_S_MODE
+uint32_t _CPU_SMP_Get_current_processor( void )
+{
+  return _Per_CPU_Get_index( _CPU_Get_current_per_CPU_control() );
+}
+#endif

cpukit/score/cpu/riscv/cpu.c

@@ -183,7 +183,13 @@ void _CPU_Initialize(void)
 
 uint32_t _CPU_ISR_Get_level( void )
 {
-  if ( _CPU_ISR_Is_enabled( read_csr( mstatus ) ) ) {
+  uint64_t status;
+#ifdef RISCV_USE_S_MODE
+  status = read_csr( sstatus );
+#else
+  status = read_csr( mstatus );
+#endif
+  if ( _CPU_ISR_Is_enabled( status ) ) {
     return 0;
   }
 

cpukit/score/cpu/riscv/include/rtems/asm.h

@@ -185,7 +185,11 @@
 
 .macro GET_SELF_CPU_CONTROL REG
 #ifdef RTEMS_SMP
+#ifdef RISCV_USE_S_MODE
+	csrr	\REG, sscratch
+#else
 	csrr	\REG, mscratch
+#endif
 #else
 	LADDR	\REG, _Per_CPU_Information
 #endif

cpukit/score/cpu/riscv/include/rtems/score/cpu.h

@@ -4,6 +4,7 @@
  */
 
 /*
+ * Copyright (c) 2026 Gedare Bloom
  * Copyright (c) 2018 embedded brains GmbH & Co. KG
  *
  * Copyright (c) 2015 University of York.
@@ -43,10 +44,7 @@ extern "C" {
 
 #include <rtems/score/basedefs.h>
 #include <rtems/score/riscv.h>
-
-#define RISCV_MSTATUS_MIE 0x8
-#define RISCV_MSTATUS_MDT 0x40000000000
-#define RISCV_MSTATUSH_MDT 0x400
+#include <rtems/score/riscv-utility.h>
 
 #define CPU_ISR_PASSES_FRAME_POINTER FALSE
 
@@ -151,17 +149,25 @@ typedef struct {
 
 static inline uint32_t riscv_interrupt_disable( void )
 {
-  unsigned long mstatus;
+  unsigned long status;
 
   __asm__ volatile (
     ".option push\n"
     ".option arch, +zicsr\n"
-    "csrrc %0, mstatus, " RTEMS_XSTRING( RISCV_MSTATUS_MIE ) "\n"
+#ifdef RISCV_USE_S_MODE
+    "csrrc %0, sstatus, " RTEMS_XSTRING( SSTATUS_SIE ) "\n"
+#else
+    "csrrc %0, mstatus, " RTEMS_XSTRING( MSTATUS_MIE ) "\n"
+#endif
     ".option pop" :
-      "=&r" ( mstatus )
+      "=&r" ( status )
   );
 
-  return mstatus & RISCV_MSTATUS_MIE;
+#ifdef RISCV_USE_S_MODE
+  return status & SSTATUS_SIE;
+#else
+  return status & MSTATUS_MIE;
+#endif
 }
 
 static inline void riscv_interrupt_enable( uint32_t level )
@@ -169,7 +175,11 @@ static inline void riscv_interrupt_enable( uint32_t level )
   __asm__ volatile (
     ".option push\n"
     ".option arch, +zicsr\n"
+#ifdef RISCV_USE_S_MODE
+    "csrrs zero, sstatus, %0\n"
+#else
     "csrrs zero, mstatus, %0\n"
+#endif
     ".option pop" :
     :
     "r" ( level )
@@ -190,7 +200,11 @@ static inline void riscv_interrupt_enable( uint32_t level )
 
 static inline bool _CPU_ISR_Is_enabled( unsigned long level )
 {
-  return ( level & RISCV_MSTATUS_MIE ) != 0;
+#ifdef RISCV_USE_S_MODE
+  return ( level & SSTATUS_SIE ) != 0;
+#else
+  return ( level & MSTATUS_MIE ) != 0;
+#endif
 }
 
 static inline void _CPU_ISR_Set_level( uint32_t level )
@@ -205,7 +219,11 @@ static inline void _CPU_ISR_Set_level( uint32_t level )
   __asm__ volatile (
     ".option push\n"
     ".option arch, +zicsr\n"
-    "csrrs zero, mstatus, " RTEMS_XSTRING( RISCV_MSTATUS_MIE ) "\n"
+#ifdef RISCV_USE_S_MODE
+    "csrrs zero, sstatus, " RTEMS_XSTRING( SSTATUS_SIE ) "\n"
+#else
+    "csrrs zero, mstatus, " RTEMS_XSTRING( MSTATUS_MIE ) "\n"
+#endif
     ".option pop"
   );
 }
@@ -469,6 +487,9 @@ void _CPU_SMP_Finalize_initialization( uint32_t cpu_count );
 
 void _CPU_SMP_Prepare_start_multitasking( void );
 
+#ifdef RISCV_USE_S_MODE
+uint32_t _CPU_SMP_Get_current_processor( void );
+#else
 static inline uint32_t _CPU_SMP_Get_current_processor( void )
 {
   unsigned long mhartid;
@@ -483,6 +504,7 @@ static inline uint32_t _CPU_SMP_Get_current_processor( void )
 
   return (uint32_t) mhartid - RISCV_BOOT_HARTID;
 }
+#endif
 
 void _CPU_SMP_Send_interrupt( uint32_t target_processor_index );
 

cpukit/score/cpu/riscv/include/rtems/score/riscv-utility.h

@@ -60,9 +60,12 @@
 #define MSTATUS_TVM         0x00100000
 #define MSTATUS_TW          0x00200000
 #define MSTATUS_TSR         0x00400000
+#define MSTATUS_MDT         0x40000000000
 #define MSTATUS32_SD        0x80000000
 #define MSTATUS64_SD        0x8000000000000000
 
+#define MSTATUSH_MDT        0x400
+
 #define SSTATUS_UIE         0x00000001
 #define SSTATUS_SIE         0x00000002
 #define SSTATUS_UPIE        0x00000010
@@ -72,6 +75,7 @@
 #define SSTATUS_XS          0x00018000
 #define SSTATUS_SUM         0x00040000
 #define SSTATUS_MXR         0x00080000
+#define SSTATUS_SDT         0x01000000
 #define SSTATUS32_SD        0x80000000
 #define SSTATUS64_SD        0x8000000000000000
 

cpukit/score/cpu/riscv/riscv-context-switch.S

@@ -183,11 +183,15 @@ SYM(_RISCV_Start_multitasking):
 	mv	a1, a0
 	GET_SELF_CPU_CONTROL	a2
 
-	/* Switch the stack to the temporary interrupt stack of this processor */
+	/* Switch to the temporary interrupt stack of this processor */
 	addi	sp, a2, PER_CPU_INTERRUPT_FRAME_AREA + CPU_INTERRUPT_FRAME_SIZE
 
 	/* Enable interrupts */
-	csrrs	zero, mstatus, RISCV_MSTATUS_MIE
+#ifdef RISCV_USE_S_MODE
+	csrrs	zero, sstatus, SSTATUS_SIE
+#else
+	csrrs	zero, mstatus, MSTATUS_MIE
+#endif
 
 	j	.Ltry_update_is_executing
 #endif

cpukit/score/cpu/riscv/riscv-exception-handler.S

@@ -7,8 +7,8 @@
  */
 
 /*
+ * Copyright (c) 2026 Gedare Bloom
  * Copyright (c) 2018 embedded brains GmbH & Co. KG
-
  * Copyright (c) 2015 University of York.
  * Hesham Almatary <hesham@alumni.york.ac.uk>
  *
@@ -41,6 +41,8 @@
 #include <rtems/asm.h>
 #include <rtems/score/percpu.h>
 
+#include <rtems/score/riscv-utility.h>
+
 PUBLIC(_RISCV_Exception_handler)
 
 	.section	.text, "ax", @progbits
@@ -56,9 +58,15 @@ SYM(_RISCV_Exception_handler):
 	SREG	a1, RISCV_INTERRUPT_FRAME_A1(sp)
 	SREG	a2, RISCV_INTERRUPT_FRAME_A2(sp)
 	SREG	s0, RISCV_INTERRUPT_FRAME_S0(sp)
+#ifdef RISCV_USE_S_MODE
+	csrr	a0, scause
+	csrr	a1, sstatus
+	csrr	a2, sepc
+#else
 	csrr	a0, mcause
 	csrr	a1, mstatus
 	csrr	a2, mepc
+#endif
 	GET_SELF_CPU_CONTROL	s0
 	SREG	s1, RISCV_INTERRUPT_FRAME_S1(sp)
 #if __riscv_flen > 0
@@ -112,13 +120,18 @@ SYM(_RISCV_Exception_handler):
 	 * In that case not clearing the MDT bit would prevent us from setting
 	 * the MIE bit later.
 	 */
+#ifdef RISCV_USE_S_MODE
+	li	t0, SSTATUS_SDT
+	csrrc	zero, sstatus, t0
+#else
 #if __riscv_xlen == 64
-	li	t0, RISCV_MSTATUS_MDT
+	li	t0, MSTATUS_MDT
 	csrrc	zero, mstatus, t0
 #elif __riscv_xlen == 32
-	li	t0, RISCV_MSTATUSH_MDT
+	li	t0, MSTATUSH_MDT
 	csrrc	zero, mstatush, t0
 #endif
+#endif /* RISCV_USE_S_MODE */
 
 	/* Increment interrupt nest and thread dispatch disable level */
 	lw	t0, PER_CPU_ISR_NEST_LEVEL(s0)
@@ -177,11 +190,19 @@ SYM(_RISCV_Exception_handler):
 
 	/* Call _Thread_Do_dispatch(), this function will enable interrupts */
 	mv	a0, s0
-	li	a1, RISCV_MSTATUS_MIE
+#ifdef RISCV_USE_S_MODE
+	li	a1, SSTATUS_SIE
+#else
+	li	a1, MSTATUS_MIE
+#endif
 	call	_Thread_Do_dispatch
 
 	/* Disable interrupts */
-	csrrc	zero, mstatus, RISCV_MSTATUS_MIE
+#ifdef RISCV_USE_S_MODE
+	csrrc	zero, sstatus, SSTATUS_SIE
+#else
+	csrrc	zero, mstatus, MSTATUS_MIE
+#endif
 
 #ifdef RTEMS_SMP
 	GET_SELF_CPU_CONTROL	s0
@@ -215,6 +236,14 @@ SYM(_RISCV_Exception_handler):
 	LREG	t5, RISCV_INTERRUPT_FRAME_T5(sp)
 	LREG	t6, RISCV_INTERRUPT_FRAME_T6(sp)
 
+#ifdef RISCV_USE_S_MODE
+	/* Clear SDT bit before restoring sstatus register */
+	li	a2, SSTATUS_SDT
+	not	a2, a2
+	and	a0, a0, a2
+	csrw	sstatus, a0
+	csrw	sepc, a1
+#else
 	/*
 	 * Clear MDT bit before restoring mstatus register.
 	 * This is only necessary if the Smdbltrp extension is implemented.
@@ -223,13 +252,14 @@ SYM(_RISCV_Exception_handler):
 	 * On RV32, the MDT bit is in the mstatush CSR which is not restored.
 	 */
 #if __riscv_xlen == 64
-	li	a2, RISCV_MSTATUS_MDT
+	li	a2, MSTATUS_MDT
 	not	a2, a2
 	and	a0, a0, a2
 #endif
-
 	csrw	mstatus, a0
 	csrw	mepc, a1
+#endif /* RISCV_USE_S_MODE */
+
 #if __riscv_flen > 0
 	lw	a0, RISCV_INTERRUPT_FRAME_FCSR(sp)
 	FLREG	ft0, RISCV_INTERRUPT_FRAME_FT0(sp)
@@ -260,7 +290,11 @@ SYM(_RISCV_Exception_handler):
 
 	addi	sp, sp, CPU_INTERRUPT_FRAME_SIZE
 
+#ifdef RISCV_USE_S_MODE
+	sret
+#else
 	mret
+#endif
 
 .Lsynchronous_exception:
 

testsuites/smptests/smpfatal08/init.c

@@ -87,7 +87,7 @@ void _CPU_SMP_Prepare_start_multitasking( void )
   assert( 0 );
 }
 
-#if defined( RTEMS_PARAVIRT ) ||                                          \
+#if defined( RTEMS_PARAVIRT ) || defined( RISCV_USE_S_MODE ) ||           \
   ( !defined( __leon__ ) && !defined( __PPC__ ) && !defined( __arm__ ) && \
     !defined( __riscv ) )
 uint32_t _CPU_SMP_Get_current_processor( void )