platform/mipi_dsi: (cleanup) separating mmss and mdp from dsi

seperating mmss clk settings and mdp settings from the
msm_shared mipi_dsi.c file. Clocks will be done in acpuclock.c
Mdp settings will be done in mdp4.c file. Registers bases
moved to iomap.

Change-Id: I51c27db88b9328cb9484be5de4aeb4376cca0cc2
diff --git a/platform/msm8960/include/platform/clock.h b/platform/msm8960/include/platform/clock.h
index f254f36..ae95601 100644
--- a/platform/msm8960/include/platform/clock.h
+++ b/platform/msm8960/include/platform/clock.h
@@ -71,6 +71,7 @@
 #define DSI_NS_REG              REG_MM(0x54)
 #define DSI_MD_REG              REG_MM(0x50)
 #define DSI_CC_REG              REG_MM(0x4C)
+#define MISC_CC2_REG            REG_MM(0x5C)
 #define MDP_LUT_CC_REG          REG_MM(0x016C)
 
 #define MM_PLL1_MODE_REG        REG_MM(0x031C)
diff --git a/platform/msm8960/include/platform/iomap.h b/platform/msm8960/include/platform/iomap.h
index 78d2ca9..d9660e0 100644
--- a/platform/msm8960/include/platform/iomap.h
+++ b/platform/msm8960/include/platform/iomap.h
@@ -92,4 +92,33 @@
 
 #define MSM_MMSS_CLK_CTL_BASE 0x04000000
 
+#define MIPI_DSI_BASE                         (0x04700000)
+#define REG_DSI(off)                          (MIPI_DSI_BASE + (off))
+
+#define DSIPHY_REGULATOR_BASE                 (0x500)
+#define DSIPHY_TIMING_BASE                    (0x440)
+#define DSIPHY_CTRL_BASE                      (0x470)
+#define DSIPHY_PLL_BASE                       (0x200)
+#define DSIPHY_STRENGTH_BASE                  (0x480)
+
+/* Range 0 - 4 */
+#define DSIPHY_REGULATOR_CTRL(x) REG_DSI(DSIPHY_REGULATOR_BASE + (x) * 4)
+/* Range 0 - 11 */
+#define DSIPHY_TIMING_CTRL(x)    REG_DSI(DSIPHY_TIMING_BASE + (x) * 4)
+/* Range 0 - 3 */
+#define DSIPHY_CTRL(x)           REG_DSI(DSIPHY_CTRL_BASE + (x) * 4)
+/* Range 0 - 2 */
+#define DSIPHY_STRENGTH_CTRL(x)  REG_DSI(DSIPHY_STRENGTH_BASE + (x) * 4)
+/* Range 0 - 19 */
+#define DSIPHY_PLL_CTRL(x)       REG_DSI(DSIPHY_PLL_BASE + (x) * 4)
+
+//TODO: Use mem on the stack
+#define DSI_CMD_DMA_MEM_START_ADDR_PANEL      (0x90000000)
+
+#define MDP_BASE                              (0x05100000)
+#define REG_MDP(off)                          (MDP_BASE + (off))
+
+//TODO: Where does this go?
+#define MMSS_SFPB_GPREG                       (0x05700058)
+
 #endif
diff --git a/platform/msm8x60/acpuclock.c b/platform/msm8x60/acpuclock.c
index 0577bb8..544fc5a 100644
--- a/platform/msm8x60/acpuclock.c
+++ b/platform/msm8x60/acpuclock.c
@@ -367,3 +367,60 @@
 	reg |= MMC_BOOT_MCI_CLK_IN_FEEDBACK;
 	writel( reg, MMC_BOOT_MCI_CLK );
 }
+
+void mdp_clock_init(void)
+{
+	/* Turn on the PLL2, to ramp up the MDP clock to max (200MHz) */
+	nt_pll_enable(PLL_2, 1);
+
+	config_mdp_clk(MDP_NS_VAL, MDP_MD_VAL,
+				MDP_CC_VAL, MDP_NS_REG, MDP_MD_REG, MDP_CC_REG);
+}
+
+void mmss_pixel_clock_configure(void)
+{
+	config_pixel_clk(PIXEL_NS_VAL, PIXEL_MD_VAL,
+					PIXEL_CC_VAL, MMSS_PIXEL_NS_REG,
+					MMSS_PIXEL_MD_REG, MMSS_PIXEL_CC_REG);
+}
+
+void configure_dsicore_dsiclk()
+{
+	unsigned char mnd_mode, root_en, clk_en;
+	unsigned long src_sel = 0x3;	// dsi_phy_pll0_src
+	unsigned long pre_div_func = 0x00;  // predivide by 1
+	unsigned long pmxo_sel;
+
+	secure_writel(pre_div_func << 14 | src_sel, DSI_NS_REG);
+	mnd_mode = 0;			   // Bypass MND
+	root_en = 1;
+	clk_en = 1;
+	pmxo_sel = 0;
+
+	secure_writel((pmxo_sel << 8) | (mnd_mode << 6), DSI_CC_REG);
+	secure_writel(secure_readl(DSI_CC_REG) | root_en << 2, DSI_CC_REG);
+	secure_writel(secure_readl(DSI_CC_REG) | clk_en, DSI_CC_REG);
+}
+
+void configure_dsicore_byteclk(void)
+{
+	secure_writel(0x00400401, MISC_CC2_REG);  // select pxo
+}
+
+void configure_dsicore_pclk(void)
+{
+	unsigned char mnd_mode, root_en, clk_en;
+	unsigned long src_sel = 0x3;	// dsi_phy_pll0_src
+	unsigned long pre_div_func = 0x01;  // predivide by 2
+
+	secure_writel(pre_div_func << 12 | src_sel, PIXEL_NS_REG);
+
+	mnd_mode = 0;			   // Bypass MND
+	root_en = 1;
+	clk_en = 1;
+	secure_writel(mnd_mode << 6, PIXEL_CC_REG);
+	secure_writel(secure_readl(PIXEL_CC_REG) | root_en << 2,
+				PIXEL_CC_REG);
+	secure_writel(secure_readl(PIXEL_CC_REG) | clk_en,
+				PIXEL_CC_REG);
+}
diff --git a/platform/msm8x60/include/platform/clock.h b/platform/msm8x60/include/platform/clock.h
index 7f8f818..0783efc 100644
--- a/platform/msm8x60/include/platform/clock.h
+++ b/platform/msm8x60/include/platform/clock.h
@@ -29,8 +29,6 @@
 #ifndef __PLATFORM_MSM8X60_CLOCK_H
 #define __PLATFORM_MSM8X60_CLOCK_H
 
-/* MMSS CLK CTR base address */
-#define MSM_MMSS_CLK_CTL_BASE 0x04000000
 #define MSM_MMSS_CLK_CTL_SIZE 4096
 #define REG_MM(off)     (MSM_MMSS_CLK_CTL_BASE + (off))
 
@@ -56,19 +54,28 @@
 #define MM_PLL2_STATUS_REG                      REG_MM(0x0350)
 
 /* LCD related clock defines */
