diff --git a/FreeRTOS/Source/portable/GCC/RISC-V-RV32/port.c b/FreeRTOS/Source/portable/GCC/RISC-V-RV32/port.c
index ed2c298..2a0686b 100644
--- a/FreeRTOS/Source/portable/GCC/RISC-V-RV32/port.c
+++ b/FreeRTOS/Source/portable/GCC/RISC-V-RV32/port.c
@@ -76,6 +76,8 @@
  */
 StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
 {
+uint32_t mstatus;
+const uint32_t ulMPIE_Bit = 0x80, ulMPP_Bits = 0x1800;
 	/*
 	   X1 to X31 integer registers for the 'I' profile, X1 to X15 for the 'E' profile.
 
@@ -94,10 +96,11 @@
 		x28-31 		t3-6 			Temporaries 					Caller
 	*/
 
-	/* To ensure alignment. */
-	//_RB_	pxTopOfStack--;
-	//_RB_	pxTopOfStack--;
-	//_RB_pxTopOfStack--;
+	/* Start task with interrupt enabled. */
+	__asm volatile ("csrr %0, mstatus" : "=r"(mstatus));
+	mstatus |= ulMPIE_Bit | ulMPP_Bits;
+	pxTopOfStack--;
+	*pxTopOfStack = mstatus;
 
 	/* Numbers correspond to the x register number. */
 	pxTopOfStack--;
diff --git a/FreeRTOS/Source/portable/GCC/RISC-V-RV32/portASM.S b/FreeRTOS/Source/portable/GCC/RISC-V-RV32/portASM.S
index f376b61..4a128cb 100644
--- a/FreeRTOS/Source/portable/GCC/RISC-V-RV32/portASM.S
+++ b/FreeRTOS/Source/portable/GCC/RISC-V-RV32/portASM.S
@@ -36,7 +36,7 @@
     #error Assembler has not defined __riscv_xlen
 #endif
 
-#define CONTEXT_SIZE ( 28 * WORD_SIZE )
+#define CONTEXT_SIZE ( 30 * WORD_SIZE )
 
 .global xPortStartFirstTask
 .global vPortTrapHandler
@@ -48,41 +48,40 @@
 .align 8
 xPortStartFirstTask:
 
-    lw  sp, pxCurrentTCB            /* Load pxCurrentTCB. */
-    lw  sp, 0( sp )                 /* Read sp from first TCB member. */
+	lw  sp, pxCurrentTCB            /* Load pxCurrentTCB. */
+	lw  sp, 0( sp )                 /* Read sp from first TCB member. */
 
-    lw  x1, 0( sp ) /* Note for starting the scheduler the exception return address is used as the function return address. */
-    lw  x5, 2 * WORD_SIZE( sp )     /* t0 */
-    lw  x6, 3 * WORD_SIZE( sp )     /* t1 */
-    lw  x7, 4 * WORD_SIZE( sp )     /* t2 */
-    lw  x8, 5 * WORD_SIZE( sp )     /* s0/fp */
-    lw  x9, 6 * WORD_SIZE( sp )     /* s1 */
-    lw  x10, 7 * WORD_SIZE( sp )    /* a0 */
-    lw  x11, 8 * WORD_SIZE( sp )    /* a1 */
-    lw  x12, 9 * WORD_SIZE( sp )    /* a2 */
-    lw  x13, 10 * WORD_SIZE( sp )   /* a3 */
-    lw  x14, 11 * WORD_SIZE( sp )   /* a4 */
-    lw  x15, 12 * WORD_SIZE( sp )   /* a5 */
-    lw  x16, 13 * WORD_SIZE( sp )   /* a6 */
-    lw  x17, 14 * WORD_SIZE( sp )   /* a7 */
-    lw  x18, 15 * WORD_SIZE( sp )   /* s2 */
-    lw  x19, 16 * WORD_SIZE( sp )   /* s3 */
-    lw  x20, 17 * WORD_SIZE( sp )   /* s4 */
-    lw  x21, 18 * WORD_SIZE( sp )   /* s5 */
-    lw  x22, 19 * WORD_SIZE( sp )   /* s6 */
-    lw  x23, 20 * WORD_SIZE( sp )   /* s7 */
-    lw  x24, 21 * WORD_SIZE( sp )   /* s8 */
-    lw  x25, 22 * WORD_SIZE( sp )   /* s9 */
-    lw  x26, 23 * WORD_SIZE( sp )   /* s10 */
-    lw  x27, 24 * WORD_SIZE( sp )   /* s11 */
-    lw  x28, 25 * WORD_SIZE( sp )   /* t3 */
-    lw  x29, 26 * WORD_SIZE( sp )   /* t4 */
-    lw  x30, 27 * WORD_SIZE( sp )   /* t5 */
-    lw  x31, 28 * WORD_SIZE( sp )   /* t6 */
-    addi    sp, sp, CONTEXT_SIZE
-    csrs    mstatus, 8              /* Enable machine interrupts. */
-    csrs    mie, 8                  /* Enable soft interrupt. */
-    ret
+	lw  x1, 0( sp ) /* Note for starting the scheduler the exception return address is used as the function return address. */
+	lw  x5, 2 * WORD_SIZE( sp )     /* t0 */
+	lw  x6, 3 * WORD_SIZE( sp )     /* t1 */
+	lw  x7, 4 * WORD_SIZE( sp )     /* t2 */
+	lw  x8, 5 * WORD_SIZE( sp )     /* s0/fp */
+	lw  x9, 6 * WORD_SIZE( sp )     /* s1 */
+	lw  x10, 7 * WORD_SIZE( sp )    /* a0 */
+	lw  x11, 8 * WORD_SIZE( sp )    /* a1 */
+	lw  x12, 9 * WORD_SIZE( sp )    /* a2 */
+	lw  x13, 10 * WORD_SIZE( sp )   /* a3 */
+	lw  x14, 11 * WORD_SIZE( sp )   /* a4 */
+	lw  x15, 12 * WORD_SIZE( sp )   /* a5 */
+	lw  x16, 13 * WORD_SIZE( sp )   /* a6 */
+	lw  x17, 14 * WORD_SIZE( sp )   /* a7 */
+	lw  x18, 15 * WORD_SIZE( sp )   /* s2 */
+	lw  x19, 16 * WORD_SIZE( sp )   /* s3 */
+	lw  x20, 17 * WORD_SIZE( sp )   /* s4 */
+	lw  x21, 18 * WORD_SIZE( sp )   /* s5 */
+	lw  x22, 19 * WORD_SIZE( sp )   /* s6 */
+	lw  x23, 20 * WORD_SIZE( sp )   /* s7 */
+	lw  x24, 21 * WORD_SIZE( sp )   /* s8 */
+	lw  x25, 22 * WORD_SIZE( sp )   /* s9 */
+	lw  x26, 23 * WORD_SIZE( sp )   /* s10 */
+	lw  x27, 24 * WORD_SIZE( sp )   /* s11 */
+	lw  x28, 25 * WORD_SIZE( sp )   /* t3 */
+	lw  x29, 26 * WORD_SIZE( sp )   /* t4 */
+	lw  x30, 27 * WORD_SIZE( sp )   /* t5 */
+	lw  x31, 28 * WORD_SIZE( sp )   /* t6 */
+	addi    sp, sp, CONTEXT_SIZE
+	csrs    mstatus, 8              /* Enable machine interrupts. */
+	ret
 
 /*-----------------------------------------------------------*/
 
@@ -118,59 +117,63 @@
 	sw x30, 27 * WORD_SIZE( sp )
 	sw x31, 28 * WORD_SIZE( sp )
 
-    lw  t0, pxCurrentTCB            /* Load pxCurrentTCB. */
-    sw  sp, 0( t0 )                 /* Write sp from first TCB member. */
+	csrr t0, mstatus                /* Required for MPIE bit. */
+	sw t0, 29 * WORD_SIZE( sp )
+
+	lw  t0, pxCurrentTCB            /* Load pxCurrentTCB. */
+	sw  sp, 0( t0 )                 /* Write sp to first TCB member. */
 
 	csrr a0, mcause
 	csrr a1, mepc
 	mv a2, sp
+/*_RB_ Does stack need aligning here? */
 	jal handle_trap
 	csrw mepc, a0
 	/* Save exception return address. */
 	sw a0, 0( sp )
 
 
-	# Remain in M-mode after mret
-	li t0, 0x00001800 /* MSTATUS MPP */
-	csrs mstatus, t0
+	lw  sp, pxCurrentTCB            /* Load pxCurrentTCB. */
+	lw  sp, 0( sp )                 /* Read sp from first TCB member. */
 
-    lw  sp, pxCurrentTCB            /* Load pxCurrentTCB. */
-    lw  sp, 0( sp )                 /* Read sp from first TCB member. */
+	/* Load mret with the address of the next task. */
+	lw t0, 0( sp )
+	csrw mepc, t0
 
-    /* Load mret with the address of the next task. */
-    lw t0, 0( sp )
-    csrw mepc, t0
+	/* Load mstatus with the interrupt enable bits used by the task. */
+	lw  t0, 29 * WORD_SIZE( sp )
+	csrw mstatus, t0                /* Required for MPIE bit. */
 
-    lw  x1, 1 * WORD_SIZE( sp )
-    lw  x5, 2 * WORD_SIZE( sp )     /* t0 */
-    lw  x6, 3 * WORD_SIZE( sp )     /* t1 */
-    lw  x7, 4 * WORD_SIZE( sp )     /* t2 */
-    lw  x8, 5 * WORD_SIZE( sp )     /* s0/fp */
-    lw  x9, 6 * WORD_SIZE( sp )     /* s1 */
-    lw  x10, 7 * WORD_SIZE( sp )    /* a0 */
-    lw  x11, 8 * WORD_SIZE( sp )    /* a1 */
-    lw  x12, 9 * WORD_SIZE( sp )    /* a2 */
-    lw  x13, 10 * WORD_SIZE( sp )   /* a3 */
-    lw  x14, 11 * WORD_SIZE( sp )   /* a4 */
-    lw  x15, 12 * WORD_SIZE( sp )   /* a5 */
-    lw  x16, 13 * WORD_SIZE( sp )   /* a6 */
-    lw  x17, 14 * WORD_SIZE( sp )   /* a7 */
-    lw  x18, 15 * WORD_SIZE( sp )   /* s2 */
-    lw  x19, 16 * WORD_SIZE( sp )   /* s3 */
-    lw  x20, 17 * WORD_SIZE( sp )   /* s4 */
-    lw  x21, 18 * WORD_SIZE( sp )   /* s5 */
-    lw  x22, 19 * WORD_SIZE( sp )   /* s6 */
-    lw  x23, 20 * WORD_SIZE( sp )   /* s7 */
-    lw  x24, 21 * WORD_SIZE( sp )   /* s8 */
-    lw  x25, 22 * WORD_SIZE( sp )   /* s9 */
-    lw  x26, 23 * WORD_SIZE( sp )   /* s10 */
-    lw  x27, 24 * WORD_SIZE( sp )   /* s11 */
-    lw  x28, 25 * WORD_SIZE( sp )   /* t3 */
-    lw  x29, 26 * WORD_SIZE( sp )   /* t4 */
-    lw  x30, 27 * WORD_SIZE( sp )   /* t5 */
-    lw  x31, 28 * WORD_SIZE( sp )   /* t6 */
-    addi    sp, sp, CONTEXT_SIZE
+	lw  x1, 1 * WORD_SIZE( sp )
+	lw  x5, 2 * WORD_SIZE( sp )     /* t0 */
+	lw  x6, 3 * WORD_SIZE( sp )     /* t1 */
+	lw  x7, 4 * WORD_SIZE( sp )     /* t2 */
+	lw  x8, 5 * WORD_SIZE( sp )     /* s0/fp */
+	lw  x9, 6 * WORD_SIZE( sp )     /* s1 */
+	lw  x10, 7 * WORD_SIZE( sp )    /* a0 */
+	lw  x11, 8 * WORD_SIZE( sp )    /* a1 */
+	lw  x12, 9 * WORD_SIZE( sp )    /* a2 */
+	lw  x13, 10 * WORD_SIZE( sp )   /* a3 */
+	lw  x14, 11 * WORD_SIZE( sp )   /* a4 */
+	lw  x15, 12 * WORD_SIZE( sp )   /* a5 */
+	lw  x16, 13 * WORD_SIZE( sp )   /* a6 */
+	lw  x17, 14 * WORD_SIZE( sp )   /* a7 */
+	lw  x18, 15 * WORD_SIZE( sp )   /* s2 */
+	lw  x19, 16 * WORD_SIZE( sp )   /* s3 */
+	lw  x20, 17 * WORD_SIZE( sp )   /* s4 */
+	lw  x21, 18 * WORD_SIZE( sp )   /* s5 */
+	lw  x22, 19 * WORD_SIZE( sp )   /* s6 */
+	lw  x23, 20 * WORD_SIZE( sp )   /* s7 */
+	lw  x24, 21 * WORD_SIZE( sp )   /* s8 */
+	lw  x25, 22 * WORD_SIZE( sp )   /* s9 */
+	lw  x26, 23 * WORD_SIZE( sp )   /* s10 */
+	lw  x27, 24 * WORD_SIZE( sp )   /* s11 */
+	lw  x28, 25 * WORD_SIZE( sp )   /* t3 */
+	lw  x29, 26 * WORD_SIZE( sp )   /* t4 */
+	lw  x30, 27 * WORD_SIZE( sp )   /* t5 */
+	lw  x31, 28 * WORD_SIZE( sp )   /* t6 */
+	addi    sp, sp, CONTEXT_SIZE
 
-    mret
+	mret