riscv: Enable interrupts during dispatch after ISR

The code sequence is derived from the ARM code
(see _ARMV4_Exception_interrupt).

Update #2751.
Update #3433.

5 files changed, 91 insertions, 55 deletions

diff --git a/cpukit/score/cpu/riscv/cpu.c b/cpukit/score/cpu/riscv/cpu.c
index deae25d34f..c5d309a5d6 100644
--- a/cpukit/score/cpu/riscv/cpu.c
+++ b/cpukit/score/cpu/riscv/cpu.c
@@ -1,5 +1,5 @@
 /*
- * RISC-V CPU Dependent Source
+ * Copyright (c) 2018 embedded brains GmbH
  *
  * Copyright (c) 2015 University of York.
  * Hesham ALmatary <hesham@alumni.york.ac.uk>
@@ -29,10 +29,18 @@
  * SUCH DAMAGE.
  */
 
-#include <rtems/score/cpu.h>
+#include <rtems/score/cpuimpl.h>
 #include <rtems/score/isr.h>
 #include <rtems/score/riscv-utility.h>
 
+#define RISCV_ASSERT_CONTEXT_OFFSET( field, off ) \
+  RTEMS_STATIC_ASSERT( \
+    offsetof( Context_Control, field) == RISCV_CONTEXT_ ## off, \
+    riscv_context_offset_ ## field \
+  )
+
+RISCV_ASSERT_CONTEXT_OFFSET( isr_dispatch_disable, ISR_DISPATCH_DISABLE );
+
 /* bsp_start_vector_table_begin is the start address of the vector table
  * containing addresses to ISR Handlers. It's defined at the BSP linkcmds
  * and may differ from one BSP to another.
diff --git a/cpukit/score/cpu/riscv/include/rtems/score/cpu.h b/cpukit/score/cpu/riscv/include/rtems/score/cpu.h
index 30adbbca38..4d9f828086 100644
--- a/cpukit/score/cpu/riscv/include/rtems/score/cpu.h
+++ b/cpukit/score/cpu/riscv/include/rtems/score/cpu.h
@@ -3,6 +3,7 @@
  */
 
 /*
+ * Copyright (c) 2018 embedded brains GmbH
  *
  * Copyright (c) 2015 University of York.
  * Hesham Almatary <hesham@alumni.york.ac.uk>
@@ -75,6 +76,7 @@ typedef struct {
   unsigned long mstatus;
   unsigned long mcause;
   unsigned long mepc;
+  uint32_t isr_dispatch_disable;
 #ifdef RTEMS_SMP
   volatile bool is_executing;
 #endif
diff --git a/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h b/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h
index 1370e656dd..6279c7c22e 100644
--- a/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h
+++ b/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h
@@ -38,10 +38,14 @@
 
 #if __riscv_xlen == 32
 
+#define RISCV_CONTEXT_ISR_DISPATCH_DISABLE 140
+
 #define CPU_INTERRUPT_FRAME_SIZE 144
 
 #elif __riscv_xlen == 64
 
+#define RISCV_CONTEXT_ISR_DISPATCH_DISABLE 280
+
 #define CPU_INTERRUPT_FRAME_SIZE 288
 
 #endif /* __riscv_xlen */
diff --git a/cpukit/score/cpu/riscv/riscv-context-switch.S b/cpukit/score/cpu/riscv/riscv-context-switch.S
index b5e43522c7..54adc6d0b6 100644
--- a/cpukit/score/cpu/riscv/riscv-context-switch.S
+++ b/cpukit/score/cpu/riscv/riscv-context-switch.S
@@ -1,5 +1,5 @@
 /*
- * riscv32 CPU Dependent Source
+ * Copyright (c) 2018 embedded brains GmbH
  *
  * Copyright (c) 2015 University of York.
  * Hesham ALmatary <hesham@alumni.york.ac.uk>
@@ -31,8 +31,7 @@
 #endif
 
 #include <rtems/asm.h>
-#include <rtems/score/cpu.h>
-#include <rtems/score/riscv-utility.h>
+#include <rtems/score/percpu.h>
 
 	.section	.text, "ax", @progbits
 	.align	2
@@ -41,14 +40,16 @@ PUBLIC(_CPU_Context_switch)
 PUBLIC(_CPU_Context_restore)
 PUBLIC(_CPU_Context_restore_fp)
 PUBLIC(_CPU_Context_save_fp)
-PUBLIC(restore)
 
 SYM(_CPU_Context_switch):
+	GET_SELF_CPU_CONTROL	a2
+	lw	a3, PER_CPU_ISR_DISPATCH_DISABLE(a2)
+
 	/* Disable interrupts and store all registers */
 	csrr	t0, mstatus
 	SREG	t0, (32 * CPU_SIZEOF_POINTER)(a0)
 
