imx233/fuze+: replace software i2c by hardware i2c, make some code more correct, reduce code size of lcd init sequences

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@30120 a1c6a512-1295-4272-9138-f99709370657

8 files changed, 222 insertions, 220 deletions

diff --git a/firmware/target/arm/imx233/clkctrl-imx233.h b/firmware/target/arm/imx233/clkctrl-imx233.h
index 2a12129171..f1a51e25b2 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.h
+++ b/firmware/target/arm/imx233/clkctrl-imx233.h
@@ -42,6 +42,11 @@
 #define HW_CLKCTRL_HBUS__DIV_BP     0
 #define HW_CLKCTRL_HBUS__DIV_BM     0x1f
 
+#define HW_CLKCTRL_XBUS     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x40))
+#define HW_CLKCTRL_XBUS__DIV_BP     0
+#define HW_CLKCTRL_XBUS__DIV_BM     0x3ff
+#define HW_CLKCTRL_XBUS__BUSY       (1 << 31)
+
 #define HW_CLKCTRL_XTAL     (*(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x50))
 #define HW_CLKCTRL_XTAL__TIMROT_CLK32K_GATE (1 << 26)
 
diff --git a/firmware/target/arm/imx233/dma-imx233.h b/firmware/target/arm/imx233/dma-imx233.h
index ce13aba89f..fcf1a2c5c2 100644
--- a/firmware/target/arm/imx233/dma-imx233.h
+++ b/firmware/target/arm/imx233/dma-imx233.h
@@ -73,6 +73,7 @@
 /* APHX channels */
 #define HW_APBX_AUDIO_ADC   0
 #define HW_APBX_AUDIO_DAC   1
+#define HW_APBX_I2C         3
 
 #define HW_APBX_BASE        0x80024000
 
@@ -123,6 +124,7 @@ struct apb_dma_command_t
 
 #define APB_SSP(ssp)        APBH_DMA_CHANNEL(HW_APBH_SSP(ssp))
 #define APB_AUDIO_ADC       APBX_DMA_CHANNEL(HW_APBX_AUDIO_ADC)
+#define APB_I2C             APBX_DMA_CHANNEL(HW_APBX_I2C)
 
 #define HW_APB_CHx_CMD__COMMAND_BM         0x3
 #define HW_APB_CHx_CMD__COMMAND__NO_XFER   0
diff --git a/firmware/target/arm/imx233/pinctrl-imx233.h b/firmware/target/arm/imx233/pinctrl-imx233.h
index 4e3a9a035e..a2e02adec4 100644
--- a/firmware/target/arm/imx233/pinctrl-imx233.h
+++ b/firmware/target/arm/imx233/pinctrl-imx233.h
@@ -53,8 +53,7 @@
 
 static inline void imx233_pinctrl_init(void)
 {
-    __REG_CLR(HW_PINCTRL_CTRL) = __BLOCK_CLKGATE;
-    __REG_CLR(HW_PINCTRL_CTRL) = __BLOCK_SFTRST;
+    __REG_CLR(HW_PINCTRL_CTRL) = __BLOCK_CLKGATE | __BLOCK_SFTRST;
 }
 
 static inline void imx233_set_pin_drive_strength(unsigned bank, unsigned pin, unsigned strength)
diff --git a/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c b/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
index f6efe6d6a8..1b3a529f2d 100644
--- a/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
+++ b/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
@@ -28,59 +28,6 @@
 #include "string.h"
 
 #ifndef BOOTLOADER