-#define MMSS_AHB_NS_REG     (MSM_MMSS_CLK_CTL_BASE + 0x04)
-#define MMSS_AHB_EN_REG     (MSM_MMSS_CLK_CTL_BASE + 0x08)
-#define MMSS_AXI_NS_REG     (MSM_MMSS_CLK_CTL_BASE + 0x14)
-#define MMSS_MAXI_EN_REG    (MSM_MMSS_CLK_CTL_BASE + 0x18)
-#define MMSS_MAXI_EN2_REG   (MSM_MMSS_CLK_CTL_BASE + 0x20)
-#define MMSS_SAXI_EN_REG    (MSM_MMSS_CLK_CTL_BASE + 0x30)
+#define MMSS_AHB_NS_REG     REG_MM(0x04)
+#define MMSS_AHB_EN_REG     REG_MM(0x08)
+#define MMSS_AXI_NS_REG     REG_MM(0x14)
+#define MMSS_MAXI_EN_REG    REG_MM(0x18)
+#define MMSS_MAXI_EN2_REG   REG_MM(0x20)
+#define MMSS_SAXI_EN_REG    REG_MM(0x30)
+#define DSI_NS_REG          REG_MM(0x54)
+#define DSI_MD_REG          REG_MM(0x50)
+#define DSI_CC_REG          REG_MM(0x4C)
+#define MISC_CC2_REG        REG_MM(0x5C)
 
-#define MDP_CC_REG      (MSM_MMSS_CLK_CTL_BASE + 0xC0)
-#define MDP_MD_REG      (MSM_MMSS_CLK_CTL_BASE + 0xC4)
-#define MDP_NS_REG      (MSM_MMSS_CLK_CTL_BASE + 0xD0)
-#define LCD_PIXEL_CC_REG    (MSM_MMSS_CLK_CTL_BASE + 0xD4)
-#define LCD_PIXEL_NS_REG    (MSM_MMSS_CLK_CTL_BASE + 0xDC)
-#define LCD_PIXEL_MD_REG    (MSM_MMSS_CLK_CTL_BASE + 0xD8)
+#define MDP_CC_REG          REG_MM(0xC0)
+#define MDP_MD_REG          REG_MM(0xC4)
+#define MDP_NS_REG          REG_MM(0xD0)
+#define MMSS_PIXEL_MD_REG   REG_MM(0xD8)
+#define MMSS_PIXEL_NS_REG   REG_MM(0xDC)
+#define MMSS_PIXEL_CC_REG   REG_MM(0xD4)
+
+/* MMSS DSI Pixel Registers not MMSS Pixel */
+#define PIXEL_MD_REG        REG_MM(0x134)
+#define PIXEL_NS_REG        REG_MM(0x138)
+#define PIXEL_CC_REG        REG_MM(0x130)
 
 /* Configured at 200 MHz */
 #define MDP_NS_VAL              0x3F000008
@@ -124,5 +131,7 @@
 void hsusb_clock_init(void);
 void clock_config_uart_dm(uint8_t id);
 void clock_config_i2c(uint8_t id, uint32_t freq);
+void mdp_clock_init(void);
+void mmss_pixel_clock_configure(void);
 
 #endif
diff --git a/platform/msm8x60/include/platform/iomap.h b/platform/msm8x60/include/platform/iomap.h
index 714c5c8..8da32e2 100755
--- a/platform/msm8x60/include/platform/iomap.h
+++ b/platform/msm8x60/include/platform/iomap.h
@@ -135,4 +135,36 @@
 #define MSM_ADM_BASE            0x18400000
 #define MSM_ADM_SD_OFFSET       0x00020800
 
+/* MMSS CLK CTR base address */
+#define MSM_MMSS_CLK_CTL_BASE 0x04000000
+
+#define MIPI_DSI_BASE                         (0x04700000)
+#define REG_DSI(off)                          (MIPI_DSI_BASE + (off))
+
+#define DSIPHY_REGULATOR_BASE                 (0x2CC)
+#define DSIPHY_TIMING_BASE                    (0x260)
+#define DSIPHY_CTRL_BASE                      (0x290)
+#define DSIPHY_PLL_BASE                       (0x200)
+#define DSIPHY_STRENGTH_BASE                  (0x2A0)
+
+/* Range 0 - 4 */
+#define DSIPHY_REGULATOR_CTRL(x) REG_DSI(DSIPHY_REGULATOR_BASE + (x) * 4)
+/* Range 0 - 11 */
+#define DSIPHY_TIMING_CTRL(x)    REG_DSI(DSIPHY_TIMING_BASE + (x) * 4)
+/* Range 0 - 3 */
+#define DSIPHY_CTRL(x)           REG_DSI(DSIPHY_CTRL_BASE + (x) * 4)
+/* Range 0 - 2 */
+#define DSIPHY_STRENGTH_CTRL(x)  REG_DSI(DSIPHY_STRENGTH_BASE + (x) * 4)
+/* Range 0 - 19 */
+#define DSIPHY_PLL_CTRL(x)       REG_DSI(DSIPHY_PLL_BASE + (x) * 4)
+
+//TODO: Use mem on the stack
+#define DSI_CMD_DMA_MEM_START_ADDR_PANEL      (0x46000000)
+
+#define MDP_BASE                              (0x05100000)
+#define REG_MDP(off)                          (MDP_BASE + (off))
+
+//TODO: Where does this belong?
+#define MMSS_SFPB_GPREG                       (0x05700058)
+
 #endif
diff --git a/platform/msm8x60/panel.c b/platform/msm8x60/panel.c
index 0e78ed3..47ceabb 100644
--- a/platform/msm8x60/panel.c
+++ b/platform/msm8x60/panel.c
@@ -361,19 +361,6 @@
 
 }
 
