imx233: fix/improve lcdif for stmp3600 and stmp4700, fix drivers

Factorise pin setup, rewrite PIO code, add support for lcdif irq,
handle all the various differences between the stmps, drop yuv
blitting code since it already exists in the common lcd drivers.

Change-Id: Ifc40aed9b3b12f16611ce960602e46a5bc87ae53

6 files changed, 418 insertions, 693 deletions

diff --git a/firmware/target/arm/imx233/creative-zenxfi2/lcd-zenxfi2.c b/firmware/target/arm/imx233/creative-zenxfi2/lcd-zenxfi2.c
index 9ff64c4c14..0131a39233 100644
--- a/firmware/target/arm/imx233/creative-zenxfi2/lcd-zenxfi2.c
+++ b/firmware/target/arm/imx233/creative-zenxfi2/lcd-zenxfi2.c
@@ -34,66 +34,15 @@ static bool lcd_on;
 #endif
 static unsigned lcd_yuv_options = 0;
 
-static void setup_parameters(void)
+static void setup_lcdif(void)
 {
-    imx233_lcdif_reset();
-    imx233_lcdif_set_lcd_databus_width(BV_LCDIF_CTRL_LCD_DATABUS_WIDTH__18_BIT);
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__18_BIT);
+    imx233_lcdif_init();
+    imx233_lcdif_set_lcd_databus_width(18);
+    imx233_lcdif_set_word_length(18);
     imx233_lcdif_set_timings(2, 2, 3, 3);
     imx233_lcdif_enable_underflow_recover(true);
-}
-
-static void setup_lcd_pins(bool use_lcdif)
-{
-    imx233_pinctrl_acquire(1, 18, "lcd reset");
-    imx233_pinctrl_acquire(1, 19, "lcd rs");
-    imx233_pinctrl_acquire(1, 20, "lcd wr");
-    imx233_pinctrl_acquire(1, 21, "lcd cs");
-    imx233_pinctrl_acquire(1, 22, "lcd dotclk");
-    imx233_pinctrl_acquire(1, 23, "lcd enable");
-    imx233_pinctrl_acquire(1, 24, "lcd hsync");
-    imx233_pinctrl_acquire(1, 25, "lcd vsync");
-    //imx233_pinctrl_acquire_mask(1, 0x3ffff, "lcd data");
-    if(use_lcdif)
-    {
-        imx233_pinctrl_set_function(1, 25, PINCTRL_FUNCTION_MAIN); /* lcd_vsync */
-        imx233_pinctrl_set_function(1, 21, PINCTRL_FUNCTION_MAIN); /* lcd_cs */
-        imx233_pinctrl_set_function(1, 22, PINCTRL_FUNCTION_MAIN); /* lcd_dotclk */
-        imx233_pinctrl_set_function(1, 23, PINCTRL_FUNCTION_MAIN); /* lcd_enable */
-        imx233_pinctrl_set_function(1, 24, PINCTRL_FUNCTION_MAIN); /* lcd_hsync */
-        imx233_pinctrl_set_function(1, 18, PINCTRL_FUNCTION_MAIN); /* lcd_reset */
-        imx233_pinctrl_set_function(1, 19, PINCTRL_FUNCTION_MAIN); /* lcd_rs */
-        imx233_pinctrl_set_function(1, 16, PINCTRL_FUNCTION_MAIN); /* lcd_d16 */
-        imx233_pinctrl_set_function(1, 17, PINCTRL_FUNCTION_MAIN); /* lcd_d17 */
-        imx233_pinctrl_set_function(1, 20, PINCTRL_FUNCTION_MAIN); /* lcd_wr */
-        HW_PINCTRL_MUXSELn_CLR(2) = 0xffffffff; /* lcd_d{0-15} */
-    }
-    else
-    {
-        HW_PINCTRL_MUXSELn_SET(2) = 0xffffffff; /* lcd_d{0-15} */
-        HW_PINCTRL_DOEn_CLR(1) = 0x3ffffff; /* lcd_{d{0-17},reset,rs,wr,cs,dotclk,enable,hsync,vsync} */
-        imx233_pinctrl_set_function(1, 16, PINCTRL_FUNCTION_GPIO); /* lcd_d16 */
-        imx233_pinctrl_set_function(1, 17, PINCTRL_FUNCTION_GPIO); /* lcd_d17 */
-        imx233_pinctrl_set_function(1, 19, PINCTRL_FUNCTION_GPIO); /* lcd_rs */
-        imx233_pinctrl_set_function(1, 20, PINCTRL_FUNCTION_GPIO); /* lcd_wr */
-        imx233_pinctrl_set_function(1, 21, PINCTRL_FUNCTION_GPIO); /* lcd_cs */
-        imx233_pinctrl_set_function(1, 22, PINCTRL_FUNCTION_GPIO); /* lcd_dotclk */
-        imx233_pinctrl_set_function(1, 23, PINCTRL_FUNCTION_GPIO); /* lcd_enable */
-        imx233_pinctrl_set_function(1, 24, PINCTRL_FUNCTION_GPIO); /* lcd_hsync */
-        imx233_pinctrl_set_function(1, 25, PINCTRL_FUNCTION_GPIO); /* lcd_vsync */
-    }
-}
-
-static void common_lcd_enable(bool enable)
-{
-    imx233_lcdif_enable(enable);
-    setup_lcd_pins(enable); /* use GPIO pins when disable */
-}
-
-static void setup_lcdif(void)
-{
-    setup_parameters();
-    common_lcd_enable(true);
+    imx233_lcdif_enable(true);
+    imx233_lcdif_setup_system_pins(18);
     imx233_lcdif_enable_bus_master(true);
 }
 
@@ -120,13 +69,13 @@ static void lcd_write_reg(uint32_t reg, uint32_t data)
 {
     uint32_t old_reg = reg;
     /* get back to 18-bit word length */
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__18_BIT);
+    imx233_lcdif_set_word_length(18);
     reg = encode_16_to_18(reg);
     data = encode_16_to_18(data);
     
-    imx233_lcdif_pio_send(false, 2, &reg);
+    imx233_lcdif_pio_send(false, 1, &reg);
     if(old_reg != 0 && old_reg != 0x202)
-        imx233_lcdif_pio_send(true, 2, &data);
+        imx233_lcdif_pio_send(true, 1, &data);
 }
 
 #define REG_MDELAY  0xffffffff
@@ -208,11 +157,11 @@ void lcd_init_device(void)
     setup_lcdif_clock();
 
     // reset device
