diff --git a/firmware/fw_base.S b/firmware/fw_base.S
index 3c509cfb..83fe2aae 100644
--- a/firmware/fw_base.S
+++ b/firmware/fw_base.S
@@ -184,6 +184,11 @@ _start_warm:
 	/* Setup trap handler */
 	la	a4, _trap_handler
 	csrw	CSR_MTVEC, a4
+	/* Make sure that mtvec is updated */
+	1:
+	csrr	a5, CSR_MTVEC
+	bne	a4, a5, 1b
+
 
 	/* Initialize SBI runtime */
 	csrr	a0, CSR_MSCRATCH