diff --git a/litex/soc/software/bios/isr.c b/litex/soc/software/bios/isr.c
index fa6e8db96..4ec7b8c2f 100644
--- a/litex/soc/software/bios/isr.c
+++ b/litex/soc/software/bios/isr.c
@@ -76,6 +76,40 @@ void isr(void)
 #define ECALL 11
 #define RISCV_TEST
 
+void isr(void)
+{
+    unsigned int cause = csrr(mcause) & IRQ_MASK;
+
+    if (csrr(mcause) & 0x80000000) {
+#ifndef UART_POLLING
+        if (cause == (UART_INTERRUPT+FIRQ_OFFSET)){
+            uart_isr();
+        }
+#endif
+    } else {
+#ifdef RISCV_TEST
+        int gp;
+        asm volatile ("mv %0, gp" : "=r"(gp));
+        printf("E %d\n", cause);
+        if (cause == INVINST) {
+            printf("Inv Instr\n");
+            for(;;);
+        }
+        if (cause == ECALL) {
+            printf("Ecall (gp: %d)\n", gp);
+            csrw(mepc, csrr(mepc)+4);
+        }
+#endif
+    }
+}
+#elif defined(__cv32e41p__)
+
+#define FIRQ_OFFSET 16
+#define IRQ_MASK 0x7FFFFFFF
+#define INVINST 2
+#define ECALL 11
+#define RISCV_TEST
+
 void isr(void)
 {
     unsigned int cause = csrr(mcause) & IRQ_MASK;