-    BF_SET(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(true);
     mdelay(50);
-    BF_CLR(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(false);
     mdelay(10);
-    BF_SET(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(true);
 
     lcd_init_seq();
 #ifdef HAVE_LCD_ENABLE
@@ -247,12 +196,8 @@ void lcd_enable(bool enable)
 
     lcd_on = enable;
     
-    if(enable)
-        common_lcd_enable(true);
     lcd_enable_seq(enable);
-    if(!enable)
-        common_lcd_enable(false);
-    else
+    if(enable)
         send_event(LCD_EVENT_ACTIVATION, NULL);
 }
 #endif
@@ -283,9 +228,8 @@ void lcd_update_rect(int x, int y, int w, int h)
     lcd_write_reg(0x201, 0);
     lcd_write_reg(0x202, 0);
     imx233_lcdif_wait_ready();
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__16_BIT);
+    imx233_lcdif_set_word_length(16);
     imx233_lcdif_set_byte_packing_format(0xf); /* two pixels per 32-bit word */
-    imx233_lcdif_set_data_format(false, false, false); /* RGB565, don't care, don't care */
 
     /* there are two cases here:
      * - either width = LCD_WIDTH and we can directly memcopy a part of lcd_framebuffer to FRAME
diff --git a/firmware/target/arm/imx233/creative-zenxfi3/lcd-zenxfi3.c b/firmware/target/arm/imx233/creative-zenxfi3/lcd-zenxfi3.c
index 7afdaa316e..59496f2d24 100644
--- a/firmware/target/arm/imx233/creative-zenxfi3/lcd-zenxfi3.c
+++ b/firmware/target/arm/imx233/creative-zenxfi3/lcd-zenxfi3.c
@@ -32,161 +32,76 @@
 #ifdef HAVE_LCD_ENABLE
 static bool lcd_on;
 #endif
-static unsigned lcd_yuv_options = 0;
 
-static void setup_parameters(void)
+static void lcd_write_reg(uint16_t reg, uint16_t data)
 {
-    imx233_lcdif_reset();
-    imx233_lcdif_set_lcd_databus_width(BV_LCDIF_CTRL_LCD_DATABUS_WIDTH__16_BIT);
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__16_BIT);
-    imx233_lcdif_set_timings(2, 2, 3, 3);
-    imx233_lcdif_enable_underflow_recover(true);
-}
-
-static void setup_lcd_pins(bool use_lcdif)
-{
-    /* WARNING
-     * the B1P22 pins is used to gate the speaker! Do NOT drive
-     * them as lcd_dotclk and lcd_hsync or it will break audio */
-    imx233_pinctrl_acquire(1, 18, "lcd reset");
-    imx233_pinctrl_acquire(1, 19, "lcd rs");
-    imx233_pinctrl_acquire(1, 20, "lcd wr");
-    imx233_pinctrl_acquire(1, 21, "lcd cs");
-    imx233_pinctrl_acquire(1, 23, "lcd enable");
-    imx233_pinctrl_acquire(1, 25, "lcd vsync");
-    //imx233_pinctrl_acquire_mask(1, 0x3ffff, "lcd data");
-    if(use_lcdif)
-    {
-        imx233_pinctrl_set_function(1, 25, PINCTRL_FUNCTION_MAIN); /* lcd_vsync */
-        imx233_pinctrl_set_function(1, 21, PINCTRL_FUNCTION_MAIN); /* lcd_cs */
-        imx233_pinctrl_set_function(1, 23, PINCTRL_FUNCTION_MAIN); /* lcd_enable */
-        imx233_pinctrl_set_function(1, 18, PINCTRL_FUNCTION_MAIN); /* lcd_reset */
-        imx233_pinctrl_set_function(1, 19, PINCTRL_FUNCTION_MAIN); /* lcd_rs */
-        imx233_pinctrl_set_function(1, 16, PINCTRL_FUNCTION_MAIN); /* lcd_d16 */
-        imx233_pinctrl_set_function(1, 17, PINCTRL_FUNCTION_MAIN); /* lcd_d17 */
-        imx233_pinctrl_set_function(1, 20, PINCTRL_FUNCTION_MAIN); /* lcd_wr */
-        HW_PINCTRL_MUXSELn_CLR(2) = 0xffffffff; /* lcd_d{0-15} */
-    }
-    else
-    {
-        HW_PINCTRL_MUXSELn_SET(2) = 0xffffffff; /* lcd_d{0-15} */
-        HW_PINCTRL_DOEn_CLR(1) = 0x2bfffff;
-        imx233_pinctrl_set_function(1, 16, PINCTRL_FUNCTION_GPIO); /* lcd_d16 */
-        imx233_pinctrl_set_function(1, 17, PINCTRL_FUNCTION_GPIO); /* lcd_d17 */
-        imx233_pinctrl_set_function(1, 19, PINCTRL_FUNCTION_GPIO); /* lcd_rs */
-        imx233_pinctrl_set_function(1, 20, PINCTRL_FUNCTION_GPIO); /* lcd_wr */
-        imx233_pinctrl_set_function(1, 21, PINCTRL_FUNCTION_GPIO); /* lcd_cs */
-        imx233_pinctrl_set_function(1, 23, PINCTRL_FUNCTION_GPIO); /* lcd_enable */
-        imx233_pinctrl_set_function(1, 25, PINCTRL_FUNCTION_GPIO); /* lcd_vsync */
-    }
-}
-
-static void common_lcd_enable(bool enable)
-{
-    imx233_lcdif_enable(enable);
-    setup_lcd_pins(enable); /* use GPIO pins when disable */
+    imx233_lcdif_pio_send(false, 1, &reg);
+    if(reg != 0x22)
+        imx233_lcdif_pio_send(true, 1, &data);
 }
 
-static void setup_lcdif(void)
+static void lcd_init_seq(void)
 {
-    setup_parameters();
-    common_lcd_enable(true);
-    imx233_lcdif_enable_bus_master(true);
+    lcd_write_reg(1, 0x11c);
+    lcd_write_reg(2, 0x100);
+    lcd_write_reg(3, 0x1030);
+    lcd_write_reg(8, 0x808);
+    lcd_write_reg(0xc, 0);
+    lcd_write_reg(0xf, 0xc01);
+    lcd_write_reg(0x20, 0);
+    lcd_write_reg(0x21, 0);
+    udelay(30);
+    lcd_write_reg(0x10, 0xa00);
+    lcd_write_reg(0x11, 0x1038);
+    udelay(30);
+    lcd_write_reg(0x12, 0x1010);
+    lcd_write_reg(0x13, 0x50);
+    lcd_write_reg(0x14, 0x4f58);
+    lcd_write_reg(0x30, 0);
+    lcd_write_reg(0x31, 0xdb);
+    lcd_write_reg(0x32, 0);
+    lcd_write_reg(0x33, 0);
+    lcd_write_reg(0x34, 0xdb);
+    lcd_write_reg(0x35, 0);
+    lcd_write_reg(0x36, 0xaf);
+    lcd_write_reg(0x37, 0);
+    lcd_write_reg(0x38, 0xdb);
+    lcd_write_reg(0x39, 0);
+    lcd_write_reg(0x50, 0);
+    lcd_write_reg(0x51, 0x705);
+    lcd_write_reg(0x52, 0xe0a);
+    lcd_write_reg(0x53, 0x300);
+    lcd_write_reg(0x54, 0xa0e);
+    lcd_write_reg(0x55, 0x507);
+    lcd_write_reg(0x56, 0);
+    lcd_write_reg(0x57, 3);
+    lcd_write_reg(0x58, 0x90a);
+    lcd_write_reg(0x59, 0xa09);
+    udelay(30);
+    lcd_write_reg(7, 0x1017);
+    udelay(40);
 }
 
