diff --git a/drivers/video/davincifb.c b/drivers/video/davincifb.c
index 2a7b2c9..24d1424 100644
--- a/drivers/video/davincifb.c
+++ b/drivers/video/davincifb.c
@@ -29,12 +29,15 @@
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 
+#include <media/davinci/vpss.h>
 #include <asm/irq.h>
 #include <asm/uaccess.h>
 
 #include <video/davincifb.h>
 #include <asm/system.h>
 
+#include <mach/dm365.h>
+#include <mach/cputype.h>
 #define MODULE_NAME "davincifb"
 
 /* Output Format Selection  */
@@ -286,6 +289,10 @@ static irqreturn_t davincifb_isr(int irq, void *arg)
 					 dm->vid1->info.fix.line_length);
 			dm->vid1->sdram_address = 0;
 		}
+		if (dm->osd0->info.var.vmode == FB_VMODE_NONINTERLACED) {
+			++dm->vsync_cnt;
+			wake_up_interruptible(&dm->vsync_wait);
+		}
 		return IRQ_HANDLED;
 	} else {
 		++dm->vsync_cnt;
@@ -631,21 +638,29 @@ static void set_sdram_params(char *id, u32 addr, u32 line_length)
 	/* The parameters to be written to the registers should be in
 	 * multiple of 32 bytes
 	 */
-	addr = addr;		/* div by 32 */
-	line_length = line_length / 32;
+	addr = (addr - DAVINCI_DDR_BASE) >> 5;		/* div by 32 */
+	line_length = line_length >> 5;
 
 	if (is_win(id, VID0)) {
-		dispc_reg_out(OSD_VIDWIN0ADR, addr);
-		dispc_reg_out(OSD_VIDWIN0OFST, line_length);
+		dispc_reg_out(OSD_VIDWIN0ADR, addr & 0xFFFF);
+		dispc_reg_merge(OSD_VIDWINADH, (addr & 0x7F0000) >> 16, 0x7F);
+		dispc_reg_out(OSD_VIDWIN0OFST, line_length | 0x1000);
+			/* From docs it's not clear why bit12 is needed */
 	} else if (is_win(id, VID1)) {
-		dispc_reg_out(OSD_VIDWIN1ADR, addr);
-		dispc_reg_out(OSD_VIDWIN1OFST, line_length);
+		dispc_reg_out(OSD_VIDWIN1ADR, addr & 0xFFFF);
+		dispc_reg_merge(OSD_VIDWINADH, (addr & 0x7F0000) >> 8, 0x7F00);
+		dispc_reg_out(OSD_VIDWIN1OFST, line_length | 0x1000);
+			/* From docs it's not clear why bit12 is needed */
 	} else if (is_win(id, OSD0)) {
-		dispc_reg_out(OSD_OSDWIN0ADR, addr);
-		dispc_reg_out(OSD_OSDWIN0OFST, line_length);
+		dispc_reg_out(OSD_OSDWIN0ADR, addr & 0xFFFF);
+		dispc_reg_out(OSD_OSDWIN0OFST, line_length | 0x1000);
+		dispc_reg_merge(OSD_OSDWINADH, (addr & 0x7F0000) >> 16, 0x7F);
+			/* From docs it's not clear why bit12 is needed */
 	} else if (is_win(id, OSD1)) {
-		dispc_reg_out(OSD_OSDWIN1ADR, addr);
-		dispc_reg_out(OSD_OSDWIN1OFST, line_length);
+		dispc_reg_out(OSD_OSDWIN1ADR, addr & 0xFFFF);
+		dispc_reg_merge(OSD_OSDWINADH, (addr & 0x7F0000) >> 8, 0x7F00);
+		dispc_reg_out(OSD_OSDWIN1OFST, line_length | 0x1000);
+			/* From docs it's not clear why bit12 is needed */
 	}
 }
 
