2 files changed, 56 insertions, 5 deletions

diff --git a/arch/arm/mach-shmobile/board-ag5evm.c b/arch/arm/mach-shmobile/board-ag5evm.c
index 6993844424c6..15072c51e035 100644
--- a/arch/arm/mach-shmobile/board-ag5evm.c
+++ b/arch/arm/mach-shmobile/board-ag5evm.c
@@ -321,6 +321,36 @@ static struct resource mipidsi0_resources[] = {
 	},
 };
 
+#define DSI0PHYCR	0xe615006c
+static int sh_mipi_set_dot_clock(struct platform_device *pdev,
+				 void __iomem *base,
+				 int enable)
+{
+	struct clk *pck;
+	int ret;
+
+	pck = clk_get(&pdev->dev, "dsip_clk");
+	if (IS_ERR(pck)) {
+		ret = PTR_ERR(pck);
+		goto sh_mipi_set_dot_clock_pck_err;
+	}
+
+	if (enable) {
+		clk_set_rate(pck, clk_round_rate(pck,  24000000));
+		__raw_writel(0x2a809010, DSI0PHYCR);
+		clk_enable(pck);
+	} else {
+		clk_disable(pck);
+	}
+
+	ret = 0;
+
+	clk_put(pck);
+
+sh_mipi_set_dot_clock_pck_err:
+	return ret;
+}
+
 static struct sh_mipi_dsi_info mipidsi0_info = {
 	.data_format	= MIPI_RGB888,
 	.lcd_chan	= &lcdc0_info.ch[0],
@@ -329,6 +359,7 @@ static struct sh_mipi_dsi_info mipidsi0_info = {
 	.clksrc		= 1,
 	.flags		= SH_MIPI_DSI_HSABM |
 			  SH_MIPI_DSI_SYNC_PULSES_MODE,
+	.set_dot_clock	= sh_mipi_set_dot_clock,
 };
 
 static struct platform_device mipidsi0_device = {
@@ -476,8 +507,6 @@ static void __init ag5evm_map_io(void)
 	shmobile_setup_console();
 }
 
-#define DSI0PHYCR	0xe615006c
-
 static void __init ag5evm_init(void)
 {
 	sh73a0_pinmux_init();
@@ -558,9 +587,6 @@ static void __init ag5evm_init(void)
 	gpio_direction_output(GPIO_PORT235, 0);
 	lcd_backlight_reset();
 
-	/* MIPI-DSI clock setup */
-	__raw_writel(0x2a809010, DSI0PHYCR);
-
 	/* enable SDHI0 on CN15 [SD I/F] */
 	gpio_request(GPIO_FN_SDHICD0, NULL);
 	gpio_request(GPIO_FN_SDHIWP0, NULL);
diff --git a/arch/arm/mach-shmobile/board-ap4evb.c b/arch/arm/mach-shmobile/board-ap4evb.c
index b2e32c8c9e54..73503ed8bde2 100644
--- a/arch/arm/mach-shmobile/board-ap4evb.c
+++ b/arch/arm/mach-shmobile/board-ap4evb.c
@@ -564,6 +564,30 @@ static struct platform_device keysc_device = {
 };
 
 /* MIPI-DSI */
+#define PHYCTRL		0x0070
+static int sh_mipi_set_dot_clock(struct platform_device *pdev,
+				 void __iomem *base,
+				 int enable)
+{
+	struct clk *pck = clk_get(&pdev->dev, "dsip_clk");
+	void __iomem *phy =  base + PHYCTRL;
+
+	if (IS_ERR(pck))
+		return PTR_ERR(pck);
+
+	if (enable) {
+		clk_set_rate(pck, clk_round_rate(pck, 24000000));
+		iowrite32(ioread32(phy) | (0xb << 8), phy);
+		clk_enable(pck);
+	} else {
+		clk_disable(pck);
+	}
+
+	clk_put(pck);
+
+	return 0;
+}
+
 static struct resource mipidsi0_resources[] = {
 	[0] = {
 		.start  = 0xffc60000,
@@ -583,6 +607,7 @@ static struct sh_mipi_dsi_info mipidsi0_info = {
 	.lane		= 2,
 	.vsynw_offset	= 17,
 	.flags		= SH_MIPI_DSI_SYNC_PULSES_MODE,
+	.set_dot_clock	= sh_mipi_set_dot_clock,
 };
 
 static struct platform_device mipidsi0_device = {