-static void setup_lcdif_clock(void)
+void lcd_init_device(void)
 {
     /* the LCD seems to work at 24Mhz, so use the xtal clock with no divider */
     imx233_clkctrl_enable(CLK_PIX, false);
     imx233_clkctrl_set_div(CLK_PIX, 1);
     imx233_clkctrl_set_bypass(CLK_PIX, true); /* use XTAL */
     imx233_clkctrl_enable(CLK_PIX, true);
-}
-
-static void lcd_write_reg(uint32_t reg, uint32_t data)
-{
-    imx233_lcdif_pio_send(false, 2, &reg);
-    if(reg != 0x22)
-        imx233_lcdif_pio_send(true, 2, &data);
-}
-
-#define REG_UDELAY  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_UDELAY)
-            udelay(seq->data);
-        else
-            lcd_write_reg(seq->reg, seq->data);
-    }
-}
-
-#define _begin_seq() static struct lcd_sequence_entry_t __seq[] = {
-#define _udelay(a) {REG_UDELAY, 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(void)
-{
-    _begin_seq()
-    _lcd_write_reg(1, 0x11c)
-    _lcd_write_reg(2, 0x100)
-    _lcd_write_reg(3, 0x1030)
-    _lcd_write_reg(8, 0x808)
-    _lcd_write_reg(0xc, 0)
-    _lcd_write_reg(0xf, 0xc01)
-    _lcd_write_reg(0x20, 0)
-    _lcd_write_reg(0x21, 0)
-    _udelay(30)
-    _lcd_write_reg(0x10, 0xa00)
-    _lcd_write_reg(0x11, 0x1038)
-    _udelay(30)
-    _lcd_write_reg(0x12, 0x1010)
-    _lcd_write_reg(0x13, 0x50)
-    _lcd_write_reg(0x14, 0x4f58)
-    _lcd_write_reg(0x30, 0)
-    _lcd_write_reg(0x31, 0xdb)
-    _lcd_write_reg(0x32, 0)
-    _lcd_write_reg(0x33, 0)
-    _lcd_write_reg(0x34, 0xdb)
-    _lcd_write_reg(0x35, 0)
-    _lcd_write_reg(0x36, 0xaf)
-    _lcd_write_reg(0x37, 0)
-    _lcd_write_reg(0x38, 0xdb)
-    _lcd_write_reg(0x39, 0)
-    _lcd_write_reg(0x50, 0)
-    _lcd_write_reg(0x51, 0x705)
-    _lcd_write_reg(0x52, 0xe0a)
-    _lcd_write_reg(0x53, 0x300)
-    _lcd_write_reg(0x54, 0xa0e)
-    _lcd_write_reg(0x55, 0x507)
-    _lcd_write_reg(0x56, 0)
-    _lcd_write_reg(0x57, 3)
-    _lcd_write_reg(0x58, 0x90a)
-    _lcd_write_reg(0x59, 0xa09)
-    _udelay(30)
-    _lcd_write_reg(7, 0x1017)
-    _udelay(40)
-    _end_seq()
-}
-
-void lcd_init_device(void)
-{
-    setup_lcdif();
-    setup_lcdif_clock();
+    imx233_lcdif_init();
+    imx233_lcdif_set_lcd_databus_width(16);
+    imx233_lcdif_set_word_length(16);
+    imx233_lcdif_set_timings(2, 2, 3, 3);
+    imx233_lcdif_enable_underflow_recover(true);
+    imx233_lcdif_setup_system_pins(16);
 
     // reset device
-    BF_SET(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(true);
     mdelay(50);
-    BF_CLR(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(false);
     mdelay(10);
-    BF_SET(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(true);
 
     lcd_init_seq();
 #ifdef HAVE_LCD_ENABLE
@@ -204,13 +119,9 @@ static void lcd_enable_seq(bool enable)
 {
     if(!enable)
     {
-        _begin_seq()
-        _end_seq()
     }
     else
     {
-        _begin_seq()
-        _end_seq()
     }
 }
 
@@ -220,12 +131,12 @@ void lcd_enable(bool enable)
         return;
 
     lcd_on = enable;
-    
+
     if(enable)
-        common_lcd_enable(true);
+        imx233_lcdif_enable(true);
     lcd_enable_seq(enable);
     if(!enable)
-        common_lcd_enable(false);
+        imx233_lcdif_enable(false);
     else
         send_event(LCD_EVENT_ACTIVATION, NULL);
 }
@@ -258,9 +169,8 @@ void lcd_update_rect(int x, int y, int w, int h)
     lcd_write_reg(0x22, 0);
     
     imx233_lcdif_wait_ready();
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__16_BIT);
+    imx233_lcdif_set_word_length(16);
     imx233_lcdif_set_byte_packing_format(0xf); /* two pixels per 32-bit word */
-    imx233_lcdif_set_data_format(false, false, false); /* RGB565, don't care, don't care */
 
     /* there are two cases here:
      * - either width = LCD_WIDTH and we can directly memcopy a part of lcd_framebuffer to FRAME
@@ -306,194 +216,3 @@ void lcd_update_rect(int x, int y, int w, int h)
     imx233_lcdif_dma_send((void *)FRAME_PHYS_ADDR, w, h);
 }
 
-void lcd_yuv_set_options(unsigned options)
-{
-    lcd_yuv_options = options;
-}
-
-#define YFAC    (74)
-#define RVFAC   (101)
-#define GUFAC   (-24)
-#define GVFAC   (-51)
-#define BUFAC   (128)
-
-static inline int clamp(int val, int min, int max)
-{
-    if (val < min)
-        val = min;
-    else if (val > max)
-        val = max;
-    return val;
-}
-
-void lcd_blit_yuv(unsigned char * const src[3],
-                  int src_x, int src_y, int stride,
-                  int x, int y, int width, int height)
-{
-    const unsigned char *ysrc, *usrc, *vsrc;
-    int linecounter;
-    fb_data *dst, *row_end;
-    long z;
-
-    /* width and height must be >= 2 and an even number */
-    width &= ~1;
-    linecounter = height >> 1;
-
-    #if LCD_WIDTH >= LCD_HEIGHT
-    dst     = FBADDR(x,y);
-    row_end = dst + width;
-    #else
-    dst     = FBADDR(LCD_WIDTH - y - 1,x);
-    row_end = dst + LCD_WIDTH * width;
-    #endif
-
-    z    = stride * src_y;
-    ysrc = src[0] + z + src_x;
-    usrc = src[1] + (z >> 2) + (src_x >> 1);
-    vsrc = src[2] + (usrc - src[1]);
-
-    /* stride => amount to jump from end of last row to start of next */
-    stride -= width;
-
-    /* upsampling, YUV->RGB conversion and reduction to RGB565 in one go */
-
-    do
-    {
-        do
-        {
-            int y, cb, cr, rv, guv, bu, r, g, b;
-
-            y  = YFAC*(*ysrc++ - 16);
-            cb = *usrc++ - 128;
-            cr = *vsrc++ - 128;
-
-            rv  =            RVFAC*cr;
-            guv = GUFAC*cb + GVFAC*cr;
-            bu  = BUFAC*cb;
-
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-
-            y = YFAC*(*ysrc++ - 16);
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-        }
-        while (dst < row_end);
-
-        ysrc    += stride;
-        usrc    -= width >> 1;
-        vsrc    -= width >> 1;
-
-        #if LCD_WIDTH >= LCD_HEIGHT
-        row_end += LCD_WIDTH;
-        dst     += LCD_WIDTH - width;
-        #else
-        row_end -= 1;
-        dst     -= LCD_WIDTH*width + 1;
-        #endif
-
-        do
-        {
-            int y, cb, cr, rv, guv, bu, r, g, b;
-
-            y  = YFAC*(*ysrc++ - 16);
-            cb = *usrc++ - 128;
-            cr = *vsrc++ - 128;
-
-            rv  =            RVFAC*cr;
-            guv = GUFAC*cb + GVFAC*cr;
-            bu  = BUFAC*cb;
-
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-
-            y = YFAC*(*ysrc++ - 16);
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-        }
-        while (dst < row_end);
-
-        ysrc    += stride;
-        usrc    += stride >> 1;
-        vsrc    += stride >> 1;
-
-        #if LCD_WIDTH >= LCD_HEIGHT
-        row_end += LCD_WIDTH;
-        dst     += LCD_WIDTH - width;
-        #else
-        row_end -= 1;
-        dst     -= LCD_WIDTH*width + 1;
-        #endif
-    }
-    while (--linecounter > 0);
-
-    #if LCD_WIDTH >= LCD_HEIGHT
-    lcd_update_rect(x, y, width, height);
-    #else
-    lcd_update_rect(LCD_WIDTH - y - height, x, height, width);
-    #endif
-}
diff --git a/firmware/target/arm/imx233/lcdif-imx233.c b/firmware/target/arm/imx233/lcdif-imx233.c
index f947fa69f1..da58270498 100644
--- a/firmware/target/arm/imx233/lcdif-imx233.c
+++ b/firmware/target/arm/imx233/lcdif-imx233.c
@@ -19,36 +19,62 @@
  *
  ****************************************************************************/
 #include "lcdif-imx233.h"
+#include "pinctrl-imx233.h"
+#include "icoll-imx233.h"
 
-static unsigned lcdif_word_length = 0;
+#if IMX233_SUBTARGET >= 3700
+static lcdif_irq_cb_t g_cur_frame_cb = NULL;
+static lcdif_irq_cb_t g_vsync_edge_cb = NULL;
+#endif
 
-void imx233_lcdif_enable_underflow_recover(bool enable)
+/* for some crazy reason, all "non-dma" interrupts are routed to the ERROR irq */
+#if IMX233_SUBTARGET >= 3700
+void INT_LCDIF_ERROR(void)
 {
-    if(enable)
-        BF_SET(LCDIF_CTRL1, RECOVER_ON_UNDERFLOW);
-    else
-        BF_CLR(LCDIF_CTRL1, RECOVER_ON_UNDERFLOW);
+    if(BF_RD(LCDIF_CTRL1, CUR_FRAME_DONE_IRQ))
+    {
+        if(g_cur_frame_cb)
+            g_cur_frame_cb();
+        BF_CLR(LCDIF_CTRL1, CUR_FRAME_DONE_IRQ);
+    }
+    if(BF_RD(LCDIF_CTRL1, VSYNC_EDGE_IRQ))
+    {
+        if(g_vsync_edge_cb)
+            g_vsync_edge_cb();
+        BF_CLR(LCDIF_CTRL1, VSYNC_EDGE_IRQ);
+    }
 }
+#endif
 
-void imx233_lcdif_enable_bus_master(bool enable)
+void imx233_lcdif_enable(bool enable)
 {
     if(enable)
-        BF_SET(LCDIF_CTRL, LCDIF_MASTER);
+        BF_CLR(LCDIF_CTRL, CLKGATE);
     else
-        BF_CLR(LCDIF_CTRL, LCDIF_MASTER);
+        BF_SET(LCDIF_CTRL, CLKGATE);
 }
 
-void imx233_lcdif_enable(bool enable)
+void imx233_lcdif_reset_lcd(bool enable)
 {
+#if IMX233_SUBTARGET < 3700
     if(enable)
-        BF_CLR(LCDIF_CTRL, CLKGATE);
+        BF_SET(LCDIF_CTRL, RESET);
     else
-        BF_SET(LCDIF_CTRL, CLKGATE);
+        BF_CLR(LCDIF_CTRL, RESET);
+#else
+    if(enable)
+        BF_SET(LCDIF_CTRL1, RESET);
+    else
+        BF_CLR(LCDIF_CTRL1, RESET);
+#endif
 }
 
-void imx233_lcdif_reset(void)
+void imx233_lcdif_init(void)
 {
     imx233_reset_block(&HW_LCDIF_CTRL);
+#if IMX233_SUBTARGET >= 3700
+    imx233_icoll_enable_interrupt(INT_SRC_LCDIF_ERROR, true);
+#endif
 }
 
 void imx233_lcdif_set_timings(unsigned data_setup, unsigned data_hold,
@@ -58,90 +84,286 @@ void imx233_lcdif_set_timings(unsigned data_setup, unsigned data_hold,
         DATA_HOLD(data_hold), CMD_SETUP(cmd_setup), CMD_HOLD(cmd_hold));
 }
 
-void imx233_lcdif_set_lcd_databus_width(unsigned width)
+void imx233_lcdif_set_word_length(unsigned word_length)
 {
-    BF_WR(LCDIF_CTRL, LCD_DATABUS_WIDTH, width);
+    switch(word_length)
+    {
+        case 8: BF_WR_V(LCDIF_CTRL, WORD_LENGTH, 8_BIT); break;
+        case 16: BF_WR_V(LCDIF_CTRL, WORD_LENGTH, 16_BIT); break;
+#if IMX233_SUBTARGET >= 3780
+        case 18: BF_WR_V(LCDIF_CTRL, WORD_LENGTH, 18_BIT); break;
+        case 24: BF_WR_V(LCDIF_CTRL, WORD_LENGTH, 24_BIT); break;
+#endif
+        default:
+            panicf("this chip cannot handle a lcd word length of %d", word_length);
+            break;
+    }
 }
 
-void imx233_lcdif_set_word_length(unsigned word_length)
+void imx233_lcdif_wait_ready(void)
 {
-    BF_WR(LCDIF_CTRL, WORD_LENGTH, word_length);
-    lcdif_word_length = word_length;
+    while(BF_RD(LCDIF_CTRL, RUN));
 }
 
-void imx233_lcdif_set_byte_packing_format(unsigned byte_packing)
+void imx233_lcdif_set_data_swizzle(unsigned swizzle)
 {
-    BF_WR(LCDIF_CTRL1, BYTE_PACKING_FORMAT, byte_packing);
+#if IMX233_SUBTARGET >= 3780
+    BF_WR(LCDIF_CTRL, INPUT_DATA_SWIZZLE, swizzle);
+#else
+    BF_WR(LCDIF_CTRL, DATA_SWIZZLE, swizzle);
+#endif
 }
 
-void imx233_lcdif_set_data_format(bool data_fmt_16, bool data_fmt_18, bool data_fmt_24)
+void imx233_lcdif_wait_fifo(void)
 {
-    if(data_fmt_16)
-        BF_SET(LCDIF_CTRL, DATA_FORMAT_16_BIT);
-    else
-        BF_CLR(LCDIF_CTRL, DATA_FORMAT_16_BIT);
-    if(data_fmt_18)
-        BF_SET(LCDIF_CTRL, DATA_FORMAT_18_BIT);
-    else
-        BF_CLR(LCDIF_CTRL, DATA_FORMAT_18_BIT);
-    if(data_fmt_24)
-        BF_SET(LCDIF_CTRL, DATA_FORMAT_24_BIT);
-    else
-        BF_CLR(LCDIF_CTRL, DATA_FORMAT_24_BIT);
+#if IMX233_SUBTARGET >= 3700
+    while(BF_RD(LCDIF_STAT, TXFIFO_FULL));
+#else
+    while(!BF_RD(LCDIF_CTRL, FIFO_STATUS));
+#endif
 }
 
-void imx233_lcdif_wait_ready(void)
+/* The following function set byte packing often, ifdefing everytime is painful */
+#if IMX233_SUBTARGET < 3700
+#define imx233_lcdif_set_byte_packing_format(a) 
+#endif
+
+// bbp = bytes per pixel
+static void pio_send(unsigned len, unsigned bpp, uint8_t *buf)
 {
-    while(BF_RD(LCDIF_CTRL, RUN));
+    /* WARNING: the imx233 has a limitation on count wrt to byte packing, the
+     * count must be a multiple of 2 with maximum packing when word-length is
+     * 16-bit!
+     * On the other hand, 8-bit word length doesn't seem to have any limitations,
+     * for example one can send 3 bytes with a packing format of 0xf
+     * WARNING for this function to work properly with any swizzle, we have to
+     * make sure we pack as many 32-bits as possible even when the data is not
+     * word-aligned */
+    imx233_lcdif_set_byte_packing_format(0xf);
+    /* compute shift between buf and next word-aligned pointer */
+    int shift = 0;
+    uint32_t temp_buf = 0;
+    int count = len * bpp; // number of bytes
+    while(0x3 & (intptr_t)buf)
+    {
+        temp_buf = temp_buf | *buf++ << shift;
+        shift += 8;
+        count--;
+    }
+    /* starting from now, all read are 32-bit */
+    uint32_t *wbuf = (void *)buf;
+#if IMX233_SUBTARGET >= 3780
+    HW_LCDIF_TRANSFER_COUNT = BF_OR2(LCDIF_TRANSFER_COUNT, V_COUNT(1), H_COUNT(len));
+#else
+    BF_WR(LCDIF_CTRL, COUNT, len);
+#endif
+    BF_SET(LCDIF_CTRL, RUN);
+    while(count > 0)
+    {
+        uint32_t val = *wbuf++;
+        imx233_lcdif_wait_fifo();
+        HW_LCDIF_DATA = temp_buf | val << shift;
+        if(shift != 0)
+            temp_buf = val >> (32 - shift);
+        count -= 4;
+    }
+    /* send remaining bytes if any */
+    if(shift != 0)
+    {
+        imx233_lcdif_wait_fifo();
+        HW_LCDIF_DATA = temp_buf;
+    }
+    imx233_lcdif_wait_ready();
 }
 
-void imx233_lcdif_pio_send(bool data_mode, unsigned len, uint32_t *buf)
+void imx233_lcdif_pio_send(bool data_mode, unsigned len, void *buf)
 {
-    unsigned max_xfer_size = 0xffff;
-    if(len == 0)
-        return;
-    if(lcdif_word_length == BV_LCDIF_CTRL_WORD_LENGTH__16_BIT)
-        max_xfer_size = 0x1fffe;
     imx233_lcdif_wait_ready();
+#if IMX233_SUBTARGET >= 3780
     imx233_lcdif_enable_bus_master(false);
+#endif
+    if(data_mode)
+        BF_SET(LCDIF_CTRL, DATA_SELECT);
+    else
+        BF_CLR(LCDIF_CTRL, DATA_SELECT);
 
-    do
+    switch(BF_RD(LCDIF_CTRL, WORD_LENGTH))
     {
-        unsigned burst = MIN(len, max_xfer_size);
-        len -= burst;
-        unsigned count = burst;
-        if(lcdif_word_length != BV_LCDIF_CTRL_WORD_LENGTH__8_BIT)
-        {
-            if(burst & 1)
-                burst++;
-            count = burst / 2;
-        }
-        else
-            count = burst;
-        HW_LCDIF_TRANSFER_COUNT = 0;
-        HW_LCDIF_TRANSFER_COUNT = 0x10000 | count;
-        BF_CLR(LCDIF_CTRL, DATA_SELECT);
-        BF_CLR(LCDIF_CTRL, RUN);
-        if(data_mode)
-            BF_SET(LCDIF_CTRL, DATA_SELECT);
-        BF_SET(LCDIF_CTRL, RUN);
-        burst = (burst + 3) / 4;
-        while(burst-- > 0)
-        {
-            while(BF_RD(LCDIF_STAT, LFIFO_FULL));
-            HW_LCDIF_DATA = *buf++;
-        }
-        imx233_lcdif_wait_ready();
-    }while(len > 0);
-    imx233_lcdif_enable_bus_master(true);
+        case BV_LCDIF_CTRL_WORD_LENGTH__8_BIT: pio_send(len, 1, buf); break;
+        case BV_LCDIF_CTRL_WORD_LENGTH__16_BIT: pio_send(len, 2, buf); break;
+#if IMX233_SUBTARGET >= 3780
+        case BV_LCDIF_CTRL_WORD_LENGTH__18_BIT: pio_send(len, 4, buf); break;
+#endif
+        default: panicf("Don't know how to handle this word length");
+    }
 }
 
 void imx233_lcdif_dma_send(void *buf, unsigned width, unsigned height)
 {
+#if IMX233_SUBTARGET >= 3780
+    imx233_lcdif_enable_bus_master(true);
     HW_LCDIF_CUR_BUF = (uint32_t)buf;
-    HW_LCDIF_TRANSFER_COUNT = 0;
-    HW_LCDIF_TRANSFER_COUNT = (height << 16) | width;
-    BF_CLR(LCDIF_CTRL, RUN);
+    HW_LCDIF_TRANSFER_COUNT = BF_OR2(LCDIF_TRANSFER_COUNT, V_COUNT(height), H_COUNT(width));
     BF_SET(LCDIF_CTRL, DATA_SELECT);
     BF_SET(LCDIF_CTRL, RUN);
+#endif
+}
+
+static void setup_data_pins(unsigned bus_width)
+{
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D0, "lcd d0", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D1, "lcd d1", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D2, "lcd d2", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D3, "lcd d3", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D4, "lcd d4", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D5, "lcd d5", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D6, "lcd d6", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_D7, "lcd d7", PINCTRL_DRIVE_4mA, false);
+    if(bus_width >= 16)
+    {
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D8, "lcd d8", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D9, "lcd d9", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D10, "lcd d10", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D11, "lcd d11", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D12, "lcd d12", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D13, "lcd d13", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D14, "lcd d14", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D15, "lcd d15", PINCTRL_DRIVE_4mA, false);
+    }
+#if IMX233_SUBTARGET >= 3780
+    if(bus_width >= 18)
+    {
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D16, "lcd d16", PINCTRL_DRIVE_4mA, false);
+        imx233_pinctrl_setup_vpin(VPIN_LCD_D17, "lcd d17", PINCTRL_DRIVE_4mA, false);
+    }
+#endif
+}
+
+void imx233_lcdif_setup_system_pins(unsigned bus_width)
+{
+    imx233_pinctrl_setup_vpin(VPIN_LCD_RESET, "lcd reset", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_RS, "lcd rs", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_WR, "lcd wr", PINCTRL_DRIVE_4mA, false);
+#ifdef VPIN_LCD_RD
+    imx233_pinctrl_setup_vpin(VPIN_LCD_RD, "lcd rd", PINCTRL_DRIVE_4mA, false);
+#endif
+    imx233_pinctrl_setup_vpin(VPIN_LCD_CS, "lcd cs", PINCTRL_DRIVE_4mA, false);
+
+    setup_data_pins(bus_width);
+}
+
+#if IMX233_SUBTARGET >= 3700
+void imx233_lcdif_setup_dotclk_pins(unsigned bus_width, bool have_enable)
+{
+    if(have_enable)
+        imx233_pinctrl_setup_vpin(VPIN_LCD_ENABLE, "lcd enable", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_RESET, "lcd reset", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_HSYNC, "lcd hsync", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_VSYNC, "lcd vsync", PINCTRL_DRIVE_4mA, false);
+    imx233_pinctrl_setup_vpin(VPIN_LCD_DOTCLK, "lcd dotclk", PINCTRL_DRIVE_4mA, false);
+
+    setup_data_pins(bus_width);
+}
+
+void imx233_lcdif_set_byte_packing_format(unsigned byte_packing)
+{
+    BF_WR(LCDIF_CTRL1, BYTE_PACKING_FORMAT, byte_packing);
+}
+#endif
+
+#if IMX233_SUBTARGET >= 3700 && IMX233_SUBTARGET < 3780
+void imx233_lcdif_enable_sync_signals(bool en)
+{
+    BF_WR(LCDIF_VDCTRL3, SYNC_SIGNALS_ON, en);
+}
+
+void imx233_lcdif_setup_dotclk(unsigned v_pulse_width, unsigned v_period,
+    unsigned v_wait_cnt, unsigned v_active, unsigned h_pulse_width,
+    unsigned h_period, unsigned h_wait_cnt, unsigned h_active, bool enable_present)
+{
+    HW_LCDIF_VDCTRL0 = BF_OR4(LCDIF_VDCTRL0, ENABLE_PRESENT(enable_present),
+        VSYNC_PERIOD_UNIT(1), VSYNC_PULSE_WIDTH_UNIT(1),
+         DOTCLK_V_VALID_DATA_CNT(v_active));
+    HW_LCDIF_VDCTRL1 = BF_OR2(LCDIF_VDCTRL1, VSYNC_PERIOD(v_period),
+        VSYNC_PULSE_WIDTH(v_pulse_width));
+    HW_LCDIF_VDCTRL2 = BF_OR3(LCDIF_VDCTRL2, HSYNC_PULSE_WIDTH(h_pulse_width),
+        HSYNC_PERIOD(h_period), DOTCLK_H_VALID_DATA_CNT(h_active));
+    HW_LCDIF_VDCTRL3 = BF_OR2(LCDIF_VDCTRL3, VERTICAL_WAIT_CNT(v_wait_cnt),
+        HORIZONTAL_WAIT_CNT(h_wait_cnt));
+    // setup dotclk mode, always bypass count, apparently data select is needed
+    HW_LCDIF_CTRL_SET = BM_OR3(LCDIF_CTRL, DOTCLK_MODE, BYPASS_COUNT, DATA_SELECT);
+}
+
+void imx233_lcdif_setup_dotclk_ex(unsigned v_pulse_width, unsigned v_back_porch,
+    unsigned v_front_porch, unsigned h_pulse_width, unsigned h_back_porch,
+    unsigned h_front_porch, unsigned width, unsigned height, unsigned clk_per_pix,
+    bool enable_present)
+{
+    unsigned h_active = clk_per_pix * width;
+    unsigned h_period = h_active + h_back_porch + h_front_porch;
+    unsigned v_active = height;
+    unsigned v_period = v_active + v_back_porch + v_front_porch;
+    imx233_lcdif_setup_dotclk(v_pulse_width, v_period, v_back_porch, v_active,
+        h_pulse_width, h_period, h_back_porch, h_active, enable_present);
+}
+
+void imx233_lcdif_enable_frame_done_irq(bool en)
+{
+    if(en)
+        BF_SET(LCDIF_CTRL1, CUR_FRAME_DONE_IRQ_EN);
+    else
+        BF_CLR(LCDIF_CTRL1, CUR_FRAME_DONE_IRQ_EN);
+    BF_CLR(LCDIF_CTRL1, CUR_FRAME_DONE_IRQ);
+}
+
+void imx233_lcdif_set_frame_done_cb(lcdif_irq_cb_t cb)
+{
+    g_cur_frame_cb = cb;
+}
+
+void imx233_lcdif_enable_vsync_edge_irq(bool en)
+{
+    if(en)
+        BF_SET(LCDIF_CTRL1, VSYNC_EDGE_IRQ_EN);
+    else
+        BF_CLR(LCDIF_CTRL1, VSYNC_EDGE_IRQ_EN);
+    BF_CLR(LCDIF_CTRL1, VSYNC_EDGE_IRQ);
+}
+
+void imx233_lcdif_set_vsync_edge_cb(lcdif_irq_cb_t cb)
+{
+    g_vsync_edge_cb = cb;
+}
+#endif
+
+#if IMX233_SUBTARGET >= 3780
+void imx233_lcdif_set_lcd_databus_width(unsigned width)
+{
+    switch(width)
+    {
+        case 8: BF_WR_V(LCDIF_CTRL, LCD_DATABUS_WIDTH, 8_BIT); break;
+        case 16: BF_WR_V(LCDIF_CTRL, LCD_DATABUS_WIDTH, 16_BIT); break;
+        case 18: BF_WR_V(LCDIF_CTRL, LCD_DATABUS_WIDTH, 18_BIT); break;
+        case 24: BF_WR_V(LCDIF_CTRL, LCD_DATABUS_WIDTH, 24_BIT); break;
+        default:
+            panicf("this chip cannot handle a lcd bus width of %d", width);
+            break;
+    }
+}
+
+void imx233_lcdif_enable_underflow_recover(bool enable)
+{
+    if(enable)
+        BF_SET(LCDIF_CTRL1, RECOVER_ON_UNDERFLOW);
+    else
+        BF_CLR(LCDIF_CTRL1, RECOVER_ON_UNDERFLOW);
+}
+
+void imx233_lcdif_enable_bus_master(bool enable)
+{
+    if(enable)
+        BF_SET(LCDIF_CTRL, LCDIF_MASTER);
+    else
+        BF_CLR(LCDIF_CTRL, LCDIF_MASTER);
 }
