diff --git a/components/freertos/port/riscv/port.c b/components/freertos/port/riscv/port.c
index 6c25ad11ad..6895604c60 100644
--- a/components/freertos/port/riscv/port.c
+++ b/components/freertos/port/riscv/port.c
@@ -102,6 +102,7 @@ static UBaseType_t uxCriticalNesting = 0;
 static UBaseType_t uxSavedInterruptState = 0;
 BaseType_t uxSchedulerRunning = 0;
 UBaseType_t uxInterruptNesting = 0;
+BaseType_t xPortSwitchFlag = 0;
 __attribute__((aligned(16))) static StackType_t xIsrStack[configISR_STACK_SIZE];
 StackType_t *xIsrStackTop = &xIsrStack[0] + (configISR_STACK_SIZE & (~((portPOINTER_SIZE_TYPE)portBYTE_ALIGNMENT_MASK)));
 
@@ -243,7 +244,7 @@ void vPortSysTickHandler(void)
     }
 
     if (xTaskIncrementTick() != pdFALSE) {
-        vTaskSwitchContext();
+        vPortYieldFromISR();
     }
 }
 
@@ -251,6 +252,7 @@ BaseType_t xPortStartScheduler(void)
 {
     vPortSetupTimer();
     vPortSetupSoftwareInterrupt();
+    uxInterruptNesting = 0;
     uxCriticalNesting = 0;
     uxSchedulerRunning = 0;          /* this means first yield */
     esprv_intc_int_set_threshold(1); /* set global INTC masking level */
@@ -270,7 +272,6 @@ void vPortEndScheduler(void)
 void vPortSoftwareInterrupt(void)
 {
     uxSchedulerRunning = 1;
-    vTaskSwitchContext();
     REG_WRITE(SYSTEM_CPU_INTR_FROM_CPU_0_REG, 0);
 }
 
@@ -285,6 +286,7 @@ void vPortYield(void)
     if (uxInterruptNesting) {
         vPortYieldFromISR();
     } else {
+        xPortSwitchFlag = 1;
         REG_WRITE(SYSTEM_CPU_INTR_FROM_CPU_0_REG, 1);
 
         /* There are 3-4 instructions of latency between triggering the software
@@ -299,11 +301,12 @@ void vPortYield(void)
         */
         while(uxSchedulerRunning && uxCriticalNesting == 0 && REG_READ(SYSTEM_CPU_INTR_FROM_CPU_0_REG) != 0) { }
     }
+
 }
 
-void vPortYieldFromISR(void)
+void vPortYieldFromISR( void )
 {
-    vTaskSwitchContext();
+    xPortSwitchFlag = 1;
 }
 
 void vPortSetStackWatchpoint(void *pxStackStart)
diff --git a/components/freertos/port/riscv/portasm.S b/components/freertos/port/riscv/portasm.S
index e4e05e3d68..8d19ba3467 100644
--- a/components/freertos/port/riscv/portasm.S
+++ b/components/freertos/port/riscv/portasm.S
@@ -36,15 +36,24 @@ rtos_int_enter:
     mv t2, a0
 
     /* scheduler not enabled, jump directly to ISR handler */
-    lw t0, uxSchedulerRunning
-    beq t0,zero, not_rtos_enter
+    lw t0, uxSchedulerRunning 
+    beq t0,zero, rtos_enter_end
 
-    /* Sabe current TCB and load the ISR stack */
+    /* increments the ISR nesting count */
+	la t3, uxInterruptNesting
+	lw t4, 0x0(t3)
+	addi t5,t4,1
+	sw  t5, 0x0(t3)
+    
+    /* If reached here from another low-prio ISR, skip stack pushing to TCB */
+	bne t4,zero, rtos_enter_end
+
+    /* Save current TCB and load the ISR stack */
     lw  t0, pxCurrentTCB
     sw 	t2, 0x0(t0)
     lw  sp, xIsrStackTop
 
-not_rtos_enter:
+rtos_enter_end:
     mv  ra, t1
     ret
 
@@ -56,10 +65,23 @@ not_rtos_enter:
     .global rtos_int_exit
     .type rtos_int_exit, @function
 rtos_int_exit:
