Index: cpukit/score/cpu/riscv/cpu.c
===================================================================
--- cpukit/score/cpu/riscv/cpu.c	(revision 0fd8287b2be109cd8671a65cac955f943ab60d32)
+++ cpukit/score/cpu/riscv/cpu.c	(revision 9704d86f86c5a800a06dd814538df4cd83367fc5)
@@ -1,4 +1,4 @@
 /*
- * RISC-V CPU Dependent Source
+ * Copyright (c) 2018 embedded brains GmbH
  *
  * Copyright (c) 2015 University of York.
@@ -30,7 +30,15 @@
  */
 
-#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
Index: cpukit/score/cpu/riscv/include/rtems/score/cpu.h
===================================================================
--- cpukit/score/cpu/riscv/include/rtems/score/cpu.h	(revision 0fd8287b2be109cd8671a65cac955f943ab60d32)
+++ cpukit/score/cpu/riscv/include/rtems/score/cpu.h	(revision 9704d86f86c5a800a06dd814538df4cd83367fc5)
@@ -4,4 +4,5 @@
 
 /*
+ * Copyright (c) 2018 embedded brains GmbH
  *
  * Copyright (c) 2015 University of York.
@@ -76,4 +77,5 @@
   unsigned long mcause;
   unsigned long mepc;
+  uint32_t isr_dispatch_disable;
 #ifdef RTEMS_SMP
   volatile bool is_executing;
Index: cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h
===================================================================
--- cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h	(revision 0fd8287b2be109cd8671a65cac955f943ab60d32)
+++ cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h	(revision 9704d86f86c5a800a06dd814538df4cd83367fc5)
@@ -39,7 +39,11 @@
 #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
Index: cpukit/score/cpu/riscv/riscv-context-switch.S
===================================================================
--- cpukit/score/cpu/riscv/riscv-context-switch.S	(revision 0fd8287b2be109cd8671a65cac955f943ab60d32)
+++ cpukit/score/cpu/riscv/riscv-context-switch.S	(revision 9704d86f86c5a800a06dd814538df4cd83367fc5)
@@ -1,4 +1,4 @@
 /*
- * riscv32 CPU Dependent Source
+ * Copyright (c) 2018 embedded brains GmbH
  *
  * Copyright (c) 2015 University of York.
@@ -32,6 +32,5 @@
 
 #include <rtems/asm.h>
-#include <rtems/score/cpu.h>
-#include <rtems/score/riscv-utility.h>
+#include <rtems/score/percpu.h>
 
 	.section	.text, "ax", @progbits
@@ -42,12 +41,14 @@
 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)
@@ -82,5 +83,10 @@
 	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)
@@ -124,7 +130,8 @@
 	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 */
Index: cpukit/score/cpu/riscv/riscv-exception-handler.S
===================================================================
--- cpukit/score/cpu/riscv/riscv-exception-handler.S	(revision 0fd8287b2be109cd8671a65cac955f943ab60d32)
+++ cpukit/score/cpu/riscv/riscv-exception-handler.S	(revision 9704d86f86c5a800a06dd814538df4cd83367fc5)
@@ -8,4 +8,6 @@
 
 /*
+ * Copyright (c) 2018 embedded brains GmbH
+
  * Copyright (c) 2015 University of York.
  * Hesham Almatary <hesham@alumni.york.ac.uk>
@@ -36,6 +38,4 @@
 #include "config.h"
 #endif
-
-#include <rtems/score/cpu.h>
 
 #include <rtems/asm.h>
@@ -96,16 +96,14 @@
 	andi	a0, a0, 0xf
 
-	/* Increment nesting level */
-	la	t0, ISR_NEST_LEVEL
-
-	/* Disable multitasking */
-	la	t1, THREAD_DISPATCH_DISABLE_LEVEL
-
-	lw	t2, (t0)
-	lw	t3, (t1)
-	addi	t2, t2, 1
-	addi	t3, t3, 1
-	sw	t2, (t0)
-	sw	t3, (t1)
+	/* Get per-CPU control of current processor */
+	GET_SELF_CPU_CONTROL	s0
+
+	/* 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 */
@@ -133,48 +131,65 @@
 	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 RTEMS dedicated interrupt stack */
-	la	sp, INTERRUPT_STACK_HIGH
-	LREG	sp, (sp)
-
-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:
+
 	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
-
-	/* Enable multitasking */
-	la	t1, THREAD_DISPATCH_DISABLE_LEVEL
-
-	Lw	t2, (t0)
-	lw	t3, (t1)
-	addi	t2, t2, -1
+	/* 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
-	sw	t2, (t0)
-	sw	t3, (t1)
-
-	/* Check if _ISR_Nest_level > 0 */
-	bgtz	t2, exception_frame_restore
-
-	/* Check if _Thread_Dispatch_disable_level > 0 */
-	bgtz	t3, exception_frame_restore
-
-	/* Check if dispatch needed */
-	la	x31, DISPATCH_NEEDED
-	lw	x31, (x31)
-	beqz	x31, exception_frame_restore
-
-	la	x31, _Thread_Dispatch
-	jalr	x31
-
-	SYM(exception_frame_restore):
+
+	/* 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)
+
+	/*
+	 * Check thread dispatch necessary, ISR dispatch disable and thread
+	 * dispatch disable level.
+	 */
+	bnez	t1, .Lthread_dispatch_done
+
+.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)
+
+	/* 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 we have to do the thread dispatch again */
+	lbu	t0, PER_CPU_DISPATCH_NEEDED(s0)
+	bnez	t0, .Ldo_thread_dispatch
+
+	/* We are done with thread dispatching */
+	sw	zero, PER_CPU_ISR_DISPATCH_DISABLE(s0)
+
+.Lthread_dispatch_done:
+
 	LREG	x1, (1 * CPU_SIZEOF_POINTER)(sp)
 	/* Skip sp/x2 */