+#endif
\ No newline at end of file
diff --git a/firmware/target/arm/imx233/lcdif-imx233.h b/firmware/target/arm/imx233/lcdif-imx233.h
index 1aee72bce4..dabc1f4c71 100644
--- a/firmware/target/arm/imx233/lcdif-imx233.h
+++ b/firmware/target/arm/imx233/lcdif-imx233.h
@@ -28,19 +28,49 @@
 
 #include "regs/regs-lcdif.h"
 
-void imx233_lcdif_enable_underflow_recover(bool enable);
-void imx233_lcdif_enable_bus_master(bool enable);
+typedef void (*lcdif_irq_cb_t)(void);
+
 void imx233_lcdif_enable(bool enable);
-void imx233_lcdif_reset(void);// reset lcdif block
+void imx233_lcdif_init(void);// reset lcdif block
+void imx233_lcdif_reset_lcd(bool enable);// set/clr reset line
 void imx233_lcdif_set_timings(unsigned data_setup, unsigned data_hold,
     unsigned cmd_setup, unsigned cmd_hold);
-void imx233_lcdif_set_lcd_databus_width(unsigned width);
 void imx233_lcdif_set_word_length(unsigned word_length);
-void imx233_lcdif_set_byte_packing_format(unsigned byte_packing);
-void imx233_lcdif_set_data_format(bool data_fmt_16, bool data_fmt_18, bool data_fmt_24);
-unsigned imx233_lcdif_enable_irqs(unsigned irq_bm); /* return old mask */
 void imx233_lcdif_wait_ready(void);