@@ -980,14 +995,12 @@ int __init davincifb_setup(char *options)
 		if (!strncmp(this_opt, "output=", 7)) {
 			if (!strncmp(this_opt + 7, "lcd", 3)) {
 				dmparams.output = LCD;
-				dmparams.format = 0;
+				dmparams.format = RGB;
 			} else if (!strncmp(this_opt + 7, "ntsc", 4))
 				dmparams.output = NTSC;
 			else if (!strncmp(this_opt + 7, "pal", 3))
 				dmparams.output = PAL;
 		} else if (!strncmp(this_opt, "format=", 7)) {
-			if (dmparams.output == LCD)
-				continue;
 			if (!strncmp(this_opt + 7, "composite", 9))
 				dmparams.format = COMPOSITE;
 			else if (!strncmp(this_opt + 7, "s-video", 7))
@@ -1060,7 +1073,9 @@ int __init davincifb_setup(char *options)
 		format_yres = 480;
 	} else if (dmparams.output == PAL) {
 		format_yres = 576;
-	} else {
+	} else if (dmparams.output == LCD)
+		format_yres = dmparams.osd0_yres;
+	else {
 		printk(KERN_INFO
 		       "DaVinci:invalid format..defaulting width to 480\n");
 	}
@@ -1180,7 +1195,10 @@ static struct fb_info *init_fb_info(struct dm_win_info *w,
 	struct dm_info *dm = w->dm;
 
 	/* initialize the fb_info structure */
-	info->flags = FBINFO_DEFAULT;
+	info->flags = FBINFO_DEFAULT |
+			FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT |
+			FBINFO_HWACCEL_IMAGEBLIT | FBINFO_HWACCEL_XPAN |
+			FBINFO_HWACCEL_YPAN;
 	info->fbops = &davincifb_ops;
 	info->screen_base = (char *)(w->fb_base);
 	info->pseudo_palette = w->pseudo_palette;
@@ -1388,6 +1406,123 @@ static void davincifb_pal_component_config(int on)
 	}
 }
 