-	csrci	mstatus, MSTATUS_MIE
+	csrci	mstatus, RISCV_MSTATUS_MIE
 
 	SREG	x1, (1 * CPU_SIZEOF_POINTER)(a0)
 	SREG	x2, (2 * CPU_SIZEOF_POINTER)(a0)
@@ -81,7 +82,12 @@ SYM(_CPU_Context_switch):
 	SREG	x30, (30 * CPU_SIZEOF_POINTER)(a0)
 	SREG	x31, (31 * CPU_SIZEOF_POINTER)(a0)
 
-	SYM(restore):
+	sw	a3, RISCV_CONTEXT_ISR_DISPATCH_DISABLE(a0)
+
+.Lrestore:
+	lw	a3, RISCV_CONTEXT_ISR_DISPATCH_DISABLE(a1)
+
+	sw	a3, PER_CPU_ISR_DISPATCH_DISABLE(a2)
 
 	LREG	x1, (1 * CPU_SIZEOF_POINTER)(a1)
 	LREG	x2, (2 * CPU_SIZEOF_POINTER)(a1)
@@ -123,9 +129,10 @@ SYM(_CPU_Context_switch):
 
 	ret
 
-	SYM(_CPU_Context_restore):
+SYM(_CPU_Context_restore):
 	mv	a1, a0
-	j	restore
+	GET_SELF_CPU_CONTROL	a2
+	j	.Lrestore
 
 	/* TODO no FP support for riscv32 yet */
 	SYM(_CPU_Context_restore_fp):
diff --git a/cpukit/score/cpu/riscv/riscv-exception-handler.S b/cpukit/score/cpu/riscv/riscv-exception-handler.S
index 9af88eadcd..0bdfa2f481 100644
--- a/cpukit/score/cpu/riscv/riscv-exception-handler.S
+++ b/cpukit/score/cpu/riscv/riscv-exception-handler.S
@@ -7,6 +7,8 @@
  */
 
 /*
+ * Copyright (c) 2018 embedded brains GmbH
+
  * Copyright (c) 2015 University of York.
  * Hesham Almatary <hesham@alumni.york.ac.uk>
  *
@@ -36,8 +38,6 @@
 #include "config.h"
 #endif
 
-#include <rtems/score/cpu.h>
-
 #include <rtems/asm.h>
 #include <rtems/score/percpu.h>
 
@@ -95,18 +95,16 @@ SYM(ISR_Handler):
 	/* FIXME Only handle interrupts for now (MSB = 1) */
 	andi	a0, a0, 0xf
 
-	/* Increment nesting level */
-	la	t0, ISR_NEST_LEVEL
-
-	/* Disable multitasking */
-	la	t1, THREAD_DISPATCH_DISABLE_LEVEL
+	/* Get per-CPU control of current processor */
+	GET_SELF_CPU_CONTROL	s0
 
-	lw	t2, (t0)
-	lw	t3, (t1)
-	addi	t2, t2, 1
-	addi	t3, t3, 1
-	sw	t2, (t0)
-	sw	t3, (t1)
+	/* Increment interrupt nest and thread dispatch disable level */
+	lw	t0, PER_CPU_ISR_NEST_LEVEL(s0)
+	lw	t1, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(s0)
+	addi	t2, t0, 1
+	addi	t1, t1, 1
+	sw	t2, PER_CPU_ISR_NEST_LEVEL(s0)
+	sw	t1, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(s0)
 
 	/* Save interrupted task stack pointer */
 	addi	t4, sp, 36 * CPU_SIZEOF_POINTER