-void imx233_lcdif_pio_send(bool data_mode, unsigned len, uint32_t *buf);
+void imx233_lcdif_set_data_swizzle(unsigned swizzle);
+/* This function assumes the data is packed in 8/16-bit mode and unpacked in
+ * 18-bit mode.
+ * WARNING: doesn't support 24-bit mode
+ * WARNING: count must be lower than or equal to 0xffff
+ * Note that this function might affect the byte packing format and bus master.
+ * Note that count is number of pixels, NOT the number of bytes ! */
+void imx233_lcdif_pio_send(bool data_mode, unsigned count, void *buf);
 void imx233_lcdif_dma_send(void *buf, unsigned width, unsigned height);
+void imx233_lcdif_setup_system_pins(unsigned bus_width);
+void imx233_lcdif_setup_dotclk_pins(unsigned bus_width, bool have_enable);
+
+#if IMX233_SUBTARGET >= 3700
+void imx233_lcdif_set_byte_packing_format(unsigned byte_packing);
+/* low-level function */
+void imx233_lcdif_setup_dotclk(unsigned v_pulse_width, unsigned v_period,
+    unsigned v_wait_cnt, unsigned v_active, unsigned h_pulse_width,
+    unsigned h_period, unsigned h_wait_cnt, unsigned h_active, bool enable_present);
+/* high-level function */
+void imx233_lcdif_setup_dotclk_ex(unsigned v_pulse_width, unsigned v_back_porch,
+    unsigned v_front_porch, unsigned h_pulse_width, unsigned h_back_porch,
+    unsigned h_front_porch, unsigned width, unsigned height, unsigned clk_per_pix,
+    bool enable_present);
+void imx233_lcdif_enable_frame_done_irq(bool en);
+void imx233_lcdif_set_frame_done_cb(lcdif_irq_cb_t cb);
+void imx233_lcdif_enable_vsync_edge_irq(bool en);
+void imx233_lcdif_set_vsync_edge_cb(lcdif_irq_cb_t cb);
+void imx233_lcdif_enable_sync_signals(bool en);
+#endif
+
+#if IMX233_SUBTARGET >= 3780
+void imx233_lcdif_enable_underflow_recover(bool enable);
+void imx233_lcdif_enable_bus_master(bool enable);
+void imx233_lcdif_set_lcd_databus_width(unsigned width);
+#endif
 
 #endif /* __LCDIF_IMX233_H__ */