+int dm365_set_pixelclock(int pixclock)
+{
+	u32 pllfreq;
+	int ret;
+	struct clk *clk6;
+
+	pixclock /= 1000;
+	pllfreq = 1000000000 / pixclock;
+
+	clk6 = clk_get(dm->dev, "pll1_sysclk6");
+	if (clk_set_rate(clk6, pllfreq)) {
+		dispc_reg_out(VENC_DCLKCTL, 0x03);
+		dispc_reg_out(VENC_DCLKPTN0, 0x03);
+		dispc_reg_out(VENC_DCLKPTN1, 0x0);
+		dispc_reg_out(VENC_DCLKPTN2, 0x0);
+		dispc_reg_out(VENC_DCLKPTN3, 0x0);
+		ret = 4;
+		clk_set_rate(clk6, pllfreq * 4);
+	} else {
+		dispc_reg_out(VENC_DCLKCTL, 0x801);
+		dispc_reg_out(VENC_DCLKPTN0, 0x03);
+		dispc_reg_out(VENC_DCLKPTN1, 0x0);
+		dispc_reg_out(VENC_DCLKPTN2, 0x0);
+		dispc_reg_out(VENC_DCLKPTN3, 0x0);
+		ret = 1;
+	}
+	pllfreq = clk_get_rate(clk6);
+	return ret;
+}
+
+static void davincifb_lcd_rgb_config(int on)
+{
+	u32 divisor = 1;
+
+	if (on) {
+		/* Reset video encoder module */
+		dispc_reg_out(VENC_VMOD, 0x0);
+		dispc_reg_out(VENC_VIDCTL, 0x0);
+
+		if (cpu_is_davinci_dm365()) {
+			divisor = \
+			dm365_set_pixelclock(dm->osd0->info.var.pixclock);
+		}
+
+		/* set hsync pulse width */
+		dispc_reg_out(VENC_HSPLS, (dm->osd0->info.var.hsync_len) * \
+			divisor);
+		/* set vsync pulse width */
+		dispc_reg_out(VENC_VSPLS, dm->osd0->info.var.vsync_len);
+		/* set horizontal interval  */
+		dispc_reg_out(VENC_HINT, ((dm->osd0->info.var.left_margin + \
+		dm->osd0->info.var.right_margin + dm->osd0->info.var.width) * \
+			divisor)  - 1);
+		/* set horizontal data valid start position  */
+		dispc_reg_out(VENC_HSTART, (dm->osd0->info.var.left_margin) * \
+			divisor);
+		/* set Horizontal data valid range  */
+		dispc_reg_out(VENC_HVALID, (dm->osd0->info.var.width) * \
+			divisor);
+		/* set Vertical interval  */
+		dispc_reg_out(VENC_VINT, dm->osd0->info.var.upper_margin + \
+			dm->osd0->info.var.lower_margin + \
+			dm->osd0->info.var.height - 1);
+		/* set Vertical data valid start position */
+		dispc_reg_out(VENC_VSTART, dm->osd0->info.var.upper_margin);
+		/* set Horizontal data valid range  */
+		dispc_reg_out(VENC_VVALID, dm->osd0->info.var.height);
+		/* Enable VCLK */
+		dispc_reg_out(VENC_OSDCLK1, 3);
+		dispc_reg_out(VENC_LCDOUT, 0x81);
+		dispc_reg_out(VENC_OSDCLK0, 0x0);
+		dispc_reg_out(VENC_OSDCLK1, 0xFFFF);
+		dispc_reg_out(VENC_CLKCTL, 0x10);
+		dispc_reg_out(VENC_SYNCCTL, 0x0F);
+		dispc_reg_merge(VENC_VIDCTL, \
+			((dm->osd0->info.var.sync & FB_SYNC_PIXCLOCK_HIGH_ACT) \
+			== FB_SYNC_PIXCLOCK_HIGH_ACT) << 14, VENC_VIDCTL_VCLKP);
+		dispc_reg_merge(VENC_SYNCCTL, \
+			((dm->osd0->info.var.sync & FB_SYNC_VERT_HIGH_ACT) \
+			!= FB_SYNC_VERT_HIGH_ACT) << 3, VENC_SYNCCTL_VPL);
+		dispc_reg_merge(VENC_SYNCCTL, \
+			((dm->osd0->info.var.sync & FB_SYNC_HOR_HIGH_ACT) \
+			!= FB_SYNC_HOR_HIGH_ACT) << 2, VENC_SYNCCTL_HPL);
+
+		/* set osd window */
+		dispc_reg_out(OSD_VIDWINMD, 0x22);
+		dispc_reg_out(OSD_OSDWIN0MD, 0x2933);
+		dispc_reg_out(OSD_OSDWIN0XP, dm->osd0->info.var.xoffset);
+		dispc_reg_out(OSD_OSDWIN0YP, dm->osd0->info.var.yoffset);
+		dispc_reg_out(OSD_OSDWIN0XL, dm->osd0->info.var.xres * \
+			divisor);
+		dispc_reg_out(OSD_OSDWIN0YL, dm->osd0->info.var.yres);
+
+		/* Set RGB565 mode */
+		dispc_reg_merge(OSD_OSDWIN0MD, OSD_OSDWIN0MD_RGB0E, \
+			OSD_OSDWIN0MD_RGB0E);
+		dispc_reg_merge(OSD_OSDWIN0MD, on, OSD_OSDWIN0MD_OACT0);
+
+		dispc_reg_merge(VENC_VIDCTL, VENC_VIDCTL_VCLKE, \
+			VENC_VIDCTL_VCLKE);
+		/* set origin position */
+		/* Values manually defined
+		dispc_reg_out(OSD_BASEPX, 0x32);
+		dispc_reg_out(OSD_BASEPY, 0x08);*/
+		dispc_reg_out(OSD_BASEPX, 20 + dm->osd0->info.var.left_margin);
+		dispc_reg_out(OSD_BASEPY, dm->osd0->info.var.upper_margin);
+
+		dispc_reg_out(VENC_VMOD, (VENC_VMOD_VDMD_RGB666 << \
+			VENC_VMOD_VDMD_SHIFT) | VENC_VMOD_ITLCL | \
+			VENC_VMOD_HDMD | VENC_VMOD_VIE | VENC_VMOD_VENC \
+			| VENC_VMOD_VMD);
+	} else {
+		/* Reset video encoder module */
+		dispc_reg_out(VENC_VMOD, 0);
+	}
+}
+
 static inline void fix_default_var(struct dm_win_info *w,
 				   u32 xres, u32 yres, u32 xpos, u32 ypos,
 				   int n_buf)
