diff --git a/cpu/fe310/irq_arch.c b/cpu/fe310/irq_arch.c
index f849c189de..e6d89b528b 100644
--- a/cpu/fe310/irq_arch.c
+++ b/cpu/fe310/irq_arch.c
@@ -108,14 +108,28 @@ void handle_trap(uint32_t mcause)
         }
     }
     else {
+        switch (mcause) {
+        case CAUSE_USER_ECALL:    /* ECALL from user mode */
+        case CAUSE_MACHINE_ECALL: /* ECALL from machine mode */
+        {
+            /* TODO: get the ecall arguments */
+            sched_context_switch_request = 1;
+            /* Increment the return program counter past the ecall
+             * instruction */
+            uint32_t return_pc = read_csr(mepc);
+            write_csr(mepc, return_pc + 4);
+            break;
+        }
+        default:
 #ifdef DEVELHELP
-        printf("Unhandled trap:\n");
-        printf("  mcause: 0x%"PRIx32"\n", mcause);
-        printf("  mepc:   0x%"PRIx32"\n", read_csr(mepc));
-        printf("  mtval:  0x%"PRIx32"\n", read_csr(mtval));
+            printf("Unhandled trap:\n");
+            printf("  mcause: 0x%"PRIx32"\n", mcause);
+            printf("  mepc:   0x%"PRIx32"\n", read_csr(mepc));
+            printf("  mtval:  0x%"PRIx32"\n", read_csr(mtval));
 #endif
-        /* Unknown trap */
-        core_panic(PANIC_GENERAL_ERROR, "Unhandled trap");
+            /* Unknown trap */
+            core_panic(PANIC_GENERAL_ERROR, "Unhandled trap");
+        }
     }
     /* ISR done - no more changes to thread states */
     fe310_in_isr = 0;
diff --git a/cpu/fe310/thread_arch.c b/cpu/fe310/thread_arch.c
index ad50a233bf..4c6c7b5a32 100644
--- a/cpu/fe310/thread_arch.c
+++ b/cpu/fe310/thread_arch.c
@@ -178,13 +178,22 @@ void cpu_switch_context_exit(void)
     UNREACHABLE();
 }
 
+static inline void _ecall_dispatch(uint32_t num, void *ctx)
+{
+    /* function arguments are in a0 and a1 as per ABI */
+    __asm__ volatile (
+    "mv a0, %[num] \n"
+    "mv a1, %[ctx] \n"
+    "ECALL\n"
+    : /* No outputs */
+    : [num] "r" (num), [ctx] "r" (ctx)
+    : "memory"
+    );
+}
+
 void thread_yield_higher(void)
 {
-    /* Use SW intr to schedule context switch */
-    CLINT_REG(CLINT_MSIP) = 1;
-
-    /* Latency of SW intr can be 4-7 cycles; wait for the SW intr */
-    __asm__ volatile ("wfi");
+    _ecall_dispatch(0, NULL);
 }
 
 /**