diff --git a/firmware/target/arm/imx233/pins/pins-imx233.h b/firmware/target/arm/imx233/pins/pins-imx233.h
index 12231a60d8..7a97ea7f11 100644
--- a/firmware/target/arm/imx233/pins/pins-imx233.h
+++ b/firmware/target/arm/imx233/pins/pins-imx233.h
@@ -79,5 +79,9 @@
 #define VPIN_LCD_RS         VPIN_PACK(1, 19, MAIN)
 #define VPIN_LCD_WR         VPIN_PACK(1, 20, MAIN)
 #define VPIN_LCD_CS         VPIN_PACK(1, 21, MAIN)
+#define VPIN_LCD_DOTCLK     VPIN_PACK(1, 22, MAIN)
+#define VPIN_LCD_ENABLE     VPIN_PACK(1, 23, MAIN)
+#define VPIN_LCD_HSYNC      VPIN_PACK(1, 24, MAIN)
+#define VPIN_LCD_VSYNC      VPIN_PACK(1, 25, MAIN)
 
 #endif /* __PINS_IMX233__ */
diff --git a/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c b/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
index 3fd63de623..79a4151682 100644
--- a/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
+++ b/firmware/target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
@@ -38,7 +38,6 @@
 #ifdef HAVE_LCD_ENABLE
 static bool lcd_on;
 #endif