@@ -1487,6 +1622,8 @@ static int davincifb_probe(struct platform_device *pdev)
 		dm->output_device_config = davincifb_pal_svideo_config;
 	else if ((dmparams.output == PAL) && (dmparams.format == COMPONENT))
 		dm->output_device_config = davincifb_pal_component_config;
+	else if ((dmparams.output == LCD) && (dmparams.format == RGB))
+		dm->output_device_config = davincifb_lcd_rgb_config;
 	/* Add support for other displays here */
 	else {
 		printk(KERN_WARNING "Unsupported output device!\n");
diff --git a/include/video/davincifb.h b/include/video/davincifb.h
index c3ecdd8..c4e8127 100644
--- a/include/video/davincifb.h
+++ b/include/video/davincifb.h
@@ -14,12 +14,12 @@
 #define _DAVINCIFB_H_
 
 #include <mach/io.h>
+#include <linux/fb.h>
 
 /* Base registers */
-#define	VPBE_REG_BASE				0x01c72780
-#define	VENC_REG_BASE				0x01c72400
-#define	OSD_REG_BASE				0x01c72600
-#define OSD_REG_SIZE				0x00000180
+#define VENC_REG_BASE				0x01C71E00
+#define OSD_REG_BASE				0x01C71C00
+#define OSD_REG_SIZE				0x00000100
 
 /* VPBE Global Registers */
 #define	VPBE_PID				(VPBE_BASE + 0x0)
@@ -107,6 +107,7 @@
 #define	VENC_HVLDCL0				(VENC_REG_BASE + 0x134)
 #define	VENC_HVLDCL1				(VENC_REG_BASE + 0x138)
 #define	VENC_OSDHAD				(VENC_REG_BASE + 0x13C)
+#define VENC_CLKCTL				(VENC_REG_BASE + 0x140)
 
 #define VID0	0
 #define VID1	1
@@ -125,9 +126,11 @@
 #define	OSD_VIDWIN1OFST				(OSD_REG_BASE + 0x1C)
 #define	OSD_OSDWIN0OFST				(OSD_REG_BASE + 0x20)
 #define	OSD_OSDWIN1OFST				(OSD_REG_BASE + 0x24)
+#define	OSD_VIDWINADH				(OSD_REG_BASE + 0x28)
 #define		OSD_WINADR(i)			(OSD_REG_BASE + 0x2C + (i)*0x4)
 #define	OSD_VIDWIN0ADR				(OSD_REG_BASE + 0x2C)
 #define	OSD_VIDWIN1ADR				(OSD_REG_BASE + 0x30)
+#define	OSD_OSDWINADH				(OSD_REG_BASE + 0x34)
 #define	OSD_OSDWIN0ADR				(OSD_REG_BASE + 0x38)
 #define	OSD_OSDWIN1ADR				(OSD_REG_BASE + 0x3C)
 #define	OSD_BASEPX				(OSD_REG_BASE + 0x40)
@@ -202,6 +205,12 @@
 #define VENC_VMOD_BLNK				(1 << 3)
 #define VENC_VMOD_VIE				(1 << 1)
 #define VENC_VMOD_VENC				(1 << 0)
+
+#define VENC_VIDCTL_VCLKE			(1 << 13)
+#define VENC_VIDCTL_VCLKP			(1 << 14)
+#define VENC_SYNCCTL_HPL			(1 << 2)
+#define VENC_SYNCCTL_VPL			(1 << 3)
+
 /* other VENC registers' bit positions not defined yet */
 
 #define	OSD_MODE_CS				(1 << 15)
