msm8960: Enabling splash screen for lk

This enables splash screen support in lk for mipi
toshiba mdt61 panel.

Change-Id: Ieb9fcde096aa8c8e55a3ceb88f2fd7513ca7187e
diff --git a/platform/msm8960/acpuclock.c b/platform/msm8960/acpuclock.c
index b2e63bb..6d65504 100644
--- a/platform/msm8960/acpuclock.c
+++ b/platform/msm8960/acpuclock.c
@@ -33,7 +33,6 @@
 #include <uart_dm.h>
 #include <gsbi.h>
 
-
 /* Set rate and enable the clock */
 void clock_config(uint32_t ns, uint32_t md, uint32_t ns_addr, uint32_t md_addr)
 {
@@ -44,7 +43,8 @@
 	writel(val, ns_addr);
 
 	/* Write the MD value into the MD register */
-	writel(md, md_addr);
+	if (md_addr != 0x0)
+		writel(md, md_addr);
 
 	/* Write the ns value, and active reset for M/N Counter, again */
 	val = 1 << 7;
@@ -73,6 +73,38 @@
 	writel(val, ns_addr);
 }
 
+/* Write the M,N,D values and enable the MMSS Clocks */
+void config_mmss_clk(  uint32_t ns,
+		uint32_t md,
+		uint32_t cc,
+		uint32_t ns_addr,
+		uint32_t md_addr,
+		uint32_t cc_addr){
+	unsigned int val = 0;
+
+	clock_config(ns, md, ns_addr, md_addr);
+
+	/* Enable MND counter */
+	val = cc | (1 << 5);
+	val = val | readl(cc_addr);
+	writel(val, cc_addr);
+
+	/* Enable the root of the clock tree */
+	val = 1 << 2;
+	val = val | readl(cc_addr);
+	writel(val, cc_addr);
+
+	/* Enable the Pixel Clock */
+	val = 1 << 0;
+	val = val | readl(cc_addr);
+	writel(val, cc_addr);
+
+	/* Force On */
+	val = 1 << 31;
+	val = val | readl(cc_addr);
+	writel(val, cc_addr);
+}
+
 void pll8_enable(void)
 {
 	unsigned int curr_value = 0;
@@ -134,3 +166,54 @@
 	writel(GSBI_HCLK_CTL_CLK_ENA << GSBI_HCLK_CTL_S, GSBIn_HCLK_CTL(id));
 }
 
+void pll1_enable(void){
+	uint32_t val = 0;
+
+	/* Reset MND divider */
+	val |= (1<<2);
+	writel(val, MM_PLL1_MODE_REG);
+
+	/* Use PLL -- Disable Bypass */
+	val |= (1<<1);
+	writel(val, MM_PLL1_MODE_REG);
+
+	/* Activate PLL out control */
+	val |= 1;
+	writel(val, MM_PLL1_MODE_REG);
+
+	while (!readl(MM_PLL1_STATUS_REG));
+}
+
+void config_mdp_lut_clk(void){
+	/* Force on*/
+	writel(MDP_LUT_VAL, MDP_LUT_CC_REG);
+}
+
+/* Turn on MDP related clocks and pll's for MDP */
+void mdp_clock_init(void)
+{
+	/* Turn on the PLL1, as source for  MDP clock */
+	pll1_enable();
+
+	/* Turn on MDP clk */
+	config_mmss_clk(MDP_NS_VAL, MDP_MD_VAL,
+				MDP_CC_VAL, MDP_NS_REG, MDP_MD_REG, MDP_CC_REG);
+
+	/* Seems to lose pixels without this from status 0x051E0048 */
+	config_mdp_lut_clk();
+}
+
+/* Initialize all clocks needed by Display */
+void mmss_clock_init(void){
+	/* Configure Pixel clock */
+	config_mmss_clk(PIXEL_NS_VAL, PIXEL_MD_VAL, PIXEL_CC_VAL, PIXEL_NS_REG, PIXEL_MD_REG, PIXEL_CC_REG);
+
+	/* Configure DSI clock */
+	config_mmss_clk(DSI_NS_VAL, DSI_MD_VAL, DSI_CC_VAL, DSI_NS_REG, DSI_MD_REG, DSI_CC_REG);
+
+	/* Configure Byte clock */
+	config_mmss_clk(BYTE_NS_VAL, 0x0, BYTE_CC_VAL, BYTE_NS_REG, 0x0, BYTE_CC_REG);
+
+	/* Configure ESC clock */
+	config_mmss_clk(ESC_NS_VAL, 0x0, ESC_CC_VAL, ESC_NS_REG, 0x0, ESC_CC_REG);
+}
diff --git a/platform/msm8960/include/platform/clock.h b/platform/msm8960/include/platform/clock.h
index 3650340..39b9856 100644
--- a/platform/msm8960/include/platform/clock.h
+++ b/platform/msm8960/include/platform/clock.h
@@ -26,7 +26,6 @@
  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
  * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