-static unsigned lcd_yuv_options = 0;
 static int lcd_dcp_channel = -1;
 #ifdef HAVE_LCD_INVERT
 static int lcd_reg_0x61_val = 1; /* used to invert display */
@@ -55,9 +54,9 @@ static enum lcd_kind_t
 
 static void setup_parameters(void)
 {
-    imx233_lcdif_reset();
-    imx233_lcdif_set_lcd_databus_width(BV_LCDIF_CTRL_LCD_DATABUS_WIDTH__18_BIT);
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__18_BIT);
+    imx233_lcdif_init();
+    imx233_lcdif_set_lcd_databus_width(18);
+    imx233_lcdif_set_word_length(18);
     imx233_lcdif_set_timings(1, 2, 2, 2);
     imx233_lcdif_enable_underflow_recover(true);
 }
@@ -126,7 +125,7 @@ static void setup_lcd_pins_i80(bool i80)
     else
     {
         HW_PINCTRL_DOUTn_SET(1) = (1 << 19) | (1 << 20) | (1 << 21) | (1 << 23);
-
+        
         imx233_pinctrl_set_drive(1, 19, PINCTRL_DRIVE_4mA); /* lcd_rs */
         imx233_pinctrl_set_drive(1, 20, PINCTRL_DRIVE_4mA); /* lcd_wr */
         imx233_pinctrl_set_drive(1, 21, PINCTRL_DRIVE_4mA); /* lcd_cs */
@@ -219,13 +218,13 @@ static void lcd_write_reg(uint32_t reg, uint32_t data)
     uint32_t old_reg = reg;
     imx233_lcdif_wait_ready();
     /* get back to 18-bit word length */
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__18_BIT);
+    imx233_lcdif_set_word_length(18);
     reg = encode_16_to_18(reg);
     data = encode_16_to_18(data);
     
