diff --git a/firmware/SOURCES b/firmware/SOURCES
index 12774b051f..a4be2a5f25 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -488,6 +488,8 @@ target/arm/pnx0101/timer-pnx0101.c
 #endif
 
 #if CONFIG_CPU == IMX233
+target/arm/imx233/i2c-imx233.c
+target/arm/imx233/lcdif-imx233.c
 target/arm/imx233/clkctrl-imx233.c
 target/arm/imx233/system-imx233.c
 target/arm/imx233/timrot-imx233.c
@@ -1448,13 +1450,11 @@ target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
 #ifdef SANSA_FUZEPLUS
 #ifndef SIMULATOR
 #ifndef BOOTLOADER
-drivers/generic_i2c.c
 drivers/synaptics-rmi.c
 #endif
 target/arm/imx233/sansa-fuzeplus/backlight-fuzeplus.c
 target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
 target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
-target/arm/imx233/lcdif-imx233.c
 #endif /* SIMULATOR */
 #endif
 
diff --git a/firmware/drivers/synaptics-rmi.c b/firmware/drivers/synaptics-rmi.c
index c6a1bae168..c979927fee 100644
--- a/firmware/drivers/synaptics-rmi.c
+++ b/firmware/drivers/synaptics-rmi.c
@@ -19,20 +19,18 @@
  *
  ****************************************************************************/
 #include "system.h"
-#include "generic_i2c.h"
 #include "synaptics-rmi.h"
+#include "i2c.h"
 
 static int rmi_cur_page;
 static int rmi_i2c_addr;
-static int rmi_i2c_bus;
 
 /* NOTE:
  * RMI over i2c supports some special aliases on page 0x2 but this driver don't
  * use them */
 
-int rmi_init(int i2c_bus_index, int i2c_dev_addr)
+int rmi_init(int i2c_dev_addr)
 {
-    rmi_i2c_bus = i2c_bus_index;
     rmi_i2c_addr = i2c_dev_addr;
     rmi_cur_page = 0x4;
     return 0;
@@ -44,7 +42,7 @@ static int rmi_select_page(unsigned char page)
     if(page != rmi_cur_page)
     {
         rmi_cur_page = page;
-        return i2c_write_data(rmi_i2c_bus, rmi_i2c_addr, RMI_PAGE_SELECT, &page, 1);
+        return i2c_writemem(rmi_i2c_addr, RMI_PAGE_SELECT, &page, 1);
     }
     else
         return 0;
@@ -52,9 +50,10 @@ static int rmi_select_page(unsigned char page)
 
 int rmi_read(int address, int byte_count, unsigned char *buffer)
 {
-    if(rmi_select_page(address >> 8) < 0)
-        return -1;
-    return i2c_read_data(rmi_i2c_bus, rmi_i2c_addr, address & 0xff, buffer, byte_count);
+    int ret;
+    if((ret = rmi_select_page(address >> 8)) < 0)
+        return ret;
+    return i2c_readmem(rmi_i2c_addr, address & 0xff, buffer, byte_count);
 }
 
 int rmi_read_single(int address)
@@ -66,9 +65,10 @@ int rmi_read_single(int address)
 
 int rmi_write(int address, int byte_count, const unsigned char *buffer)
 {
-    if(rmi_select_page(address >> 8) < 0)
-        return -1;
-    return i2c_write_data(rmi_i2c_bus, rmi_i2c_addr, address & 0xff, buffer, byte_count);
+    int ret;
+    if((ret = rmi_select_page(address >> 8)) < 0)
+        return ret;
+    return i2c_writemem(rmi_i2c_addr, address & 0xff, buffer, byte_count);
 }
 
 int rmi_write_single(int address, unsigned char byte)
diff --git a/firmware/export/i2c.h b/firmware/export/i2c.h
index 49529c4635..ac9ddba055 100644
--- a/firmware/export/i2c.h
+++ b/firmware/export/i2c.h
@@ -27,6 +27,7 @@ extern void i2c_end(void);
 extern int i2c_write(int device, const unsigned char* buf, int count );
 extern int i2c_read(int device, unsigned char* buf, int count );
 extern int i2c_readmem(int device, int address, unsigned char* buf, int count );
+extern int i2c_writemem(int device, int address, const unsigned char* buf, int count );
 extern void i2c_outb(unsigned char byte);
 extern unsigned char i2c_inb(int ack);
 extern void i2c_start(void);
diff --git a/firmware/export/synaptics-rmi.h b/firmware/export/synaptics-rmi.h
index 2d7a0c8c1e..38a9955dcc 100644
--- a/firmware/export/synaptics-rmi.h
+++ b/firmware/export/synaptics-rmi.h
@@ -114,7 +114,7 @@ struct rmi_2d_gesture_data_t
  * the generic_i2c driver; the i2c_dev_addr is the i2c address of the device.
  * NOTE: the driver automatically handles the page select mechanism used for
  *       RMI over i2c and assumes a standard page select register at 0xff. */
-int rmi_init(int i2c_bus_index, int i2c_dev_addr);
+int rmi_init(int i2c_dev_addr);
 /* Read one or more registers.
  * WARNING: don't cross a page boundary ! */
 int rmi_read(int address, int byte_count, unsigned char *buffer);
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