I2C, register the handler in init

i2cspi_BaS_gcc/sources/i2c_firebee.c

@@ -8,6 +8,8 @@
 
 #include <fifo.h>
 
+int register_handler(uint8_t,uint8_t,void (*func)());
+
 static struct
 {
 	uint8_t state;
@@ -22,22 +24,6 @@ char local_buf[BUFSIZ];
 
 #define FIFO    &i2c_param.fifo
 
-/*
- * I2Cinit: I2C initilazation as master
- *
- * Parameters: None.
- *
- * Return : None.
- */
-void I2C_Init()
-{
-	i2c_param.fifo.buf = local_buf;
-	i2c_param.fifo.size = BUFSIZ;
-	i2c_param.delay = 133 * 10L; // We can safely ignore this
-	i2c_param.len = 0;
-	I2C_ioctl(0, 0);
-}
-
 void __attribute__ ((interrupt)) I2C_InterruptHandler(void)
 {
 	char ch;
@@ -123,6 +109,23 @@ void __attribute__ ((interrupt)) I2C_InterruptHandler(void)
 	}
 }
 
+/*
+ * I2Cinit: I2C initilazation as master
+ *
+ * Parameters: None.
+ *
+ * Return : None.
+ */
+void I2C_Init()
+{
+    i2c_param.fifo.buf = local_buf;
+    i2c_param.fifo.size = BUFSIZ;
+    i2c_param.delay = 133 * 10L; // We can safely ignore this
+    i2c_param.len = 0;
+    I2C_ioctl(0, 0);
+    register_handler(1,40,I2C_InterruptHandler);
+}
+
 void I2C_send(unsigned short device, unsigned char *buf, unsigned short len)
 {
 	if (len > i2c_param.fifo.size)

i2cspi_BaS_gcc/sources/interrupts.c

@@ -59,7 +59,7 @@ register_handler(uint8_t priority, uint8_t intr, void (*func)())
     // Return the saved priority level
     __asm__ volatile (
                 "move.w (%%sp)+,%%d2\n\t" \
-                "move.w %%d2,sr\n\t"
+                "move.w %%d2,%%sr\n\t"
                 : \
                 : \
                 : \