-void mdp_clock_init(void)
-{
-    /* Turn on the PLL2, to ramp up the MDP clock to max (200MHz) */
-    nt_pll_enable(PLL_2, 1);
-
-    config_mdp_clk(MDP_NS_VAL, MDP_MD_VAL,
-                   MDP_CC_VAL, MDP_NS_REG, MDP_MD_REG, MDP_CC_REG);
-
-    config_pixel_clk(PIXEL_NS_VAL, PIXEL_MD_VAL,
-                     PIXEL_CC_VAL, LCD_PIXEL_NS_REG,
-                     LCD_PIXEL_MD_REG, LCD_PIXEL_CC_REG);
-}
-
 void lcdc_on(void)
 {
     board_lcd_enable();
diff --git a/platform/msm8x60/platform.c b/platform/msm8x60/platform.c
index 80dae3b..04bc894 100644
--- a/platform/msm8x60/platform.c
+++ b/platform/msm8x60/platform.c
@@ -123,12 +123,17 @@
     struct fbcon_config *fb_cfg;
 #if DISPLAY_TYPE_LCDC
     mdp_clock_init();
+    mmss_pixel_clock_configure();
     fb_cfg = lcdc_init();
     panel_poweron();
     fbcon_setup(fb_cfg);
 #endif
 #if DISPLAY_TYPE_MIPI
     mdp_clock_init();
+    configure_dsicore_dsiclk();
+    configure_dsicore_byteclk();
+    configure_dsicore_pclk();
+
     fb_cfg = mipi_init();
     fbcon_setup(fb_cfg);
 #endif
diff --git a/platform/msm_shared/include/mdp4.h b/platform/msm_shared/include/mdp4.h
new file mode 100644
index 0000000..703a20f
--- /dev/null
+++ b/platform/msm_shared/include/mdp4.h
@@ -0,0 +1,91 @@
+/* Copyright (c) 2011, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *     * Neither the name of Code Aurora Forum, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#include <dev/fbcon.h>
+
+//TODO: Make a global PASS / FAIL define
+#define PASS                        0
+#define FAIL                        1
+
+#define MDP_OVERLAYPROC0_START                REG_MDP(0x00004)
+#define MDP_DMA_P_START                       REG_MDP(0x0000C)
+#define MDP_DMA_S_START                       REG_MDP(0x00010)
+#define MDP_AXI_RDMASTER_CONFIG               REG_MDP(0x00028)
+#define MDP_AXI_WRMASTER_CONFIG               REG_MDP(0x00030)
+#define MDP_DISP_INTF_SEL                     REG_MDP(0x00038)
+#define MDP_MAX_RD_PENDING_CMD_CONFIG         REG_MDP(0x0004C)
+#define MDP_INTR_ENABLE                       REG_MDP(0x00050)
+#define MDP_DSI_CMD_MODE_ID_MAP               REG_MDP(0x000A0)
+#define MDP_DSI_CMD_MODE_TRIGGER_EN           REG_MDP(0x000A4)
+#define MDP_OVERLAYPROC0_CFG                  REG_MDP(0x10004)
+#define MDP_DMA_P_CONFIG                      REG_MDP(0x90000)
+#define MDP_DMA_P_OUT_XY                      REG_MDP(0x90010)
+#define MDP_DMA_P_SIZE                        REG_MDP(0x90004)
+#define MDP_DMA_P_BUF_ADDR                    REG_MDP(0x90008)
+#define MDP_DMA_P_BUF_Y_STRIDE                REG_MDP(0x9000C)
+#define MDP_DMA_P_OP_MODE                     REG_MDP(0x90070)
+#define MDP_DSI_VIDEO_EN                      REG_MDP(0xE0000)
+#define MDP_DSI_VIDEO_HSYNC_CTL               REG_MDP(0xE0004)
+#define MDP_DSI_VIDEO_VSYNC_PERIOD            REG_MDP(0xE0008)
+#define MDP_DSI_VIDEO_VSYNC_PULSE_WIDTH       REG_MDP(0xE000C)
+#define MDP_DSI_VIDEO_DISPLAY_HCTL            REG_MDP(0xE0010)
+#define MDP_DSI_VIDEO_DISPLAY_V_START         REG_MDP(0xE0014)
+#define MDP_DSI_VIDEO_DISPLAY_V_END           REG_MDP(0xE0018)
+#define MDP_DSI_VIDEO_ACTIVE_HCTL             REG_MDP(0xE001C)
+#define MDP_DSI_VIDEO_BORDER_CLR              REG_MDP(0xE0028)
+#define MDP_DSI_VIDEO_HSYNC_SKEW              REG_MDP(0xE0030)
+#define MDP_DSI_VIDEO_CTL_POLARITY            REG_MDP(0xE0038)
+#define MDP_DSI_VIDEO_TEST_CTL                REG_MDP(0xE0034)
+
+#define MDP_TEST_MODE_CLK                     REG_MDP(0xF0000)
+#define MDP_INTR_STATUS                       REG_MDP(0x00054)
+
+void mdp_setup_dma_p_video_config(unsigned short pack_pattern,
+                                    unsigned short img_width,
+                                    unsigned short img_height,
+                                    unsigned long input_img_addr,
+                                    unsigned short img_width_full_size,
+                                    unsigned char ystride);
+int mdp_setup_dma_p_video_mode(unsigned short disp_width,
+                               unsigned short disp_height,
+                               unsigned short img_width,
+                               unsigned short img_height,
+                               unsigned short hsync_porch0_fp,
+                               unsigned short hsync_porch0_bp,
+                               unsigned short vsync_porch0_fp,
+                               unsigned short vsync_porch0_bp,
+                               unsigned short hsync_width,
+                               unsigned short vsync_width,
+                               unsigned long input_img_addr,
+                               unsigned short img_width_full_size,
+                               unsigned short pack_pattern,
+                               unsigned char ystride);
+int mipi_dsi_cmd_config(struct fbcon_config mipi_fb_cfg, unsigned short num_of_lanes);
+void mdp_shutdown(void);
+void mdp_disable(void);
+void mdp_start_dma(void);
diff --git a/platform/msm_shared/include/mipi_dsi.h b/platform/msm_shared/include/mipi_dsi.h
index 2a2b951..abbc0be 100644
--- a/platform/msm_shared/include/mipi_dsi.h
+++ b/platform/msm_shared/include/mipi_dsi.h
@@ -33,143 +33,43 @@
 #define PASS                        0
 #define FAIL                        1
 
-#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+#define DSI_CLKOUT_TIMING_CTRL                REG_DSI(0x0C0)
+#define DSI_SOFT_RESET                        REG_DSI(0x114)
+#define DSI_CAL_CTRL                          REG_DSI(0x0F4)
 
-#define MIPI_DSI_BASE                         (0x04700000)
+#define DSIPHY_SW_RESET                       REG_DSI(0x128)
+#define DSIPHY_PLL_RDY                        REG_DSI(0x280)
+#define DSIPHY_REGULATOR_CAL_PWR_CFG          REG_DSI(0x518)
 
-#define DSI_CLKOUT_TIMING_CTRL                (0x047000C0)
-#define MMSS_DSI_PIXEL_MD                     (0x04000134)
-#define MMSS_DSI_PIXEL_NS                     (0x04000138)
-#define MMSS_DSI_PIXEL_CC                     (0x04000130)
-#define MMSS_DSI_CC                           (0x0400004C)
-#define MMSS_DSI_MD                           (0x04000050)
-#define MMSS_DSI_NS                           (0x04000054)
-#define MMSS_MISC_CC2                         (0x0400005C)
-#define MMSS_MISC_CC                          (0x04000058)
-#define DSI_PHY_SW_RESET                      (0x04700128)
-#define DSI_SOFT_RESET                        (0x04700114)
-#define DSI_CAL_CTRL                          (0x047000F4)
+#define DSI_CLK_CTRL                          REG_DSI(0x118)
+#define DSI_TRIG_CTRL                         REG_DSI(0x080)
+#define DSI_CTRL                              REG_DSI(0x000)
+#define DSI_COMMAND_MODE_DMA_CTRL             REG_DSI(0x038)
+#define DSI_COMMAND_MODE_MDP_CTRL             REG_DSI(0x03C)
+#define DSI_COMMAND_MODE_MDP_DCS_CMD_CTRL     REG_DSI(0x040)
+#define DSI_DMA_CMD_OFFSET                    REG_DSI(0x044)
+#define DSI_DMA_CMD_LENGTH                    REG_DSI(0x048)
+#define DSI_COMMAND_MODE_MDP_STREAM0_CTRL     REG_DSI(0x054)
+#define DSI_COMMAND_MODE_MDP_STREAM0_TOTAL    REG_DSI(0x058)
+#define DSI_COMMAND_MODE_MDP_STREAM1_CTRL     REG_DSI(0x05C)
+#define DSI_COMMAND_MODE_MDP_STREAM1_TOTAL    REG_DSI(0x060)
+#define DSI_ERR_INT_MASK0                     REG_DSI(0x108)
+#define DSI_INT_CTRL                          REG_DSI(0x10C)
 
-#define DSIPHY_REGULATOR_CTRL_0               (0x047002CC)
-#define DSIPHY_REGULATOR_CTRL_1               (0x047002D0)
-#define DSIPHY_REGULATOR_CTRL_2               (0x047002D4)
-#define DSIPHY_REGULATOR_CTRL_3               (0x047002D8)
-#define DSIPHY_REGULATOR_CAL_PWR_CFG          (0x04700518)
+#define DSI_VIDEO_MODE_ACTIVE_H               REG_DSI(0x020)
+#define DSI_VIDEO_MODE_ACTIVE_V               REG_DSI(0x024)
+#define DSI_VIDEO_MODE_TOTAL                  REG_DSI(0x028)
+#define DSI_VIDEO_MODE_HSYNC                  REG_DSI(0x02C)
+#define DSI_VIDEO_MODE_VSYNC                  REG_DSI(0x030)
+#define DSI_VIDEO_MODE_VSYNC_VPOS             REG_DSI(0x034)
 
-#define DSIPHY_TIMING_CTRL_0                  (0x04700260)
-#define DSIPHY_TIMING_CTRL_1                  (0x04700264)
-#define DSIPHY_TIMING_CTRL_2                  (0x04700268)
-#define DSIPHY_TIMING_CTRL_3                  (0x0470026C)
-#define DSIPHY_TIMING_CTRL_4                  (0x04700270)
-#define DSIPHY_TIMING_CTRL_5                  (0x04700274)
-#define DSIPHY_TIMING_CTRL_6                  (0x04700278)
-#define DSIPHY_TIMING_CTRL_7                  (0x0470027C)
-#define DSIPHY_TIMING_CTRL_8                  (0x04700280)
-#define DSIPHY_TIMING_CTRL_9                  (0x04700284)
-#define DSIPHY_TIMING_CTRL_10                 (0x04700288)
-
-#define DSIPHY_CTRL_0                         (0x04700290)
-#define DSIPHY_CTRL_1                         (0x04700294)
-#define DSIPHY_CTRL_2                         (0x04700298)
-#define DSIPHY_CTRL_3                         (0x0470029C)
-
-#define DSIPHY_STRENGTH_CTRL_0                (0x047002A0)
-#define DSIPHY_STRENGTH_CTRL_1                (0x047002A4)
-#define DSIPHY_STRENGTH_CTRL_2                (0x047002A8)
-#define DSIPHY_STRENGTH_CTRL_3                (0x047002AC)
-
-#define DSIPHY_PLL_CTRL_0                     (0x04700200)
-#define DSIPHY_PLL_CTRL_1                     (0x04700204)
-#define DSIPHY_PLL_CTRL_2                     (0x04700208)
-#define DSIPHY_PLL_CTRL_3                     (0x0470020C)
-#define DSIPHY_PLL_CTRL_4                     (0x04700210)
-#define DSIPHY_PLL_CTRL_5                     (0x04700214)
-#define DSIPHY_PLL_CTRL_6                     (0x04700218)
-#define DSIPHY_PLL_CTRL_7                     (0x0470021C)
-#define DSIPHY_PLL_CTRL_8                     (0x04700220)
-#define DSIPHY_PLL_CTRL_9                     (0x04700224)
-#define DSIPHY_PLL_CTRL_10                    (0x04700228)
-#define DSIPHY_PLL_CTRL_11                    (0x0470022C)
-#define DSIPHY_PLL_CTRL_12                    (0x04700230)
-#define DSIPHY_PLL_CTRL_13                    (0x04700234)
-#define DSIPHY_PLL_CTRL_14                    (0x04700238)
-#define DSIPHY_PLL_CTRL_15                    (0x0470023C)
-#define DSIPHY_PLL_CTRL_16                    (0x04700240)
-#define DSIPHY_PLL_CTRL_17                    (0x04700244)
-#define DSIPHY_PLL_CTRL_18                    (0x04700248)
-#define DSIPHY_PLL_CTRL_19                    (0x0470024C)
-
-#define DSIPHY_PLL_RDY                        (0x04700280)
-
-#if DISPLAY_MIPI_PANEL_TOSHIBA_MDT61
-#define DSI_CMD_DMA_MEM_START_ADDR_PANEL      (0x90000000)
-#else
-#define DSI_CMD_DMA_MEM_START_ADDR_PANEL      (0x46000000)
-#endif
-
-#define DSI_CLK_CTRL                          (0x04700118)
-#define DSI_TRIG_CTRL                         (0x04700080)
-#define DSI_CTRL                              (0x04700000)
-#define DSI_COMMAND_MODE_DMA_CTRL             (0x04700038)
-#define DSI_COMMAND_MODE_MDP_CTRL             (0x0470003C)
-#define DSI_COMMAND_MODE_MDP_DCS_CMD_CTRL     (0x04700040)
-#define DSI_DMA_CMD_OFFSET                    (0x04700044)
-#define DSI_DMA_CMD_LENGTH                    (0x04700048)
-#define DSI_COMMAND_MODE_MDP_STREAM0_CTRL     (0x04700054)
-#define DSI_COMMAND_MODE_MDP_STREAM0_TOTAL    (0x04700058)
-#define DSI_COMMAND_MODE_MDP_STREAM1_CTRL     (0x0470005C)
-#define DSI_COMMAND_MODE_MDP_STREAM1_TOTAL    (0x04700060)
-#define DSI_ERR_INT_MASK0                     (0x04700108)
-#define DSI_INT_CTRL                          (0x0470010C)
-
-#define DSI_VIDEO_MODE_ACTIVE_H               (0x04700020)
-#define DSI_VIDEO_MODE_ACTIVE_V               (0x04700024)
-#define DSI_VIDEO_MODE_TOTAL                  (0x04700028)
-#define DSI_VIDEO_MODE_HSYNC                  (0x0470002C)
-#define DSI_VIDEO_MODE_VSYNC                  (0x04700030)
-#define DSI_VIDEO_MODE_VSYNC_VPOS             (0x04700034)
-
-#define DSI_MISR_CMD_CTRL                     (0x0470009C)
-#define DSI_MISR_VIDEO_CTRL                   (0x047000A0)
-#define DSI_EOT_PACKET_CTRL                   (0x047000C8)
-#define DSI_VIDEO_MODE_CTRL                   (0x0470000C)
-#define DSI_CAL_STRENGTH_CTRL                 (0x04700100)
-#define DSI_CMD_MODE_DMA_SW_TRIGGER           (0x0470008C)
-#define DSI_CMD_MODE_MDP_SW_TRIGGER           (0x04700090)
-
-#define MDP_OVERLAYPROC0_START                (0x05100004)
-#define MDP_DMA_P_START                       (0x0510000C)
-#define MDP_DMA_S_START                       (0x05100010)
-#define MDP_AXI_RDMASTER_CONFIG               (0x05100028)
-#define MDP_AXI_WRMASTER_CONFIG               (0x05100030)
-#define MDP_DISP_INTF_SEL                     (0x05100038)
-#define MDP_MAX_RD_PENDING_CMD_CONFIG         (0x0510004C)
-#define MDP_INTR_ENABLE                       (0x05100050)
-#define MDP_DSI_CMD_MODE_ID_MAP               (0x051000A0)
-#define MDP_DSI_CMD_MODE_TRIGGER_EN           (0x051000A4)
-#define MDP_OVERLAYPROC0_CFG                  (0x05110004)
-#define MDP_DMA_P_CONFIG                      (0x05190000)
-#define MDP_DMA_P_OUT_XY                      (0x05190010)
-#define MDP_DMA_P_SIZE                        (0x05190004)
-#define MDP_DMA_P_BUF_ADDR                    (0x05190008)
-#define MDP_DMA_P_BUF_Y_STRIDE                (0x0519000C)
-#define MDP_DMA_P_OP_MODE                     (0x05190070)
-#define MDP_DSI_VIDEO_EN                      (0x051E0000)
-#define MDP_DSI_VIDEO_HSYNC_CTL               (0x051E0004)
-#define MDP_DSI_VIDEO_VSYNC_PERIOD            (0x051E0008)
-#define MDP_DSI_VIDEO_VSYNC_PULSE_WIDTH       (0x051E000C)
-#define MDP_DSI_VIDEO_DISPLAY_HCTL            (0x051E0010)
-#define MDP_DSI_VIDEO_DISPLAY_V_START         (0x051E0014)
-#define MDP_DSI_VIDEO_DISPLAY_V_END           (0x051E0018)
-#define MDP_DSI_VIDEO_ACTIVE_HCTL             (0x051E001C)
-#define MDP_DSI_VIDEO_BORDER_CLR              (0x051E0028)
-#define MDP_DSI_VIDEO_HSYNC_SKEW              (0x051E0030)
-#define MDP_DSI_VIDEO_CTL_POLARITY            (0x051E0038)
-#define MDP_DSI_VIDEO_TEST_CTL                (0x051E0034)
-
-#define MDP_TEST_MODE_CLK                     (0x051F0000)
-#define MDP_INTR_STATUS                       (0x05100054)
-#define MMSS_SFPB_GPREG                       (0x05700058)
+#define DSI_MISR_CMD_CTRL                     REG_DSI(0x09C)
+#define DSI_MISR_VIDEO_CTRL                   REG_DSI(0x0A0)
+#define DSI_EOT_PACKET_CTRL                   REG_DSI(0x0C8)
+#define DSI_VIDEO_MODE_CTRL                   REG_DSI(0x00C)
+#define DSI_CAL_STRENGTH_CTRL                 REG_DSI(0x100)
+#define DSI_CMD_MODE_DMA_SW_TRIGGER           REG_DSI(0x08C)
+#define DSI_CMD_MODE_MDP_SW_TRIGGER           REG_DSI(0x090)
 
 #define MIPI_DSI_MRPS       0x04 /* Maximum Return Packet Size */
 #define MIPI_DSI_REG_LEN    16   /* 4 x 4 bytes register */
diff --git a/platform/msm_shared/mdp4.c b/platform/msm_shared/mdp4.c
new file mode 100644
index 0000000..3fbf2fc
--- /dev/null
+++ b/platform/msm_shared/mdp4.c
@@ -0,0 +1,187 @@
+/* Copyright (c) 2011, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *     * Neither the name of Code Aurora Forum, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#include <mdp4.h>
+#include <debug.h>
+#include <reg.h>
+#include <target/display.h>
+#include <platform/timer.h>
+#include <platform/iomap.h>
+
+void mdp_setup_dma_p_video_config(unsigned short pack_pattern,
+                                    unsigned short img_width,
+                                    unsigned short img_height,
+                                    unsigned long input_img_addr,
+                                    unsigned short img_width_full_size,
+                                    unsigned char ystride){
+    dprintf(SPEW, "MDP4.2 Setup for DSI Video Mode\n");
+
+    // ----- programming MDP_AXI_RDMASTER_CONFIG --------
+    /* MDP_AXI_RDMASTER_CONFIG set all master to read from AXI port 0, that's
+       the only port connected */
+    //TODO: Seems to still work without this
+    writel(0x00290000, MDP_AXI_RDMASTER_CONFIG);
+    writel(0x00000004, MDP_AXI_WRMASTER_CONFIG);
+    writel(0x00007777, MDP_MAX_RD_PENDING_CMD_CONFIG);
+
+   /* Set up CMD_INTF_SEL, VIDEO_INTF_SEL, EXT_INTF_SEL, SEC_INTF_SEL, PRIM_INTF_SEL */
+   writel(0x00000049, MDP_DISP_INTF_SEL);
+
+   /* DMA P */
+   writel(0x0000000b, MDP_OVERLAYPROC0_CFG);
+
+   /* RGB 888 */
+   writel(pack_pattern << 8 | 0xbf | (0 << 25), MDP_DMA_P_CONFIG);
+
+   writel(0x0, MDP_DMA_P_OUT_XY);
+
+   writel(img_height << 16 | img_width, MDP_DMA_P_SIZE);
+
+   writel(input_img_addr, MDP_DMA_P_BUF_ADDR);
+
+   writel(img_width_full_size * ystride, MDP_DMA_P_BUF_Y_STRIDE);
+}
+
+int mdp_setup_dma_p_video_mode(unsigned short disp_width,
+                               unsigned short disp_height,
+                               unsigned short img_width,
+                               unsigned short img_height,
+                               unsigned short hsync_porch0_fp,
+                               unsigned short hsync_porch0_bp,
+                               unsigned short vsync_porch0_fp,
+                               unsigned short vsync_porch0_bp,
+                               unsigned short hsync_width,
+                               unsigned short vsync_width,
+                               unsigned long input_img_addr,
+                               unsigned short img_width_full_size,
+                               unsigned short pack_pattern,
+                               unsigned char ystride)
+{
+
+    // unsigned long mdp_intr_status;
+    int status = FAIL;
+    unsigned long hsync_period;
+    unsigned long vsync_period;
+    unsigned long vsync_period_intmd;
+
+    dprintf(SPEW, "MDP4.1 for DSI Video Mode\n");
+
+    hsync_period = img_width + hsync_porch0_fp + hsync_porch0_bp + 1;
+    vsync_period_intmd = img_height + vsync_porch0_fp + vsync_porch0_bp + 1;
+    vsync_period = vsync_period_intmd * hsync_period;
+
+    // ----- programming MDP_AXI_RDMASTER_CONFIG --------
+    /* MDP_AXI_RDMASTER_CONFIG set all master to read from AXI port 0, that's
+       the only port connected */
+    writel(0x00290000, MDP_AXI_RDMASTER_CONFIG);
+    writel(0x00000004, MDP_AXI_WRMASTER_CONFIG);
+    writel(0x00007777, MDP_MAX_RD_PENDING_CMD_CONFIG);
+    /* sets PRIM_INTF_SEL to 0x1 and SEC_INTF_SEL to 0x2 and DSI_VIDEO_INTF_SEL*/
+    writel(0x00000049, MDP_DISP_INTF_SEL);
+    writel(0x0000000b, MDP_OVERLAYPROC0_CFG);
+
+    // ------------- programming MDP_DMA_P_CONFIG ---------------------
+    writel(pack_pattern << 8 | 0xbf | (0 << 25), MDP_DMA_P_CONFIG); // rgb888
+
+    writel(0x00000000, MDP_DMA_P_OUT_XY);
+    writel(img_height << 16 | img_width, MDP_DMA_P_SIZE);
+    writel(input_img_addr, MDP_DMA_P_BUF_ADDR);
+    writel(img_width_full_size * ystride, MDP_DMA_P_BUF_Y_STRIDE);
+    writel(0x00ff0000, MDP_DMA_P_OP_MODE);
+    writel(hsync_period << 16 | hsync_width, MDP_DSI_VIDEO_HSYNC_CTL);
+    writel(vsync_period, MDP_DSI_VIDEO_VSYNC_PERIOD);
+    writel(vsync_width * hsync_period, MDP_DSI_VIDEO_VSYNC_PULSE_WIDTH);
+    writel((img_width + hsync_porch0_bp - 1) << 16 | hsync_porch0_bp,
+           MDP_DSI_VIDEO_DISPLAY_HCTL);
+    writel(vsync_porch0_bp * hsync_period, MDP_DSI_VIDEO_DISPLAY_V_START);
+    writel((img_height + vsync_porch0_bp) * hsync_period,
+           MDP_DSI_VIDEO_DISPLAY_V_END);
+    writel(0x00ABCDEF, MDP_DSI_VIDEO_BORDER_CLR);
+    writel(0x00000000, MDP_DSI_VIDEO_HSYNC_SKEW);
+    writel(0x00000000, MDP_DSI_VIDEO_CTL_POLARITY);
+    // end of cmd mdp
+
+    writel(0x00000001, MDP_DSI_VIDEO_EN);   // MDP_DSI_EN ENABLE
+
+    status = PASS;
+    return status;
+}
+
+
+int mipi_dsi_cmd_config(struct fbcon_config mipi_fb_cfg, unsigned short num_of_lanes)
+{
+
+    int status = 0;
+    unsigned long input_img_addr = MIPI_FB_ADDR;
+    unsigned short image_wd = mipi_fb_cfg.width;
+    unsigned short image_ht = mipi_fb_cfg.height;
+    unsigned short pack_pattern = 0x12;
+    unsigned char ystride = 3;
+
+    writel(0x03ffffff, MDP_INTR_ENABLE);
+    writel(0x0000000b, MDP_OVERLAYPROC0_CFG);
+
+    // ------------- programming MDP_DMA_P_CONFIG ---------------------
+    writel(pack_pattern << 8 | 0x3f | (0 << 25), MDP_DMA_P_CONFIG); // rgb888
+
+    writel(0x00000000, MDP_DMA_P_OUT_XY);
+    writel(image_ht << 16 | image_wd, MDP_DMA_P_SIZE);
+    writel(input_img_addr, MDP_DMA_P_BUF_ADDR);
+
+    writel(image_wd * ystride, MDP_DMA_P_BUF_Y_STRIDE);
+
+    writel(0x00000000, MDP_DMA_P_OP_MODE);
+
+    writel(0x10, MDP_DSI_CMD_MODE_ID_MAP);
+    writel(0x01, MDP_DSI_CMD_MODE_TRIGGER_EN);
+
+    writel(0x0001a000, MDP_AXI_RDMASTER_CONFIG);
+    writel(0x00000004, MDP_AXI_WRMASTER_CONFIG);
+    writel(0x00007777, MDP_MAX_RD_PENDING_CMD_CONFIG);
+    writel(0x8a, MDP_DISP_INTF_SEL);
+
+    return status;
+}
+
+void mdp_disable(void)
+{
+    writel(0x00000000, MDP_DSI_VIDEO_EN);
+}
+
+void mdp_shutdown(void)
+{
+    mdp_disable();
+    mdelay(60);
+    writel(0x00000000, MDP_INTR_ENABLE);
+    writel(0x00000003, MDP_OVERLAYPROC0_CFG);
+}
+
+void mdp_start_dma(void)
+{
+     writel(0x00000001, MDP_DMA_P_START);
+}
diff --git a/platform/msm_shared/mipi_dsi.c b/platform/msm_shared/mipi_dsi.c
index d5e9ff6..5c3423f 100644
--- a/platform/msm_shared/mipi_dsi.c
+++ b/platform/msm_shared/mipi_dsi.c
@@ -31,9 +31,16 @@
 #include <endian.h>
 #include <mipi_dsi.h>
 #include <dev/fbcon.h>