@@ -132,50 +130,67 @@ SYM(ISR_Handler):
 	add	t5, t5, t6
 	LREG	t5, (t5)
 
-	/* Do not switch stacks if we are in a nested interrupt. At
-		* this point t2 should be holding ISR_NEST_LEVEL value.
-		*/
-	li	s0, 1
-	bgtu	t2, s0, jump_to_c_handler
+	/* Switch to interrupt stack if necessary */
+	bnez	t0, .Linterrupt_stack_switch_done
+	LREG	sp, PER_CPU_INTERRUPT_STACK_HIGH(s0)
+.Linterrupt_stack_switch_done:
 
-	/* Switch to RTEMS dedicated interrupt stack */
-	la	sp, INTERRUPT_STACK_HIGH
-	LREG	sp, (sp)
-
-jump_to_c_handler:
 	jalr	t5
 
-	/* Switch back to the interrupted task stack */
+	/* Load some per-CPU variables */
+	lw	t0, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(s0)
+	lbu	t1, PER_CPU_DISPATCH_NEEDED(s0)
+	lw	t2, PER_CPU_ISR_DISPATCH_DISABLE(s0)
+	lw	t3, PER_CPU_ISR_NEST_LEVEL(s0)
+
+	/* Restore stack pointer */
 	mv	sp, s1
 
-	/* Decrement nesting level */
-	la	t0, ISR_NEST_LEVEL
+	/* Decrement levels and determine thread dispatch state */
+	xor	t1, t1, t0
+	addi	t0, t0, -1
+	or	t1, t1, t0
+	or	t1, t1, t2
+	addi	t3, t3, -1
+
+	/* Store thread dispatch disable and ISR nest levels */
+	sw	t0, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(s0)
+	sw	t3, PER_CPU_ISR_NEST_LEVEL(s0)
 
-	/* Enable multitasking */
-	la	t1, THREAD_DISPATCH_DISABLE_LEVEL
+	/*
+	 * Check thread dispatch necessary, ISR dispatch disable and thread
+	 * dispatch disable level.
+	 */
+	bnez	t1, .Lthread_dispatch_done
 
-	Lw	t2, (t0)
-	lw	t3, (t1)
-	addi	t2, t2, -1
-	addi	t3, t3, -1
-	sw	t2, (t0)
-	sw	t3, (t1)
+.Ldo_thread_dispatch:
+
+	/* Set ISR dispatch disable and thread dispatch disable level to one */
+	li	t0, 1
+	sw	t0, PER_CPU_ISR_DISPATCH_DISABLE(s0)
+	sw	t0, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(s0)
 
-	/* Check if _ISR_Nest_level > 0 */
-	bgtz	t2, exception_frame_restore
+	/* Call _Thread_Do_dispatch(), this function will enable interrupts */
+	mv	a0, s0
+	li	a1, RISCV_MSTATUS_MIE
+	call	_Thread_Do_dispatch
+
+	/* Disable interrupts */
+	csrrc	zero, mstatus, RISCV_MSTATUS_MIE
+
+#ifdef RTEMS_SMP
+	GET_SELF_CPU_CONTROL	s0
+#endif
 
-	/* Check if _Thread_Dispatch_disable_level > 0 */
-	bgtz	t3, exception_frame_restore
+	/* Check if we have to do the thread dispatch again */
+	lbu	t0, PER_CPU_DISPATCH_NEEDED(s0)
+	bnez	t0, .Ldo_thread_dispatch
 
-	/* Check if dispatch needed */
-	la	x31, DISPATCH_NEEDED
-	lw	x31, (x31)
-	beqz	x31, exception_frame_restore
+	/* We are done with thread dispatching */
+	sw	zero, PER_CPU_ISR_DISPATCH_DISABLE(s0)
 
-	la	x31, _Thread_Dispatch
-	jalr	x31
+.Lthread_dispatch_done:
 
-	SYM(exception_frame_restore):
 	LREG	x1, (1 * CPU_SIZEOF_POINTER)(sp)
 	/* Skip sp/x2 */
 	LREG	x3, (3 * CPU_SIZEOF_POINTER)(sp)