diff --git a/litex/soc/cores/cpu/ibex/crt0.S b/litex/soc/cores/cpu/ibex/crt0.S
index d80d3e25c..dcb598e59 100644
--- a/litex/soc/cores/cpu/ibex/crt0.S
+++ b/litex/soc/cores/cpu/ibex/crt0.S
@@ -1,44 +1,55 @@
-#define MIE_MEIE  0x800 
-#define MIE_MFIE  0x7FFF0000
-#define MIE       (MIE_MEIE|MIE_MFIE)
+.global main
+.global isr
+.global _start
 
-  .global _start
 _start:
-  j reset_vector
+  j crt_init
+  nop
+  nop
+  nop
+  nop
+  nop
+  nop
+  nop
 
-reset_vector:
-  la sp, _fstack
-  la t0, trap_vector
-  csrw mtvec, t0
+.balign 256
 
-  // initialize .data
-  la t0, _fdata
-  la t1, _edata
-  la t2, _fdata_rom
-1:  beq t0, t1, 2f
-  lw t3, 0(t2)
-  sw t3, 0(t0)
-  addi t0, t0, 4
-  addi t2, t2, 4
-  j 1b
-2:
+vector_table:
+  j trap_entry # 0 unused
+  j trap_entry # 1 unused
+  j trap_entry # 2 unused
+  j trap_entry # 3 software
+  j trap_entry # 4 unused
+  j trap_entry # 5 unused
+  j trap_entry # 6 unused
+  j trap_entry # 7 timer
+  j trap_entry # 8 unused
+  j trap_entry # 9 unused
+  j trap_entry # 10 unused
+  j trap_entry # 11 external
+  j trap_entry # 12 unused
+  j trap_entry # 13 unused
+  j trap_entry # 14 unused
+  j trap_entry # 15 unused
+  j trap_entry # 16 firq0
+  j trap_entry # 17 firq1
+  j trap_entry # 18 firq2
+  j trap_entry # 19 firq3
+  j trap_entry # 20 firq4
+  j trap_entry # 21 firq5
+  j trap_entry # 22 firq6
+  j trap_entry # 23 firq7
+  j trap_entry # 24 firq8
+  j trap_entry # 25 firq9
+  j trap_entry # 26 firq10
+  j trap_entry # 27 firq11
+  j trap_entry # 28 firq12
+  j trap_entry # 29 firq13
+  j trap_entry # 30 firq14
+  j trap_entry # 31 unused
 
-  // initialize .bss
-  la t0, _fbss
-  la t1, _ebss
-1:  beq t0, t1, 3f
-  sw zero, 0(t0)
-  addi t0, t0, 4
-  j 1b
-3:
-  // enable external interrupts
-  li t0, 0x7FFF0880
-  csrs mie, t0
-
-  call main
-1:  j 1b
-
-trap_vector:
+.global  trap_entry
+trap_entry:
   addi sp, sp, -16*4
   sw ra,  0*4(sp)
   sw t0,  1*4(sp)
@@ -75,3 +86,40 @@ trap_vector:
   lw t6, 15*4(sp)
   addi sp, sp, 16*4
   mret
+  .text
+
+crt_init:
+  la sp, _fstack
+  la t0, vector_table
+  csrw mtvec, t0
+
+data_init:
+  la t0, _fdata
+  la t1, _edata
+  la t2, _fdata_rom
+data_loop:  
+  beq t0, t1, data_done
+  lw t3, 0(t2)
+  sw t3, 0(t0)
+  addi t0, t0, 4
+  addi t2, t2, 4
+  j data_loop
+data_done:
+
+bss_init:
+  la t0, _fbss
+  la t1, _ebss
+bss_loop:  
+  beq t0, t1, bss_done
+  sw zero, 0(t0)
+  addi t0, t0, 4
+  j bss_loop
+bss_done:
+
+  li t0, 0x7FFF0880   // enable external interrupts
+  csrs mie, t0
+  call main
+
+infinit_loop:
+  j infinit_loop
+
diff --git a/litex/soc/cores/cpu/ibex/csr-defs.h b/litex/soc/cores/cpu/ibex/csr-defs.h
index d98e8dfb7..e8c2b4735 100644
--- a/litex/soc/cores/cpu/ibex/csr-defs.h
+++ b/litex/soc/cores/cpu/ibex/csr-defs.h
@@ -1,10 +1,12 @@
 #ifndef CSR_DEFS__H
 #define CSR_DEFS__H
 
+/*Reference : https://ibex-core.readthedocs.io/en/latest/03_reference/cs_registers.html# */
+
 #define CSR_MSTATUS_MIE 0x8
 
-#define CSR_IRQ_MASK 0xBC0
-#define CSR_IRQ_PENDING 0xFC0
+#define CSR_IRQ_MASK 0x304
+#define CSR_IRQ_PENDING 0x344
 
 #define CSR_DCACHE_INFO 0xCC0
 
diff --git a/litex/soc/cores/cpu/ibex/irq.h b/litex/soc/cores/cpu/ibex/irq.h
index f1dd4c285..79972c3ff 100644
--- a/litex/soc/cores/cpu/ibex/irq.h
+++ b/litex/soc/cores/cpu/ibex/irq.h
@@ -20,17 +20,21 @@ static inline void irq_setie(unsigned int ie)
 
 static inline unsigned int irq_getmask(void)
 {
-    return 0; // FIXME
+	unsigned int mask;
+	asm volatile ("csrr %0, %1" : "=r"(mask) : "i"(CSR_IRQ_MASK));
+	return mask;
 }
 
 static inline void irq_setmask(unsigned int mask)
 {
-    // FIXME
+	// asm volatile ("csrw %0, %1" :: "i"(CSR_IRQ_MASK), "r"(mask));
 }
 
 static inline unsigned int irq_pending(void)
 {
-    return 0;// FIXME
+	unsigned int pending;
+	asm volatile ("csrr %0, %1" : "=r"(pending) : "i"(CSR_IRQ_PENDING));
+	return pending;
 }
 
 #ifdef __cplusplus
diff --git a/litex/soc/software/bios/isr.c b/litex/soc/software/bios/isr.c
index 2c00a03da..041053c94 100644
--- a/litex/soc/software/bios/isr.c
+++ b/litex/soc/software/bios/isr.c
@@ -68,7 +68,7 @@ void isr(void)
 		*((unsigned int *)PLIC_CLAIM) = claim;
 	}
 }
-#elif defined(__cv32e40p__) || defined(__ibex__)
+#elif defined(__cv32e40p__)
 
 #define FIRQ_OFFSET 16
 #define IRQ_MASK 0x7FFFFFFF
@@ -106,36 +106,21 @@ void isr(void)
 
 #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;
-	puts("isr");
-    if (csrr(mcause) & 0x80000000) {
+    __attribute__((unused)) unsigned int irqs;
+
+	irqs = irq_pending() & irq_getmask();
+
+#ifdef CSR_UART_BASE
 #ifndef UART_POLLING
-        if (cause == (UART_INTERRUPT+FIRQ_OFFSET)){
-            uart_isr();
-        }
+	if(irqs & (1 << (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(__microwatt__)
 
 void isr(uint64_t vec)