-    imx233_lcdif_pio_send(false, 2, &reg);
+    imx233_lcdif_pio_send(false, 1, &reg);
     if(old_reg != 0x22)
-        imx233_lcdif_pio_send(true, 2, &data);
+        imx233_lcdif_pio_send(true, 1, &data);
 }
 
 static uint32_t lcd_read_reg(uint32_t reg)
@@ -399,11 +398,11 @@ void lcd_init_device(void)
             break;
     }
     // reset device
-    BF_SET(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(true);
     mdelay(50);
-    BF_CLR(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(false);
     mdelay(10);
-    BF_SET(LCDIF_CTRL1, RESET);
+    imx233_lcdif_reset_lcd(true);
 
     switch(lcd_kind)
     {
@@ -579,9 +578,8 @@ void lcd_update_rect(int x, int y, int w, int h)
     lcd_write_reg(0x21, y);
     lcd_write_reg(0x22, 0);
     imx233_lcdif_wait_ready();
-    imx233_lcdif_set_word_length(BV_LCDIF_CTRL_WORD_LENGTH__16_BIT);
+    imx233_lcdif_set_word_length(16);
     imx233_lcdif_set_byte_packing_format(0xf); /* two pixels per 32-bit word */
-    imx233_lcdif_set_data_format(false, false, false); /* RGB565, don't care, don't care */
     /* there are two cases here:
      * - either width = LCD_WIDTH and we can directly memcopy a part of lcd_framebuffer to FRAME
      *   and send it
@@ -626,198 +624,6 @@ void lcd_update_rect(int x, int y, int w, int h)
     imx233_lcdif_dma_send((void *)FRAME_PHYS_ADDR, w, h);
 }
 
-void lcd_yuv_set_options(unsigned options)
-{
-    lcd_yuv_options = options;
-}
-
-#define YFAC    (74)
-#define RVFAC   (101)
-#define GUFAC   (-24)
-#define GVFAC   (-51)
-#define BUFAC   (128)
-
-static inline int clamp(int val, int min, int max)
-{
-    if (val < min)
-        val = min;
-    else if (val > max)
-        val = max;
-    return val;
-}
-
-void lcd_blit_yuv(unsigned char * const src[3],
-                  int src_x, int src_y, int stride,
-                  int x, int y, int width, int height)
-{
-    const unsigned char *ysrc, *usrc, *vsrc;
-    int linecounter;
-    fb_data *dst, *row_end;
-    long z;
-    
-    /* width and height must be >= 2 and an even number */
-    width &= ~1;
-    linecounter = height >> 1;
-    
-    #if LCD_WIDTH >= LCD_HEIGHT
-    dst     = FBADDR(x,y);
-    row_end = dst + width;
-    #else
-    dst     = FBADDR(LCD_WIDTH - y - 1,x);
-    row_end = dst + LCD_WIDTH * width;
-    #endif
-    
-    z    = stride * src_y;
-    ysrc = src[0] + z + src_x;
-    usrc = src[1] + (z >> 2) + (src_x >> 1);
-    vsrc = src[2] + (usrc - src[1]);
-    
-    /* stride => amount to jump from end of last row to start of next */
-    stride -= width;
-    
-    /* upsampling, YUV->RGB conversion and reduction to RGB565 in one go */
-    
-    do
-    {
-        do
-        {
-            int y, cb, cr, rv, guv, bu, r, g, b;
-            
-            y  = YFAC*(*ysrc++ - 16);
-            cb = *usrc++ - 128;
-            cr = *vsrc++ - 128;
-            
-            rv  =            RVFAC*cr;
-            guv = GUFAC*cb + GVFAC*cr;
-            bu  = BUFAC*cb;
-            
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-            
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-            
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-            
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-            
-            y = YFAC*(*ysrc++ - 16);
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-            
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-            
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-            
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-        }
-        while (dst < row_end);
-        
-        ysrc    += stride;
-        usrc    -= width >> 1;
-        vsrc    -= width >> 1;
-        
-        #if LCD_WIDTH >= LCD_HEIGHT
-        row_end += LCD_WIDTH;
-        dst     += LCD_WIDTH - width;
-        #else
-        row_end -= 1;
-        dst     -= LCD_WIDTH*width + 1;
-        #endif
-        
-        do
-        {
-            int y, cb, cr, rv, guv, bu, r, g, b;
-            
-            y  = YFAC*(*ysrc++ - 16);
-            cb = *usrc++ - 128;
-            cr = *vsrc++ - 128;
-            
-            rv  =            RVFAC*cr;
-            guv = GUFAC*cb + GVFAC*cr;
-            bu  = BUFAC*cb;
-            
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-            
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-            
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-            
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-            
-            y = YFAC*(*ysrc++ - 16);
-            r = y + rv;
-            g = y + guv;
-            b = y + bu;
-            
-            if ((unsigned)(r | g | b) > 64*256-1)
-            {
-                r = clamp(r, 0, 64*256-1);
-                g = clamp(g, 0, 64*256-1);
-                b = clamp(b, 0, 64*256-1);
-            }
-            
-            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
-            
-            #if LCD_WIDTH >= LCD_HEIGHT
-            dst++;
-            #else
-            dst += LCD_WIDTH;
-            #endif
-        }
-        while (dst < row_end);
-        
-        ysrc    += stride;
-        usrc    += stride >> 1;
-        vsrc    += stride >> 1;
-        
-        #if LCD_WIDTH >= LCD_HEIGHT
-        row_end += LCD_WIDTH;
-        dst     += LCD_WIDTH - width;
-        #else
-        row_end -= 1;
-        dst     -= LCD_WIDTH*width + 1;
-        #endif
-    }
-    while (--linecounter > 0);
-    
-    #if LCD_WIDTH >= LCD_HEIGHT
-    lcd_update_rect(x, y, width, height);
-    #else
-    lcd_update_rect(LCD_WIDTH - y - height, x, height, width);
-    #endif
-}
-
 #ifndef BOOTLOADER
 bool lcd_debug_screen(void)
 {