-static void i2c_scl_dir(bool out)
-{
-    imx233_enable_gpio_output(0, 30, out);
-}
-
-static void i2c_sda_dir(bool out)
-{
-    imx233_enable_gpio_output(0, 31, out);
-}
-
-static void i2c_scl_out(bool high)
-{
-    imx233_set_gpio_output(0, 30, high);
-}
-
-static void i2c_sda_out(bool high)
-{
-    imx233_set_gpio_output(0, 31, high);
-}
-
-static bool i2c_scl_in(void)
-{
-    return imx233_get_gpio_input_mask(0, 1 << 30);
-}
-
-static bool i2c_sda_in(void)
-{
-    return imx233_get_gpio_input_mask(0, 1 << 31);
-}
-
-static void i2c_delay(int d)
-{
-    udelay(d);
-}
-
-struct i2c_interface btn_i2c =
-{
-    .scl_dir = i2c_scl_dir,
-    .sda_dir = i2c_sda_dir,
-    .scl_out = i2c_scl_out,
-    .sda_out = i2c_sda_out,
-    .scl_in = i2c_scl_in,
-    .sda_in = i2c_sda_in,
-    .delay = i2c_delay,
-    .delay_hd_sta = 4,
-    .delay_hd_dat = 5,
-    .delay_su_dat = 1,
-    .delay_su_sto = 4,
-    .delay_su_sta = 5,
-    .delay_thigh = 4
-};
-
-int rmi_i2c_bus = -1;
 
 void button_debug_screen(void)
 {
@@ -221,9 +168,6 @@ void button_debug_screen(void)
 
 void button_init_device(void)
 {
-    rmi_i2c_bus = i2c_add_node(&btn_i2c);
-    rmi_init(rmi_i2c_bus, 0x40);
-
     /* Synaptics TouchPad information:
      * - product id: 1533
      * - nr function: 1 (0x10 = 2D touchpad)
@@ -244,7 +188,17 @@ void button_init_device(void)
      * - Resolution: 82
      *
      * ATTENTION line: B0P27 asserted low
+     *
+     * The B0P26 line seems to be related to the touchpad
      */
+     
+    /* for touchpad ? */
+    imx233_set_pin_function(0, 26, PINCTRL_FUNCTION_GPIO);
+    imx233_enable_gpio_output(0, 26, false);
+    imx233_set_pin_drive_strength(0, 26, PINCTRL_DRIVE_8mA);
+    
+    rmi_init(0x40);
+
     rmi_write_single(RMI_2D_SENSITIVITY_ADJ, 5);
     rmi_write_single(RMI_2D_GESTURE_SETTINGS,
         RMI_2D_GESTURE_PRESS_TIME_300MS |
diff --git a/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c b/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
index f2fbf706d7..bc8a20d2ff 100644
--- a/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
+++ b/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
@@ -62,7 +62,6 @@ static void setup_lcd_pins(bool use_lcdif)
     }
     else
     {
-        
         __REG_SET(HW_PINCTRL_MUXSEL(2)) = 0xffffffff; /* lcd_d{0-15} */
         imx233_enable_gpio_output_mask(1, 0x3ffffff, false); /* lcd_{d{0-17},reset,rs,wr,cs,dotclk,enable,hsync,vsync} */
         imx233_set_pin_function(1, 16, PINCTRL_FUNCTION_GPIO); /* lcd_d16 */
@@ -212,6 +211,28 @@ static uint32_t lcd_read_reg(uint32_t reg)
     return decode_18_to_16(data_in);
 }
 
+#define REG_MDELAY  0xffffffff
+struct lcd_sequence_entry_t
+{
+    uint32_t reg, data;
+};
+
+static void lcd_send_sequence(struct lcd_sequence_entry_t *seq, unsigned count)
+{
+    for(;count-- > 0; seq++)
+    {
+        if(seq->reg == REG_MDELAY)
+            mdelay(seq->data);
+        else
+            lcd_write_reg(seq->reg, seq->data);
+    }
+}
+
+#define _begin_seq() static struct lcd_sequence_entry_t __seq[] = {
+#define _mdelay(a) {REG_MDELAY, a},
+#define _lcd_write_reg(a, b) {a, b},
+#define _end_seq() }; lcd_send_sequence(__seq, sizeof(__seq) / sizeof(__seq[0]));
+
 static void lcd_init_seq_7783(void)
 {
     __REG_SET(HW_LCDIF_CTRL1) = HW_LCDIF_CTRL1__RESET;
@@ -219,131 +240,129 @@ static void lcd_init_seq_7783(void)
     __REG_CLR(HW_LCDIF_CTRL1) = HW_LCDIF_CTRL1__RESET;
     mdelay(10);
     __REG_SET(HW_LCDIF_CTRL1) = HW_LCDIF_CTRL1__RESET;
-    mdelay(200);
-    lcd_write_reg(1, 0x100);
-    lcd_write_reg(2, 0x700);
-    lcd_write_reg(3, 0x1030);
-    lcd_write_reg(7, 0x121);
-    lcd_write_reg(8, 0x302);
-    lcd_write_reg(9, 0x200);
-    lcd_write_reg(0xa, 0);
-    lcd_write_reg(0x10, 0x790);
-    lcd_write_reg(0x11, 5);
-    lcd_write_reg(0x12, 0);
-    lcd_write_reg(0x13, 0);
-    mdelay(100);
-    lcd_write_reg(0x10, 0x12b0);
-    mdelay(100);
-    lcd_write_reg(0x11, 7);
-    mdelay(100);
-    lcd_write_reg(0x12, 0x89);
-    lcd_write_reg(0x13, 0x1d00);
-    lcd_write_reg(0x29, 0x2f);
-    mdelay(50);
-    lcd_write_reg(0x30, 0);
-    lcd_write_reg(0x31, 0x505);
-    lcd_write_reg(0x32, 0x205);
-    lcd_write_reg(0x35, 0x206);
-    lcd_write_reg(0x36, 0x408);
-    lcd_write_reg(0x37, 0);
-    lcd_write_reg(0x38, 0x504);
-    lcd_write_reg(0x39, 0x206);
-    lcd_write_reg(0x3c, 0x206);
-    lcd_write_reg(0x3d, 0x408);
-    lcd_write_reg(0x50, 0); /* left X ? */ 
-    lcd_write_reg(0x51, 0xef); /* right X ? */
-    lcd_write_reg(0x52, 0); /* top Y ? */
-    lcd_write_reg(0x53, 0x13f); /* bottom Y ? */
-    lcd_write_reg(0x20, 0); /* left X ? */ 
-    lcd_write_reg(0x21, 0); /* top Y ? */
-    lcd_write_reg(0x60, 0xa700);
-    lcd_write_reg(0x61, 1);
-    lcd_write_reg(0x90, 0x33);
-    lcd_write_reg(0x2b, 0xa);
-    lcd_write_reg(9, 0);
-    lcd_write_reg(7, 0x133);
-    mdelay(50);
-    lcd_write_reg(0x22, 0);
+
+    _begin_seq()
+    _mdelay(200)
+    _lcd_write_reg(1, 0x100)
+    _lcd_write_reg(2, 0x700)
+    _lcd_write_reg(3, 0x1030)
+    _lcd_write_reg(7, 0x121)
+    _lcd_write_reg(8, 0x302)
+    _lcd_write_reg(9, 0x200)
+    _lcd_write_reg(0xa, 0)
+    _lcd_write_reg(0x10, 0x790)
+    _lcd_write_reg(0x11, 5)
+    _lcd_write_reg(0x12, 0)
+    _lcd_write_reg(0x13, 0)
+    _mdelay(100)
+    _lcd_write_reg(0x10, 0x12b0)
+    _mdelay(100)
+    _lcd_write_reg(0x11, 7)
+    _mdelay(100)
+    _lcd_write_reg(0x12, 0x89)
+    _lcd_write_reg(0x13, 0x1d00)
+    _lcd_write_reg(0x29, 0x2f)
+    _mdelay(50)
+    _lcd_write_reg(0x30, 0)
+    _lcd_write_reg(0x31, 0x505)
+    _lcd_write_reg(0x32, 0x205)
+    _lcd_write_reg(0x35, 0x206)
+    _lcd_write_reg(0x36, 0x408)
+    _lcd_write_reg(0x37, 0)
+    _lcd_write_reg(0x38, 0x504)
+    _lcd_write_reg(0x39, 0x206)
+    _lcd_write_reg(0x3c, 0x206)
+    _lcd_write_reg(0x3d, 0x408)
+    _lcd_write_reg(0x50, 0) /* left X ? */ 
+    _lcd_write_reg(0x51, 0xef) /* right X ? */
+    _lcd_write_reg(0x52, 0) /* top Y ? */
+    _lcd_write_reg(0x53, 0x13f) /* bottom Y ? */
+    _lcd_write_reg(0x20, 0) /* left X ? */ 
+    _lcd_write_reg(0x21, 0) /* top Y ? */
+    _lcd_write_reg(0x60, 0xa700)
+    _lcd_write_reg(0x61, 1)
+    _lcd_write_reg(0x90, 0x33)
+    _lcd_write_reg(0x2b, 0xa)
+    _lcd_write_reg(9, 0)
+    _lcd_write_reg(7, 0x133)
+    _mdelay(50)
+    _lcd_write_reg(0x22, 0)
+    _end_seq()
 }
 
 static void lcd_init_seq_9325(void)
 {
-    lcd_write_reg(0xe5, 0x78f0);
-    lcd_write_reg(0xe3, 0x3008);
-    lcd_write_reg(0xe7, 0x12);
-    lcd_write_reg(0xef, 0x1231);
-    lcd_write_reg(0, 1);
-    lcd_write_reg(1, 0x100);
-    lcd_write_reg(2, 0x700);
-    lcd_write_reg(3, 0x1030);
-    lcd_write_reg(4, 0);
-    lcd_write_reg(8, 0x207);
-    lcd_write_reg(9, 0);
-    lcd_write_reg(0xa, 0);
-    lcd_write_reg(0xc, 0);
-    lcd_write_reg(0xd, 0);
-    lcd_write_reg(0xf, 0);
-    lcd_write_reg(0x10, 0);
-    lcd_write_reg(0x11, 7);
-    lcd_write_reg(0x12, 0);
-    lcd_write_reg(0x13, 0);
-    mdelay(20);
-    lcd_write_reg(0x10, 0x1290);
-    lcd_write_reg(0x11, 7);
-    mdelay(50);
-    lcd_write_reg(0x12, 0x19);
-    mdelay(50);
-    lcd_write_reg(0x13, 0x1700);
-    lcd_write_reg(0x29, 0x14);
-    mdelay(50);
-    lcd_write_reg(0x20, 0);
-    lcd_write_reg(0x21, 0);
-    lcd_write_reg(0x30, 0x504);
-    lcd_write_reg(0x31, 7);
-    lcd_write_reg(0x32, 6);
-    lcd_write_reg(0x35, 0x106);
-    lcd_write_reg(0x36, 0x202);
-    lcd_write_reg(0x37, 0x504);
-    lcd_write_reg(0x38, 0x500);
-    lcd_write_reg(0x39, 0x706);
-    lcd_write_reg(0x3c, 0x204);
-    lcd_write_reg(0x3d, 0x202);
-    lcd_write_reg(0x50, 0);
-    lcd_write_reg(0x51, 0xef);
-    lcd_write_reg(0x52, 0);
-    lcd_write_reg(0x53, 0x13f);
-    lcd_write_reg(0x60, 0xa700);
-    lcd_write_reg(0x61, 1);
-    lcd_write_reg(0x6a, 1);
-    lcd_write_reg(0x2b, 0xd);
-    mdelay(50);
-    lcd_write_reg(0x90, 0x11);
-    lcd_write_reg(0x92, 0x600);
-    lcd_write_reg(0x93, 3);
-    lcd_write_reg(0x95, 0x110);
-    lcd_write_reg(0x97, 0);
-    lcd_write_reg(0x98, 0);
-    lcd_write_reg(7, 0x173);
-    lcd_write_reg(0x22, 0);
+    _begin_seq()
+    _lcd_write_reg(0xe5, 0x78f0)
+    _lcd_write_reg(0xe3, 0x3008)
+    _lcd_write_reg(0xe7, 0x12)
+    _lcd_write_reg(0xef, 0x1231)
+    _lcd_write_reg(0, 1)
+    _lcd_write_reg(1, 0x100)
+    _lcd_write_reg(2, 0x700)
+    _lcd_write_reg(3, 0x1030)
+    _lcd_write_reg(4, 0)
+    _lcd_write_reg(8, 0x207)
+    _lcd_write_reg(9, 0)
+    _lcd_write_reg(0xa, 0)
+    _lcd_write_reg(0xc, 0)
+    _lcd_write_reg(0xd, 0)
+    _lcd_write_reg(0xf, 0)
+    _lcd_write_reg(0x10, 0)
+    _lcd_write_reg(0x11, 7)
+    _lcd_write_reg(0x12, 0)
+    _lcd_write_reg(0x13, 0)
+    _mdelay(20)
+    _lcd_write_reg(0x10, 0x1290)
+    _lcd_write_reg(0x11, 7)
+    _mdelay(50)
+    _lcd_write_reg(0x12, 0x19)
+    _mdelay(50)
+    _lcd_write_reg(0x13, 0x1700)
+    _lcd_write_reg(0x29, 0x14)
+    _mdelay(50)
+    _lcd_write_reg(0x20, 0)
+    _lcd_write_reg(0x21, 0)
+    _lcd_write_reg(0x30, 0x504)
+    _lcd_write_reg(0x31, 7)
+    _lcd_write_reg(0x32, 6)
+    _lcd_write_reg(0x35, 0x106)
+    _lcd_write_reg(0x36, 0x202)
+    _lcd_write_reg(0x37, 0x504)
+    _lcd_write_reg(0x38, 0x500)
+    _lcd_write_reg(0x39, 0x706)
+    _lcd_write_reg(0x3c, 0x204)
+    _lcd_write_reg(0x3d, 0x202)
+    _lcd_write_reg(0x50, 0)
+    _lcd_write_reg(0x51, 0xef)
+    _lcd_write_reg(0x52, 0)
+    _lcd_write_reg(0x53, 0x13f)
+    _lcd_write_reg(0x60, 0xa700)
+    _lcd_write_reg(0x61, 1)
+    _lcd_write_reg(0x6a, 1)
+    _lcd_write_reg(0x2b, 0xd)
+    _mdelay(50)
+    _lcd_write_reg(0x90, 0x11)
+    _lcd_write_reg(0x92, 0x600)
+    _lcd_write_reg(0x93, 3)
+    _lcd_write_reg(0x95, 0x110)
+    _lcd_write_reg(0x97, 0)
+    _lcd_write_reg(0x98, 0)
+    _lcd_write_reg(7, 0x173)
+    _lcd_write_reg(0x22, 0)
+    _end_seq()
 }
 
 void lcd_init_device(void)
 {
     setup_lcdif();
     setup_lcdif_clock();
-    
+
     for(int i = 0; i < 10; i++)
     {
-        uint32_t kind = lcd_read_reg(0);
-        if(kind == LCD_KIND_7783 || kind == LCD_KIND_9325)
-        {
-            lcd_kind = kind;
+        lcd_kind = lcd_read_reg(0);
+        if(lcd_kind == LCD_KIND_7783 || lcd_kind == LCD_KIND_9325)
             break;
-        }
-        else
-        {
-            lcd_kind = LCD_KIND_OTHER;
-        }
     }
     mdelay(5);
     switch(lcd_kind)
@@ -351,6 +370,7 @@ void lcd_init_device(void)
         case LCD_KIND_7783: lcd_init_seq_7783(); break;
         case LCD_KIND_9325: lcd_init_seq_9325(); break;
         default:
+            lcd_kind = LCD_KIND_9325;
             lcd_init_seq_7783(); break;
     }
 }
@@ -360,30 +380,34 @@ static void lcd_enable_7783(bool enable)
 {
     if(!enable)
     {
-        lcd_write_reg(7, 0x131);
-        mdelay(50);
-        lcd_write_reg(7, 0x20);
-        mdelay(50);
-        lcd_write_reg(0x10, 0x82);
-        mdelay(50);
+        _begin_seq()
+        _lcd_write_reg(7, 0x131)
+        _mdelay(50)
+        _lcd_write_reg(7, 0x20)
+        _mdelay(50)
+        _lcd_write_reg(0x10, 0x82)
+        _mdelay(50)
+        _end_seq()
     }
     else
     {
-        lcd_write_reg(0x11, 5);
-        lcd_write_reg(0x10, 0x12b0);
-        mdelay(50);
-        lcd_write_reg(7, 0x11);
-        mdelay(50);
-        lcd_write_reg(0x12, 0x89);
-        mdelay(50);
-        lcd_write_reg(0x13, 0x1d00);
-        mdelay(50);
-        lcd_write_reg(0x29, 0x2f);
-        mdelay(50);
-        lcd_write_reg(0x2b, 0xa);
-        lcd_write_reg(7, 0x133);
-        mdelay(50);
-        lcd_write_reg(0x22, 0);
+        _begin_seq()
+        _lcd_write_reg(0x11, 5)
+        _lcd_write_reg(0x10, 0x12b0)
+        _mdelay(50)
+        _lcd_write_reg(7, 0x11)
+        _mdelay(50)
+        _lcd_write_reg(0x12, 0x89)
+        _mdelay(50)
+        _lcd_write_reg(0x13, 0x1d00)
+        _mdelay(50)
+        _lcd_write_reg(0x29, 0x2f)
+        _mdelay(50)
+        _lcd_write_reg(0x2b, 0xa)
+        _lcd_write_reg(7, 0x133)
+        _mdelay(50)
+        _lcd_write_reg(0x22, 0)
+        _end_seq()
     }
 }
 
@@ -391,36 +415,40 @@ static void lcd_enable_9325(bool enable)
 {
     if(!enable)
     {
-        lcd_write_reg(7, 0x131);
-        mdelay(10);
-        lcd_write_reg(7, 0x130);
-        mdelay(10);
-        lcd_write_reg(7, 0);
-        lcd_write_reg(0x10, 0x80);
-        lcd_write_reg(0x11, 0);
-        lcd_write_reg(0x12, 0);
-        lcd_write_reg(0x13, 0);
-        mdelay(200);
-        lcd_write_reg(0x10, 0x82);
+        _begin_seq()
+        _lcd_write_reg(7, 0x131)
+        _mdelay(10)
+        _lcd_write_reg(7, 0x130)
+        _mdelay(10)
+        _lcd_write_reg(7, 0)
+        _lcd_write_reg(0x10, 0x80)
+        _lcd_write_reg(0x11, 0)
+        _lcd_write_reg(0x12, 0)
+        _lcd_write_reg(0x13, 0)
+        _mdelay(200)
+        _lcd_write_reg(0x10, 0x82)
+        _end_seq()
     }
     else
     {
-        lcd_write_reg(0x10, 0x80);
-        lcd_write_reg(0x11, 0);
-        lcd_write_reg(0x12, 0);
-        lcd_write_reg(0x13, 0);
-        lcd_write_reg(7, 1);
-        mdelay(200);
-        lcd_write_reg(0x10, 0x1290);
-        lcd_write_reg(0x11, 7);
-        mdelay(50);
-        lcd_write_reg(0x12, 0x19);
-        mdelay(50);
-        lcd_write_reg(0x13, 0x1700);
-        lcd_write_reg(0x29, 0x10);
-        mdelay(50);
-        lcd_write_reg(7, 0x133);
-        lcd_write_reg(0x22, 0);
+        _begin_seq()
+        _lcd_write_reg(0x10, 0x80)
+        _lcd_write_reg(0x11, 0)
+        _lcd_write_reg(0x12, 0)
+        _lcd_write_reg(0x13, 0)
+        _lcd_write_reg(7, 1)
+        _mdelay(200)
+        _lcd_write_reg(0x10, 0x1290)
+        _lcd_write_reg(0x11, 7)
+        _mdelay(50)
+        _lcd_write_reg(0x12, 0x19)
+        _mdelay(50)
+        _lcd_write_reg(0x13, 0x1700)
+        _lcd_write_reg(0x29, 0x10)
+        _mdelay(50)
+        _lcd_write_reg(7, 0x133)
+        _lcd_write_reg(0x22, 0)
+        _end_seq()
     }
 }
 
diff --git a/firmware/target/arm/imx233/ssp-imx233.c b/firmware/target/arm/imx233/ssp-imx233.c
index ef137fcfbf..59405bbdc2 100644
--- a/firmware/target/arm/imx233/ssp-imx233.c
+++ b/firmware/target/arm/imx233/ssp-imx233.c
@@ -258,6 +258,7 @@ enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd,
         (3 << HW_APB_CHx_CMD__CMDWORDS_BP) |
         (xfer_size << HW_APB_CHx_CMD__XFER_COUNT_BP);
 
+    __REG_CLR(HW_SSP_CTRL1(ssp)) = HW_SSP_CTRL1__ALL_IRQ;
     imx233_dma_reset_channel(APB_SSP(ssp));
     imx233_dma_start_command(APB_SSP(ssp), &ssp_dma_cmd[ssp - 1].dma);
 
@@ -266,7 +267,10 @@ enum imx233_ssp_error_t imx233_ssp_sd_mmc_transfer(int ssp, uint8_t cmd,
     enum imx233_ssp_error_t ret;
     
     if(semaphore_wait(&ssp_sema[ssp - 1], HZ) == OBJ_WAIT_TIMEDOUT)
+    {
+        imx233_dma_reset_channel(APB_SSP(ssp));
         ret = SSP_TIMEOUT;
+    }
     else if((HW_SSP_CTRL1(ssp) & HW_SSP_CTRL1__ALL_IRQ) == 0)
         ret =  SSP_SUCCESS;
     else if(HW_SSP_CTRL1(ssp) & (HW_SSP_CTRL1__RESP_TIMEOUT_IRQ |
diff --git a/firmware/target/arm/imx233/system-imx233.c b/firmware/target/arm/imx233/system-imx233.c
index 6114ecca08..ab95c453da 100644
--- a/firmware/target/arm/imx233/system-imx233.c
+++ b/firmware/target/arm/imx233/system-imx233.c
@@ -29,6 +29,7 @@
 #include "timrot-imx233.h"
 #include "dma-imx233.h"
 #include "ssp-imx233.h"
+#include "i2c-imx233.h"
 #include "lcd.h"
 #include "backlight-target.h"
 #include "button-target.h"
@@ -51,6 +52,8 @@ default_interrupt(INT_SSP1_DMA);
 default_interrupt(INT_SSP1_ERROR);
 default_interrupt(INT_SSP2_DMA);
 default_interrupt(INT_SSP2_ERROR);
+default_interrupt(INT_I2C_DMA);
+default_interrupt(INT_I2C_ERROR);
 
 typedef void (*isr_t)(void);
 
@@ -66,7 +69,9 @@ static isr_t isr_table[INT_SRC_NR_SOURCES] =
     [INT_SRC_SSP1_DMA] = INT_SSP1_DMA,
     [INT_SRC_SSP1_ERROR] = INT_SSP1_ERROR,
     [INT_SRC_SSP2_DMA] = INT_SSP2_DMA,
-    [INT_SRC_SSP2_ERROR] = INT_SSP2_ERROR
+    [INT_SRC_SSP2_ERROR] = INT_SSP2_ERROR,
+    [INT_SRC_I2C_DMA] = INT_I2C_DMA,
+    [INT_SRC_I2C_ERROR] = INT_I2C_ERROR,
 };
 
 static void UIRQ(void)
@@ -147,6 +152,9 @@ void system_init(void)
     imx233_timrot_init();
     imx233_dma_init();
     imx233_ssp_init();
+    #ifndef BOOTLOADER
+    imx233_i2c_init();
+    #endif
 }
 
 void power_off(void)
diff --git a/firmware/target/arm/imx233/system-target.h b/firmware/target/arm/imx233/system-target.h
index 0f7bde4896..c5c2ed8f0c 100644
--- a/firmware/target/arm/imx233/system-target.h
+++ b/firmware/target/arm/imx233/system-target.h
@@ -87,6 +87,8 @@
 #define INT_SRC_SSP1_DMA    14
 #define INT_SRC_SSP1_ERROR  15
 #define INT_SRC_SSP2_DMA    20
+#define INT_SRC_I2C_DMA     26
+#define INT_SRC_I2C_ERROR   27
 #define INT_SRC_TIMER(nr)   (28 + (nr))
 #define INT_SRC_LCDIF_DMA   45
 #define INT_SRC_LCDIF_ERROR 46