-
     /* may skip RTOS aware interrupt since scheduler was not started */
-    lw t0, uxSchedulerRunning
-    beq t0,zero, not_rtos_exit
+    lw t0, uxSchedulerRunning 
+    beq t0,zero, rtos_exit_end 
+
+    /* update nesting interrupts counter */
+	la t2, uxInterruptNesting
+	lw t3, 0x0(t2)
+
+	/* Already zero, protect againts underflow */
+	beq t3, zero, isr_skip_decrement
+	addi t3,t3, -1
+	sw  t3, 0x0(t2)
+
+isr_skip_decrement:
+
+	/* may still have interrupts pending, skip section below and exit */
+	bne t3,zero,rtos_exit_end 
 
     /* Schedule the next task if a yield is pending */
     la t0, xPortSwitchFlag
@@ -83,5 +105,5 @@ no_switch:
     lw a0, pxCurrentTCB
     lw a0, 0x0(a0)
 
-not_rtos_exit:
+rtos_exit_end:
     ret
diff --git a/components/riscv/vectors.S b/components/riscv/vectors.S
index d273f3ab17..b55f4c12fe 100644
--- a/components/riscv/vectors.S
+++ b/components/riscv/vectors.S
@@ -97,11 +97,9 @@
 	csrw	mepc, t0
 .endm
 
-	.global vPortYieldFromISR
-	.global uxInterruptNesting
-	.global uxSchedulerRunning
-	.global xIsrStackTop
-	.global pxCurrentTCB
+	.global rtos_int_enter
+	.global rtos_int_exit
+	.global uxInterruptNesting	
 	.global _global_interrupt_handler
 
 	.section .exception_vectors.text
@@ -218,26 +216,12 @@ _interrupt_handler:
 	/* entry */
 	save_regs
 	save_mepc
+	
+	/* Before doing anythig preserve the stack pointer */
+	/* It will be saved in current TCB, if needed */
+	mv a0, sp
+	call rtos_int_enter
 
-	/* scheduler not enabled, jump directly to ISR handler */
-	lw t0, uxSchedulerRunning
-	beq t0, zero, already_on_handler
-
-	/* increments the ISR nesting count */
-	la t0, uxInterruptNesting
-	lw t1, 0x0(t0)
-	addi t2,t1,1
-	sw  t2, 0x0(t0)
-
-	/* If reached here from another low priority ISR, skip stack pushing to TCB */
-	bne t1,zero, already_on_handler
-
-	/* Otherwise, save current sp, and use the isr stack from here */
-	lw  t0, pxCurrentTCB
-	sw 	sp, 0x0(t0)
-	lw  sp, xIsrStackTop
-
-already_on_handler:
 	/* Before dispatch c handler, restore interrupt to enable nested intr */
 	csrr s1, mcause
 	csrr s2, mstatus
@@ -278,29 +262,13 @@ already_on_handler:
 	sw s3, 0(t0)
 	fence
 
-	/* may skip RTOS aware interrupt since scheduler was not started */
-	lw t1, uxSchedulerRunning
-	beq t1,zero, isr_exit
+	/* Yield to the next task is needed: */
+	mv a0, sp
+	call rtos_int_exit
 
-	/* update nesting interrupts counter */
-	la t0, uxInterruptNesting
-	lw t1, 0x0(t0)
+	/* The next (or current) stack pointer is returned in a0 */
+	mv sp, a0
 
-	/* Already zero, protect against underflow */
-	beq t1, zero, isr_skip_decrement
-	addi t1,t1, -1
-	sw  t1, 0x0(t0)
-
-isr_skip_decrement:
-	/* may still have interrupts pending, skip section below and exit */
-	bne t1,zero,isr_exit
-
-	/* handled all the ISRs and scheduled the next task, take its stack */
-	/* load on sp, then exit. */
-	lw sp, pxCurrentTCB
-	lw sp, 0x0(sp)
-
-isr_exit:
 	/* restore the rest of the registers */
 	csrw mcause, s1
 	csrw mstatus, s2