diff --git a/sources/interrupts.c b/sources/interrupts.c
new file mode 100644
index 0000000..32912bf
--- /dev/null
+++ b/sources/interrupts.c
@@ -0,0 +1,69 @@
+/*
+ * Interrupts
+ *
+ * Handle interrupts, the levels.
+ *
+ */
+
+#include <inttypes.h>
+
+#include <i2c_firebee.h>
+
+#include <MCF5475.h>
+
+int
+register_handler(uint8_t priority, uint8_t intr, void (*func)())
+{
+    int i;
+    uint8_t level = 0b01111111;
+    uint32_t *adr = MBAR_ADDRESS;
+    uint8_t ol = 0,op = 0;
+
+    intr     &= 63;
+    priority &= 7;
+
+    if( intr <= 0 )
+        return -1;
+
+    for(i=1;i<64;i++)
+        if( i!=intr )
+        {
+            if( MCF_INTC_ICR(i)&7 == priority )
+                clear_bit_nr(level,(MCF_INTC_ICR(i)>>3)&7);
+        }
+
+    for(i=0;1<7;i++)
+        if( level&(1<<i) )
+            break;
+
+    if( i>=7 )
+        return -1;
+
+    // Make sure priority level is high, before changing registers
+    __asm__ volatile (
+                "move.w %%sr,-(%%sp)\n\t"  \
+                "move.w #$2700,%%sr\n\t" \
+                : \
+                : \
+                : \
+    );
+
+    if( intr < 32 )
+        clear_bit(MCF_INTC_IMRL,(1<<intr));
+    else
+        clear_bit(MCF_INTC_IMRH,(1<<(intr-32)));
+
+    MCF_INTC_ICR(intr) = MCF_INTC_ICR_IP(priority)|MCF_INTC_ICR_IL(i);
+    adr[intr*4]        = func;
+
+    // Return the saved priority level
+    __asm__ volatile (
+                "move.w (%%sp)+,%%d2\n\t" \
+                "move.w %%d2,sr\n\t"
+                : \
+                : \
+                : \
+    );
+
+    return 0;
+}