-#include <target/display.h>
 #include <stdlib.h>
 #include <debug.h>
+#include <target/display.h>
+#include <platform/iomap.h>
+#include <platform/clock.h>
+
+extern void mdp_disable(void);
+extern int mipi_dsi_cmd_config(struct fbcon_config mipi_fb_cfg, unsigned short num_of_lanes);
+extern void mdp_shutdown(void);
+extern void mdp_start_dma(void);
 
 #if DISPLAY_MIPI_PANEL_TOSHIBA
 static struct fbcon_config mipi_fb_cfg = {
@@ -102,55 +109,14 @@
 void secure_writel(uint32_t, uint32_t);
 uint32_t secure_readl(uint32_t);
 
-void configure_dsicore_dsiclk()
-{
-    unsigned char mnd_mode, root_en, clk_en;
-    unsigned long src_sel = 0x3;    // dsi_phy_pll0_src
-    unsigned long pre_div_func = 0x00;  // predivide by 1
-    unsigned long pmxo_sel;
-
-    secure_writel(pre_div_func << 14 | src_sel, MMSS_DSI_NS);
-    mnd_mode = 0;               // Bypass MND
-    root_en = 1;
-    clk_en = 1;
-    pmxo_sel = 0;
-
-    secure_writel((pmxo_sel << 8) | (mnd_mode << 6), MMSS_DSI_CC);
-    secure_writel(secure_readl(MMSS_DSI_CC) | root_en << 2, MMSS_DSI_CC);
-    secure_writel(secure_readl(MMSS_DSI_CC) | clk_en, MMSS_DSI_CC);
-}
-
-void configure_dsicore_byteclk(void)
-{
-    secure_writel(0x00400401, MMSS_MISC_CC2);  // select pxo
-}
-
-void configure_dsicore_pclk(void)
-{
-    unsigned char mnd_mode, root_en, clk_en;
-    unsigned long src_sel = 0x3;    // dsi_phy_pll0_src
-    unsigned long pre_div_func = 0x01;  // predivide by 2
-
-    secure_writel(pre_div_func << 12 | src_sel, MMSS_DSI_PIXEL_NS);
-
-    mnd_mode = 0;               // Bypass MND
-    root_en = 1;
-    clk_en = 1;
-    secure_writel(mnd_mode << 6, MMSS_DSI_PIXEL_CC);
-    secure_writel(secure_readl(MMSS_DSI_PIXEL_CC) | root_en << 2,
-                  MMSS_DSI_PIXEL_CC);
-    secure_writel(secure_readl(MMSS_DSI_PIXEL_CC) | clk_en,
-                  MMSS_DSI_PIXEL_CC);
-}
-
 int mipi_dsi_phy_ctrl_config(struct mipi_dsi_panel_config *pinfo)
 {
     unsigned i;
     unsigned off = 0;
     struct mipi_dsi_phy_ctrl *pd;
 
-    writel(0x00000001, DSI_PHY_SW_RESET);
-    writel(0x00000000, DSI_PHY_SW_RESET);
+    writel(0x00000001, DSIPHY_SW_RESET);
+    writel(0x00000000, DSIPHY_SW_RESET);
 
     pd = (pinfo->dsi_phy_config);
 
@@ -409,7 +375,7 @@
     unsigned char interleav = 0;
 
     // disable mdp first
-    writel(0x00000000, MDP_DSI_VIDEO_EN);
+    mdp_disable();
 
     writel(0x00000000, DSI_CLK_CTRL);
     writel(0x00000000, DSI_CLK_CTRL);
@@ -538,7 +504,7 @@
     writel(0x00000040, DSI_ERR_INT_MASK0);
     writel(0x1, DSI_EOT_PACKET_CTRL);
     // writel(0x0, MDP_OVERLAYPROC0_START);
-    writel(0x00000001, MDP_DMA_P_START);
+    mdp_start_dma();
     mdelay(10);
     writel(0x1, DSI_CMD_MODE_MDP_SW_TRIGGER);
 
@@ -546,105 +512,6 @@
     return status;
 }
 
-void mdp_setup_dma_p_video_config(unsigned short pack_pattern,
-                                    unsigned short img_width,
-                                    unsigned short img_height,
-                                    unsigned long input_img_addr,
-                                    unsigned short img_width_full_size,
-                                    unsigned char ystride){
-    dprintf(SPEW, "MDP4.2 Setup for DSI Video Mode\n");
-
-    // ----- programming MDP_AXI_RDMASTER_CONFIG --------
-    /* MDP_AXI_RDMASTER_CONFIG set all master to read from AXI port 0, that's
-       the only port connected */
-    //TODO: Seems to still work without this
-    writel(0x00290000, MDP_AXI_RDMASTER_CONFIG);
-    writel(0x00000004, MDP_AXI_WRMASTER_CONFIG);
-    writel(0x00007777, MDP_MAX_RD_PENDING_CMD_CONFIG);
-
-   /* Set up CMD_INTF_SEL, VIDEO_INTF_SEL, EXT_INTF_SEL, SEC_INTF_SEL, PRIM_INTF_SEL */
-   writel(0x00000049, MDP_DISP_INTF_SEL);
-
-   /* DMA P */
-   writel(0x0000000b, MDP_OVERLAYPROC0_CFG);
-
-   /* RGB 888 */
-   writel(pack_pattern << 8 | 0xbf | (0 << 25), MDP_DMA_P_CONFIG);
-
-   writel(0x0, MDP_DMA_P_OUT_XY);
-
-   writel(img_height << 16 | img_width, MDP_DMA_P_SIZE);
-
-   writel(input_img_addr, MDP_DMA_P_BUF_ADDR);
-
-   writel(img_width_full_size * ystride, MDP_DMA_P_BUF_Y_STRIDE);
-}
-
-int mdp_setup_dma_p_video_mode(unsigned short disp_width,
-                               unsigned short disp_height,
-                               unsigned short img_width,
-                               unsigned short img_height,
-                               unsigned short hsync_porch0_fp,
-                               unsigned short hsync_porch0_bp,
-                               unsigned short vsync_porch0_fp,
-                               unsigned short vsync_porch0_bp,
-                               unsigned short hsync_width,
-                               unsigned short vsync_width,
-                               unsigned long input_img_addr,
-                               unsigned short img_width_full_size,
-                               unsigned short pack_pattern,
-                               unsigned char ystride)
-{
-
-    // unsigned long mdp_intr_status;
-    int status = FAIL;
-    unsigned long hsync_period;
-    unsigned long vsync_period;
-    unsigned long vsync_period_intmd;
-
-    dprintf(SPEW, "Hi setup MDP4.1 for DSI Video Mode\n");
-
-    hsync_period = img_width + hsync_porch0_fp + hsync_porch0_bp + 1;
-    vsync_period_intmd = img_height + vsync_porch0_fp + vsync_porch0_bp + 1;
-    vsync_period = vsync_period_intmd * hsync_period;
-
-    // ----- programming MDP_AXI_RDMASTER_CONFIG --------
-    /* MDP_AXI_RDMASTER_CONFIG set all master to read from AXI port 0, that's
-       the only port connected */
-    writel(0x00290000, MDP_AXI_RDMASTER_CONFIG);
-    writel(0x00000004, MDP_AXI_WRMASTER_CONFIG);
-    writel(0x00007777, MDP_MAX_RD_PENDING_CMD_CONFIG);
-    /* sets PRIM_INTF_SEL to 0x1 and SEC_INTF_SEL to 0x2 and DSI_VIDEO_INTF_SEL*/
-    writel(0x00000049, MDP_DISP_INTF_SEL);
-    writel(0x0000000b, MDP_OVERLAYPROC0_CFG);
-
-    // ------------- programming MDP_DMA_P_CONFIG ---------------------
-    writel(pack_pattern << 8 | 0xbf | (0 << 25), MDP_DMA_P_CONFIG); // rgb888
-
-    writel(0x00000000, MDP_DMA_P_OUT_XY);
-    writel(img_height << 16 | img_width, MDP_DMA_P_SIZE);
-    writel(input_img_addr, MDP_DMA_P_BUF_ADDR);
-    writel(img_width_full_size * ystride, MDP_DMA_P_BUF_Y_STRIDE);
-    writel(0x00ff0000, MDP_DMA_P_OP_MODE);
-    writel(hsync_period << 16 | hsync_width, MDP_DSI_VIDEO_HSYNC_CTL);
-    writel(vsync_period, MDP_DSI_VIDEO_VSYNC_PERIOD);
-    writel(vsync_width * hsync_period, MDP_DSI_VIDEO_VSYNC_PULSE_WIDTH);
-    writel((img_width + hsync_porch0_bp - 1) << 16 | hsync_porch0_bp,
-           MDP_DSI_VIDEO_DISPLAY_HCTL);
-    writel(vsync_porch0_bp * hsync_period, MDP_DSI_VIDEO_DISPLAY_V_START);
-    writel((img_height + vsync_porch0_bp) * hsync_period,
-           MDP_DSI_VIDEO_DISPLAY_V_END);
-    writel(0x00ABCDEF, MDP_DSI_VIDEO_BORDER_CLR);
-    writel(0x00000000, MDP_DSI_VIDEO_HSYNC_SKEW);
-    writel(0x00000000, MDP_DSI_VIDEO_CTL_POLARITY);
-    // end of cmd mdp
-
-    writel(0x00000001, MDP_DSI_VIDEO_EN);   // MDP_DSI_EN ENABLE
-
-    status = PASS;
-    return status;
-}
-
 int mipi_dsi_video_config(unsigned short num_of_lanes)
 {
 
@@ -714,41 +581,6 @@
     return status;
 }
 
-int mipi_dsi_cmd_config(unsigned short num_of_lanes)
-{
-
-    int status = 0;
-    unsigned long input_img_addr = MIPI_FB_ADDR;
-    unsigned short image_wd = mipi_fb_cfg.width;
-    unsigned short image_ht = mipi_fb_cfg.height;
-    unsigned short pack_pattern = 0x12;
-    unsigned char ystride = 3;
-
-    writel(0x03ffffff, MDP_INTR_ENABLE);
-    writel(0x0000000b, MDP_OVERLAYPROC0_CFG);
-
-    // ------------- programming MDP_DMA_P_CONFIG ---------------------
-    writel(pack_pattern << 8 | 0x3f | (0 << 25), MDP_DMA_P_CONFIG); // rgb888
-
-    writel(0x00000000, MDP_DMA_P_OUT_XY);
-    writel(image_ht << 16 | image_wd, MDP_DMA_P_SIZE);
-    writel(input_img_addr, MDP_DMA_P_BUF_ADDR);
-
-    writel(image_wd * ystride, MDP_DMA_P_BUF_Y_STRIDE);
-
-    writel(0x00000000, MDP_DMA_P_OP_MODE);
-
-    writel(0x10, MDP_DSI_CMD_MODE_ID_MAP);
-    writel(0x01, MDP_DSI_CMD_MODE_TRIGGER_EN);
-
-    writel(0x0001a000, MDP_AXI_RDMASTER_CONFIG);
-    writel(0x00000004, MDP_AXI_WRMASTER_CONFIG);
-    writel(0x00007777, MDP_MAX_RD_PENDING_CMD_CONFIG);
-    writel(0x8a, MDP_DISP_INTF_SEL);
-
-    return status;
-}
-
 int is_cmd_mode_enabled(void)
 {
     return cmd_mode_status;
@@ -765,7 +597,7 @@
     unsigned short dst_format = 0;
     unsigned short traffic_mode = 0;
     struct mipi_dsi_panel_config *panel_info = &novatek_panel_info;
-    status += mipi_dsi_cmd_config(panel_info->num_of_lanes);
+    status += mipi_dsi_cmd_config(mipi_fb_cfg, panel_info->num_of_lanes);
     mdelay(50);
     config_dsi_cmd_mode(display_wd, display_ht, image_wd, image_ht,
                         dst_format, traffic_mode,
@@ -775,21 +607,18 @@
 
 void mipi_dsi_shutdown(void)
 {
-    writel(0x00000000, MDP_DSI_VIDEO_EN);
-    mdelay(10);
-    writel(0x00000000, MDP_INTR_ENABLE);
-    writel(0x00000003, MDP_OVERLAYPROC0_CFG);
+    mdp_shutdown();
     writel(0x01010101, DSI_INT_CTRL);
     writel(0x13FF3BFF, DSI_ERR_INT_MASK0);
-    writel(0, DSIPHY_PLL_CTRL_0);
+    writel(0, DSIPHY_PLL_CTRL(0));
     writel(0, DSI_CLK_CTRL);
     writel(0, DSI_CTRL);
 #if DISPLAY_MIPI_PANEL_TOSHIBA_MDT61
-    writel(0x0, MMSS_DSI_CC);
-    writel(0x0, MMSS_DSI_PIXEL_CC);
+    writel(0x0, DSI_CC_REG);
+    writel(0x0, PIXEL_CC_REG);
 #else
-    secure_writel(0x0, MMSS_DSI_CC);
-    secure_writel(0x0, MMSS_DSI_PIXEL_CC);
+    secure_writel(0x0, DSI_CC_REG);
+    secure_writel(0x0, PIXEL_CC_REG);
 #endif
 }
 
@@ -803,10 +632,6 @@
 #if DISPLAY_MIPI_PANEL_TOSHIBA_MDT61
     mipi_dsi_phy_init(panel_info);
 #else
-    configure_dsicore_dsiclk();
-    configure_dsicore_byteclk();
-    configure_dsicore_pclk();
-
     mipi_dsi_phy_ctrl_config(panel_info);
 #endif
 
diff --git a/platform/msm_shared/rules.mk b/platform/msm_shared/rules.mk
index d64947f..7d3aad9 100644
--- a/platform/msm_shared/rules.mk
+++ b/platform/msm_shared/rules.mk
@@ -27,14 +27,16 @@
 			$(LOCAL_DIR)/scm_decrypt.o \
 			$(LOCAL_DIR)/lcdc.o \
 			$(LOCAL_DIR)/mddi.o \
-			$(LOCAL_DIR)/qgic.o
+			$(LOCAL_DIR)/qgic.o \
+			$(LOCAL_DIR)/mdp4.o
 endif
 
 ifeq ($(PLATFORM),msm8960)
 	OBJS += $(LOCAL_DIR)/mipi_dsi.o \
 			$(LOCAL_DIR)/i2c_qup.o \
 			$(LOCAL_DIR)/uart_dm.o \
-			$(LOCAL_DIR)/qgic.o
+			$(LOCAL_DIR)/qgic.o \
+			$(LOCAL_DIR)/mdp4.o
 endif
 
 ifeq ($(PLATFORM),msm7x27a)
diff --git a/target/msm8960/panel.c b/target/msm8960/panel.c
index 8e49321..328dcce 100644
--- a/target/msm8960/panel.c
+++ b/target/msm8960/panel.c
@@ -29,6 +29,7 @@
 #include <debug.h>
 #include <reg.h>
 #include <mipi_dsi.h>
+#include <mdp4.h>
 #include <dev/pm8921.h>
 #include <platform/iomap.h>
 #include <platform/clock.h>