-
 #ifndef __PLATFORM_MSM8960_CLOCK_H
 #define __PLATFORM_MSM8960_CLOCK_H
 
@@ -50,4 +49,57 @@
 void clock_config_uart_dm(uint8_t id);
 void clock_config_i2c(uint8_t id, uint32_t freq);
 
+#define REG_MM(off)     (MSM_MMSS_CLK_CTL_BASE + (off))
+
+#define MDP_CC_REG              REG_MM(0xC0)
+#define MDP_MD_REG              REG_MM(0xC4)
+#define MDP_NS_REG              REG_MM(0xD0)
+#define ESC_CC_REG              REG_MM(0xCC)
+#define ESC_NS_REG              REG_MM(0x11C)
+#define BYTE_CC_REG             REG_MM(0x90)
+#define BYTE_NS_REG             REG_MM(0xB0)
+#define PIXEL_CC_REG            REG_MM(0x130)
+#define PIXEL_MD_REG            REG_MM(0x134)
+#define PIXEL_NS_REG            REG_MM(0x138)
+#define DSI_NS_REG              REG_MM(0x54)
+#define DSI_MD_REG              REG_MM(0x50)
+#define DSI_CC_REG              REG_MM(0x4C)
+#define MDP_LUT_CC_REG          REG_MM(0x016C)
+
+#define MM_PLL1_MODE_REG        REG_MM(0x031C)
+#define MM_PLL1_STATUS_REG      REG_MM(0x0334)
+#define MM_PLL1_CONFIG_REG      REG_MM(0x032C)
+
+/* Configured for 200MHz */
+#define MDP_NS_VAL            0x3F3FC008
+#define MDP_MD_VAL            0x000001FB
+#define MDP_CC_VAL            0x00000500
+
+/* Configured at 13.5 MHz */
+#define ESC_NS_VAL            0x00001000
+#define ESC_CC_VAL            0x00000004
+
+#define BYTE_NS_VAL           0x00000001
+#define BYTE_CC_VAL           0x00000004
+
+#define PIXEL_NS_VAL          0x00F80003
+#define PIXEL_MD_VAL          0x000001FB
+#define PIXEL_CC_VAL          0x00000080
+
+#define DSI_NS_VAL            0xFA000003
+#define DSI_MD_VAL            0x000003FB
+#define DSI_CC_VAL            0x00000080
+
+#define MDP_LUT_VAL           0x00000001
+
+void config_mmss_clk(  uint32_t ns,
+        uint32_t md,
+        uint32_t cc,
+        uint32_t ns_addr,
+        uint32_t md_addr,
+        uint32_t cc_addr);
+void pll1_enable(void);
+void config_mdp_lut_clk(void);
+void mdp_clock_init(void);
+
 #endif
diff --git a/platform/msm8960/include/platform/iomap.h b/platform/msm8960/include/platform/iomap.h
index 864b950..0be00e5 100644
--- a/platform/msm8960/include/platform/iomap.h
+++ b/platform/msm8960/include/platform/iomap.h
@@ -85,4 +85,6 @@
 #define MSM_BOOT_PLL8_STATUS    (CLK_CTL_BASE + 0x3158)
 #define MSM_BOOT_PLL_ENABLE_SC0 (CLK_CTL_BASE + 0x34C0)
 
+#define MSM_MMSS_CLK_CTL_BASE 0x04000000
+
 #endif
diff --git a/platform/msm8960/platform.c b/platform/msm8960/platform.c
index 309b98e..95a61bc 100644
--- a/platform/msm8960/platform.c
+++ b/platform/msm8960/platform.c
@@ -32,10 +32,16 @@
 #include <reg.h>
 #include <platform/iomap.h>
 #include <uart_dm.h>
+#include <dev/fbcon.h>
 
 extern void platform_init_timer(void);
 extern void platform_init_interrupts(void);
-
+extern void mipi_panel_reset(void);
+extern void mipi_dsi_panel_power_on(void);
+extern void mdp_clock_init(void);
+extern void mmss_clock_init(void);
+extern struct fbcon_config *mipi_init(void);
+extern void mipi_dsi_shutdown(void);
 
 void platform_early_init(void)
 {
@@ -48,3 +54,21 @@
 {
     dprintf(INFO, "platform_init()\n");
 }
+
+void display_init(void){
+    struct fbcon_config *fb_cfg;
+
+    mipi_dsi_panel_power_on();
+    mipi_panel_reset();
+
+    mdp_clock_init();
+    mmss_clock_init();
+
+    fb_cfg = mipi_init();
+    fbcon_setup(fb_cfg);
+}
+
+void display_shutdown(void)
+{
+    mipi_dsi_shutdown();
+}
diff --git a/platform/msm_shared/include/mipi_dsi.h b/platform/msm_shared/include/mipi_dsi.h
index 19b880b..b882e7e 100644
--- a/platform/msm_shared/include/mipi_dsi.h
+++ b/platform/msm_shared/include/mipi_dsi.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2010, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2010-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
@@ -54,6 +54,7 @@
 #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 DSIPHY_TIMING_CTRL_0                  (0x04700260)
 #define DSIPHY_TIMING_CTRL_1                  (0x04700264)
@@ -98,7 +99,13 @@
 #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)
@@ -154,6 +161,7 @@
 #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)
@@ -166,6 +174,10 @@
 #define MIPI_DSI_MRPS       0x04 /* Maximum Return Packet Size */
 #define MIPI_DSI_REG_LEN    16   /* 4 x 4 bytes register */
 
+#define DTYPE_GEN_WRITE2 0x23 /* 4th Byte is 0x80 */
+#define DTYPE_GEN_LWRITE 0x29 /* 4th Byte is 0xc0 */
+#define DTYPE_DCS_WRITE1 0x15 /* 4th Byte is 0x80 */
+
 //BEGINNING OF Tochiba Config- video mode
 
 static const unsigned char toshiba_panel_mcap_off[8] = {
@@ -337,12 +349,205 @@
 }; /* 960 - 1 */
 /* End of Novatek Blue panel commands */
 
+/* Toshiba mdt61 panel cmds */
+static const unsigned char toshiba_mdt61_mcap_start[4] = {
+    0xB0, 0x04, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_num_out_pixelform[8] = {
+    0x03, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xB3, 0x00, 0x87, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_dsi_ctrl[8] = {
+    0x03, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xB6, 0x30, 0x83, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_panel_driving[12] = {
+    0x07, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xC0, 0x01, 0x00, 0x85,
+    0x00, 0x00, 0x00, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_dispV_timing[12] = {
+    0x05, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xC1, 0x00, 0x10, 0x00,
+    0x01, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_dispCtrl[8] = {
+    0x03, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xC3, 0x00, 0x19, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_test_mode_c4[4] = {
+    0xC4, 0x03, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_dispH_timing[20] = {
+    0x0F, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xC5, 0x00, 0x01, 0x05,
+    0x04, 0x5E, 0x00, 0x00,
+    0x00, 0x00, 0x0B, 0x17,
+    0x05, 0x00, 0x00, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_test_mode_c6[4] = {
+    0xC6, 0x00, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_gamma_setA[20] = {
+    0x0D, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xC8, 0x0A, 0x15, 0x18,
+    0x1B, 0x1C, 0x0D, 0x00,
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_gamma_setB[20] = {
+    0x0D, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xC9, 0x0D, 0x1D, 0x1F,
+    0x1F, 0x1F, 0x10, 0x00,
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_gamma_setC[20] = {
+    0x0D, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xCA, 0x1E, 0x1F, 0x1E,
+    0x1D, 0x1D, 0x10, 0x00,
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_powerSet_ChrgPmp[12] = {
+    0x05, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xD0, 0x02, 0x00, 0xA3,
+    0xB8, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_testMode_d1[12] = {
+    0x06, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xD1, 0x10, 0x14, 0x53,
+    0x64, 0x00, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_powerSet_SrcAmp[8] = {
+    0x03, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xD2, 0xB3, 0x00, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_powerInt_PS[8] = {
+    0x03, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xD3, 0x33, 0x03, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_vreg[4] = {
+    0xD5, 0x00, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_test_mode_d6[4] = {
+    0xD6, 0x01, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_timingCtrl_d7[16] = {
+    0x09, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xD7, 0x09, 0x00, 0x84,
+    0x81, 0x61, 0xBC, 0xB5,
+    0x05, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_timingCtrl_d8[12] = {
+    0x07, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xD8, 0x04, 0x25, 0x90,
+    0x4C, 0x92, 0x00, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_timingCtrl_d9[8] = {
+    0x04, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xD9, 0x5B, 0x7F, 0x05
+};
+
+static const unsigned char toshiba_mdt61_white_balance[12] = {
+    0x06, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xCB, 0x00, 0x00, 0x00,
+    0x1C, 0x00, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_vcs_settings[4] = {
+    0xDD, 0x53, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_vcom_dc_settings[4] = {
+    0xDE, 0x43, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_testMode_e3[12] = {
+    0x05, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xE3, 0x00, 0x00, 0x00,
+    0x00, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_testMode_e4[12] = {
+    0x06, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xE4, 0x00, 0x00, 0x22,
+    0xAA, 0x00, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_testMode_e5[4] = {
+    0xE5, 0x00, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_testMode_fa[8] = {
+    0x04, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xFA, 0x00, 0x00, 0x00
+};
+
+
+static const unsigned char toshiba_mdt61_testMode_fd[12] = {
+    0x05, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xFD, 0x00, 0x00, 0x00,
+    0x00, 0xFF, 0xFF, 0xFF
+};
+
+
+static const unsigned char toshiba_mdt61_testMode_fe[12] = {
+    0x05, 0x00, DTYPE_GEN_LWRITE, 0xC0,
+    0xFE, 0x00, 0x00, 0x00,
+    0x00, 0xFF, 0xFF, 0xFF
+};
+
+static const unsigned char toshiba_mdt61_mcap_end[4] = {
+    0xB0, 0x03, DTYPE_GEN_WRITE2, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_set_add_mode[4] = {
+    0x36, 0x00, DTYPE_DCS_WRITE1, 0x80,
+};
+
+static const unsigned char toshiba_mdt61_set_pixel_format[4] = {
+    0x3A, 0x70, DTYPE_DCS_WRITE1, 0x80,
+};
+
+/* Done Toshiba MDT61 Panel Commands */
+/* Toshiba MDT61 (R69320) End */
+
+static const unsigned char dsi_display_exit_sleep[4] =
+{
+0x11, 0x00, 0x15, 0x80,
+};
+
+static const unsigned char dsi_display_display_on[4] =
+{
+0x29, 0x00, 0x15, 0x80,
+};
 
 #define MIPI_VIDEO_MODE	        1
 #define MIPI_CMD_MODE           2
 
 struct mipi_dsi_phy_ctrl {
-    uint32_t regulator[4];
+    uint32_t regulator[5];
     uint32_t timing[12];
     uint32_t ctrl[4];
     uint32_t strength[4];
@@ -408,6 +613,60 @@
 #endif
 };
 
+
+static struct mipi_dsi_cmd toshiba_mdt61_video_mode_cmds[] = {
+	{sizeof(toshiba_mdt61_mcap_start), toshiba_mdt61_mcap_start},
+	{sizeof(toshiba_mdt61_num_out_pixelform),toshiba_mdt61_num_out_pixelform},
+	{sizeof(toshiba_mdt61_dsi_ctrl), toshiba_mdt61_dsi_ctrl},
+	{sizeof(toshiba_mdt61_panel_driving), toshiba_mdt61_panel_driving},
+	{sizeof(toshiba_mdt61_dispV_timing), toshiba_mdt61_dispV_timing},
+	{sizeof(toshiba_mdt61_dispCtrl), toshiba_mdt61_dispCtrl},
+	{sizeof(toshiba_mdt61_test_mode_c4), toshiba_mdt61_test_mode_c4},
+	{sizeof(toshiba_mdt61_dispH_timing), toshiba_mdt61_dispH_timing},
+	{sizeof(toshiba_mdt61_test_mode_c6), toshiba_mdt61_test_mode_c6},
+	{sizeof(toshiba_mdt61_gamma_setA), toshiba_mdt61_gamma_setA},
+	{sizeof(toshiba_mdt61_gamma_setB), toshiba_mdt61_gamma_setB},
+	{sizeof(toshiba_mdt61_gamma_setC), toshiba_mdt61_gamma_setC},
+	{sizeof(toshiba_mdt61_powerSet_ChrgPmp),toshiba_mdt61_powerSet_ChrgPmp},
+	{sizeof(toshiba_mdt61_testMode_d1), toshiba_mdt61_testMode_d1},
+	{sizeof(toshiba_mdt61_powerSet_SrcAmp),toshiba_mdt61_powerSet_SrcAmp},
+	{sizeof(toshiba_mdt61_powerInt_PS), toshiba_mdt61_powerInt_PS},
+	{sizeof(toshiba_mdt61_vreg), toshiba_mdt61_vreg},
+	{sizeof(toshiba_mdt61_test_mode_d6), toshiba_mdt61_test_mode_d6},
+	{sizeof(toshiba_mdt61_timingCtrl_d7), toshiba_mdt61_timingCtrl_d7},
+	{sizeof(toshiba_mdt61_timingCtrl_d8), toshiba_mdt61_timingCtrl_d8},
+	{sizeof(toshiba_mdt61_timingCtrl_d9), toshiba_mdt61_timingCtrl_d9},
+	{sizeof(toshiba_mdt61_white_balance), toshiba_mdt61_white_balance},
+	{sizeof(toshiba_mdt61_vcs_settings), toshiba_mdt61_vcs_settings},
+	{sizeof(toshiba_mdt61_vcom_dc_settings), toshiba_mdt61_vcom_dc_settings},
+	{sizeof(toshiba_mdt61_testMode_e3), toshiba_mdt61_testMode_e3},
+	{sizeof(toshiba_mdt61_testMode_e4), toshiba_mdt61_testMode_e4},
+	{sizeof(toshiba_mdt61_testMode_e5), toshiba_mdt61_testMode_e5},
+	{sizeof(toshiba_mdt61_testMode_fa), toshiba_mdt61_testMode_fa},
+	{sizeof(toshiba_mdt61_testMode_fd), toshiba_mdt61_testMode_fd},
+	{sizeof(toshiba_mdt61_testMode_fe), toshiba_mdt61_testMode_fe},
+	{sizeof(toshiba_mdt61_mcap_end), toshiba_mdt61_mcap_end},
+	{sizeof(toshiba_mdt61_set_add_mode), toshiba_mdt61_set_add_mode},
+	{sizeof(toshiba_mdt61_set_pixel_format), toshiba_mdt61_set_pixel_format},
+	{sizeof(dsi_display_exit_sleep), dsi_display_exit_sleep},
+	{sizeof(dsi_display_display_on), dsi_display_display_on},
+};
+
+static struct mipi_dsi_phy_ctrl mipi_dsi_toshiba_mdt61_panel_phy_ctrl = {
+	/* 600*1024, RGB888, 3 Lane 55 fps video mode */
+		{0x03, 0x0a, 0x04, 0x00, 0x20},
+		/* timing   */
+		{0xab, 0x8a, 0x18, 0x00, 0x92, 0x97, 0x1b, 0x8c,
+		 0x0c, 0x03, 0x04, 0xa0},
+		{0x5f, 0x00, 0x00, 0x10},	/* phy ctrl */
+		{0xff, 0x00, 0x06, 0x00},	/* strength */
+
+		/* pll control 1- 19 */
+		{0x01, 0x7f, 0x31, 0xda, 0x00, 0x40, 0x03, 0x62,
+		0x41, 0x0f, 0x01,
+		 0x00, 0x1a, 0x00, 0x00, 0x02, 0x00, 0x20, 0x00, 0x01, 0x00 },
+};
+
 static struct mipi_dsi_cmd novatek_panel_manufacture_id_cmd =
     {sizeof(novatek_panel_manufacture_id), novatek_panel_manufacture_id};
 
@@ -440,20 +699,4 @@
    0x05, 0x14, 0x03, 0x0, 0x0, 0x54, 0x06, 0x10, 0x04, 0x0},
 };
 
-struct mipi_dsi_panel_config toshiba_panel_info = {
-    .mode = MIPI_VIDEO_MODE,
-    .num_of_lanes = 1,
-    .dsi_phy_config = &mipi_dsi_toshiba_panel_phy_ctrl,
-    .panel_cmds = toshiba_panel_video_mode_cmds,
-    .num_of_panel_cmds = ARRAY_SIZE(toshiba_panel_video_mode_cmds),
-};
-
-struct mipi_dsi_panel_config novatek_panel_info = {
-    .mode = MIPI_CMD_MODE,
-    .num_of_lanes = 2,
-    .dsi_phy_config = &mipi_dsi_novatek_panel_phy_ctrl,
-    .panel_cmds = novatek_panel_cmd_mode_cmds,
-    .num_of_panel_cmds = ARRAY_SIZE(novatek_panel_cmd_mode_cmds),
-};
-
 #endif
diff --git a/platform/msm_shared/mipi_dsi.c b/platform/msm_shared/mipi_dsi.c
index 941eefc..ee878f9 100644
--- a/platform/msm_shared/mipi_dsi.c
+++ b/platform/msm_shared/mipi_dsi.c
@@ -33,8 +33,7 @@
 #include <dev/fbcon.h>
 #include <target/display.h>
 #include <stdlib.h>
-
-#define MIPI_FB_ADDR  0x43E00000
+#include <debug.h>
 
 #if DISPLAY_MIPI_PANEL_TOSHIBA
 static struct fbcon_config mipi_fb_cfg = {
@@ -46,6 +45,13 @@
     .update_start = NULL,
     .update_done = NULL,
 };
+struct mipi_dsi_panel_config toshiba_panel_info = {
+    .mode = MIPI_VIDEO_MODE,
+    .num_of_lanes = 1,
+    .dsi_phy_config = &mipi_dsi_toshiba_panel_phy_ctrl,
+    .panel_cmds = toshiba_panel_video_mode_cmds,
+    .num_of_panel_cmds = ARRAY_SIZE(toshiba_panel_video_mode_cmds),
+};
 #elif DISPLAY_MIPI_PANEL_NOVATEK_BLUE
 static struct fbcon_config mipi_fb_cfg = {
     .height = NOV_MIPI_FB_HEIGHT,
@@ -56,6 +62,30 @@
     .update_start = NULL,
     .update_done = NULL,
 };
+struct mipi_dsi_panel_config novatek_panel_info = {
+    .mode = MIPI_CMD_MODE,
+    .num_of_lanes = 2,
+    .dsi_phy_config = &mipi_dsi_novatek_panel_phy_ctrl,
+    .panel_cmds = novatek_panel_cmd_mode_cmds,
+    .num_of_panel_cmds = ARRAY_SIZE(novatek_panel_cmd_mode_cmds),
+};
+#elif DISPLAY_MIPI_PANEL_TOSHIBA_MDT61
+static struct fbcon_config mipi_fb_cfg = {
+    .height = TSH_MDT61_MIPI_FB_HEIGHT,
+    .width = TSH_MDT61_MIPI_FB_WIDTH,
+    .stride = TSH_MDT61_MIPI_FB_WIDTH,
+    .format = FB_FORMAT_RGB888,
+    .bpp = 24,
+    .update_start = NULL,
+    .update_done = NULL,
+};
+struct mipi_dsi_panel_config toshiba_mdt61_panel_info = {
+    .mode = MIPI_VIDEO_MODE,
+    .num_of_lanes = 3,
+    .dsi_phy_config = &mipi_dsi_toshiba_mdt61_panel_phy_ctrl,
+    .panel_cmds = toshiba_mdt61_video_mode_cmds,
+    .num_of_panel_cmds = ARRAY_SIZE(toshiba_mdt61_video_mode_cmds),
+};
 #else
 static struct fbcon_config mipi_fb_cfg = {
     .height = 0,
@@ -108,14 +138,13 @@
     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);
+                  MMSS_DSI_PIXEL_CC);
     secure_writel(secure_readl(MMSS_DSI_PIXEL_CC) | clk_en,
-		  MMSS_DSI_PIXEL_CC);
+                  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;
@@ -173,9 +202,10 @@
     return &toshiba_panel_info;
 #elif DISPLAY_MIPI_PANEL_NOVATEK_BLUE
     return &novatek_panel_info;
+#elif DISPLAY_MIPI_PANEL_TOSHIBA_MDT61
+    return &toshiba_mdt61_panel_info;
 #endif
     return NULL;
-
 }
 
 int dsi_cmd_dma_trigger_for_panel()
@@ -193,14 +223,14 @@
         count++;
         if (count > 0xffff) {
             status = FAIL;
-            printf("\n\nThis command mode dma test is failed");
+            dprintf(CRITICAL, "Panel CMD: command mode dma test failed\n");
             return status;
         }
     }
 
     writel((readl(DSI_INT_CTRL) | 0x01000001), DSI_INT_CTRL);
-    printf
-        ("\n\nThis command mode is tested successfully, continue on next command mode test");
+    dprintf
+        (SPEW, "Panel CMD: command mode dma tested successfully\n");
     return status;
 }
 
@@ -343,9 +373,10 @@
     writel(0x0001, DSI_SOFT_RESET);
     writel(0x0000, DSI_SOFT_RESET);
 
-    writel((0 << 16) | 0x3f, DSI_CLK_CTRL); // reg:0x118
+    writel((0 << 16) | 0x3f, DSI_CLK_CTRL);    /* Turn on all DSI Clks */
     writel(DMA_STREAM1 << 8 | 0x04, DSI_TRIG_CTRL); // reg 0x80 dma trigger: sw
     // trigger 0x4; dma stream1
+
     writel(0 << 30 | DLNx_EN << 4 | 0x105, DSI_CTRL);   // reg 0x00 for this
     // build
     writel(EMBED_MODE1 << 28 | POWER_MODE2 << 26
@@ -357,6 +388,7 @@
     return status;
 }
 
+//TODO: Clean up arguments being passed in not being used
 int config_dsi_video_mode(unsigned short disp_width, unsigned short disp_height,
                           unsigned short img_width, unsigned short img_height,
                           unsigned short hsync_porch0_fp,
@@ -396,13 +428,13 @@
     writel(0, DSI_ERR_INT_MASK0);
 
     DST_FORMAT = 0;             // RGB565
-    printf("\nDSI_Video_Mode - Dst Format: RGB565");
+    dprintf(SPEW, "DSI_Video_Mode - Dst Format: RGB565\n");
 
     DLNx_EN = 1;                // 1 lane with clk programming
-    printf("\nData Lane: 1 lane\n");
+    dprintf(SPEW, "Data Lane: 1 lane\n");
 
     TRAFIC_MODE = 0;            // non burst mode with sync pulses
-    printf("\nTraffic mode: non burst mode with sync pulses\n");
+    dprintf(SPEW, "Traffic mode: non burst mode with sync pulses\n");
 
     writel(0x02020202, DSI_INT_CTRL);
 
@@ -479,13 +511,13 @@
     // writel(0, DSI_ERR_INT_MASK0);
 
     DST_FORMAT = 8;             // RGB888
-    printf("\nDSI_Cmd_Mode - Dst Format: RGB888");
+    dprintf(SPEW, "DSI_Cmd_Mode - Dst Format: RGB888\n");
 
     DLNx_EN = 3;                // 2 lane with clk programming
-    printf("\nData Lane: 2 lane\n");
+    dprintf(SPEW, "Data Lane: 2 lane\n");
 
     TRAFIC_MODE = 0;            // non burst mode with sync pulses
-    printf("\nTraffic mode: non burst mode with sync pulses\n");
+    dprintf(SPEW, "Traffic mode: non burst mode with sync pulses\n");
 
     writel(0x02020202, DSI_INT_CTRL);
 
@@ -516,6 +548,40 @@
     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,
@@ -538,7 +604,7 @@
     unsigned long vsync_period;
     unsigned long vsync_period_intmd;
 
-    printf("\nHi setup MDP4.1 for DSI Video Mode\n");
+    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;
@@ -550,6 +616,7 @@
     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);
 
@@ -603,7 +670,7 @@
     unsigned short vsync_width = MIPI_VSYNC_PULSE_WIDTH;
     unsigned short dst_format = 0;
     unsigned short traffic_mode = 0;
-    unsigned short pack_pattern = 0x12;
+    unsigned short pack_pattern = 0x12; //BGR
     unsigned char ystride = 3;
 
     low_pwr_stop_mode = 0x1111; // low pwr mode bit16:HSA, bit20:HBA,
@@ -612,6 +679,14 @@
     eof_bllp_pwr = 0x9;         // low power stop mode or let cmd mode eng send
     // packets in hs or lp mode
 
+#if DISPLAY_MIPI_PANEL_TOSHIBA_MDT61
+    pack_pattern = 0x21; //RGB
+    config_mdt61_dsi_video_mode();
+
+    /* Two functions make up mdp_setup_dma_p_video_mode with mdt61 panel functions*/
+    mdp_setup_dma_p_video_config(pack_pattern, image_wd, image_ht, MIPI_FB_ADDR, image_wd, ystride);
+    mdp_setup_mdt61_video_dsi_config();
+#else
     status += config_dsi_video_mode(display_wd, display_ht, image_wd, image_ht,
                                     hsync_porch_fp, hsync_porch_bp,
                                     vsync_porch_fp, vsync_porch_bp, hsync_width,
@@ -624,6 +699,7 @@
                                    vsync_porch_fp, vsync_porch_bp, hsync_width,
                                    vsync_width, MIPI_FB_ADDR, image_wd,
                                    pack_pattern, ystride);
+#endif
 
     ReadValue = readl(DSI_INT_CTRL) & 0x00010000;
     while (ReadValue != 0x00010000) {
@@ -631,12 +707,12 @@
         count++;
         if (count > 0xffff) {
             status = FAIL;
-            printf("\nToshiba Video 565 pulse 1 lane test is failed\n");
+            dprintf(CRITICAL, "Toshiba Video lane test failed\n");
             return status;
         }
     }
 
-    printf("\nToshiba Video 565 pulse 1 lane is tested successfully \n");
+    dprintf(SPEW, "Toshiba Video lane tested successfully\n");
     return status;
 }
 
@@ -680,6 +756,7 @@
     return cmd_mode_status;
 }
 
+#if DISPLAY_MIPI_PANEL_NOVATEK_BLUE
 void mipi_dsi_cmd_mode_trigger(void)
 {
     int status = 0;
@@ -696,6 +773,7 @@
                         dst_format, traffic_mode,
                         panel_info->num_of_lanes /* num_of_lanes */ );
 }
+#endif
 
 void mipi_dsi_shutdown(void)
 {
@@ -708,20 +786,34 @@
     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);
+#else
     secure_writel(0x0, MMSS_DSI_CC);
     secure_writel(0x0, MMSS_DSI_PIXEL_CC);
+#endif
 }
 
 struct fbcon_config *mipi_init(void)
 {
     int status = 0;
     struct mipi_dsi_panel_config *panel_info = get_panel_info();
+    /* Enable MMSS_AHB_ARB_MATER_PORT_E for arbiter master0 and master 1 request */
     writel(0x00001800, MMSS_SFPB_GPREG);
+
+#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
+
     status += mipi_dsi_panel_initialize(panel_info);
+
 #if DISPLAY_MIPI_PANEL_NOVATEK_BLUE
     mipi_dsi_cmd_bta_sw_trigger();
     mipi_novatek_manufacture_id();
diff --git a/platform/msm_shared/rules.mk b/platform/msm_shared/rules.mk
index 728e564..39d5cb6 100644
--- a/platform/msm_shared/rules.mk
+++ b/platform/msm_shared/rules.mk
@@ -15,8 +15,6 @@
 	$(LOCAL_DIR)/jtag_hook.o \
 	$(LOCAL_DIR)/jtag.o \
 	$(LOCAL_DIR)/nand.o \
-	$(LOCAL_DIR)/lcdc.o \
-	$(LOCAL_DIR)/mddi.o \
 	$(LOCAL_DIR)/mmc.o \
 	$(LOCAL_DIR)/partition_parser.o
 
@@ -26,7 +24,9 @@
 	        $(LOCAL_DIR)/uart_dm.o \
 	        $(LOCAL_DIR)/crypto_eng.o \
 	        $(LOCAL_DIR)/crypto_hash.o \
-		$(LOCAL_DIR)/scm_decrypt.o
+		$(LOCAL_DIR)/scm_decrypt.o \
+		$(LOCAL_DIR)/lcdc.o \
+		$(LOCAL_DIR)/mddi.o
 endif
 
 ifeq ($(PLATFORM),msm8960)
@@ -37,17 +37,23 @@
 
 ifeq ($(PLATFORM),msm7x27a)
 	OBJS += $(LOCAL_DIR)/uart.o \
-		$(LOCAL_DIR)/proc_comm.o
+		$(LOCAL_DIR)/proc_comm.o \
+		$(LOCAL_DIR)/lcdc.o \
+		$(LOCAL_DIR)/mddi.o
 endif
 
 ifeq ($(PLATFORM),msm7k)
 	OBJS += $(LOCAL_DIR)/uart.o \
-		$(LOCAL_DIR)/proc_comm.o
+		$(LOCAL_DIR)/proc_comm.o \
+		$(LOCAL_DIR)/lcdc.o \
+		$(LOCAL_DIR)/mddi.o
 endif
 
 ifeq ($(PLATFORM),msm7x30)
 	OBJS += $(LOCAL_DIR)/crypto_eng.o \
 		$(LOCAL_DIR)/crypto_hash.o \
 		$(LOCAL_DIR)/uart.o \
-		$(LOCAL_DIR)/proc_comm.o
+		$(LOCAL_DIR)/proc_comm.o \
+		$(LOCAL_DIR)/lcdc.o \
+		$(LOCAL_DIR)/mddi.o
 endif