From fd964decbc83699d2e4fb7d76ba192640292dcb6 Mon Sep 17 00:00:00 2001
From: WeiYong Bi <bivvy.bi@rock-chips.com>
Date: Thu, 13 Jul 2017 16:19:08 +0800
Subject: [PATCH 1/1] phy: rockchip-inno-mipi-dphy: export PLL clock

Change-Id: I1eb96241930e380997dd66f3a3b479fe53a72a98
Signed-off-by: WeiYong Bi <bivvy.bi@rock-chips.com>
---
 .../phy/phy-rockchip-inno-mipi-dphy.txt       |  39 +-
 drivers/phy/phy-rockchip-inno-mipi-dphy.c     | 810 +++++++++---------
 2 files changed, 438 insertions(+), 411 deletions(-)

diff --git a/Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt b/Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt
index 7e596717c1b6..b33e59780f99 100644
--- a/Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt
+++ b/Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt
@@ -11,8 +11,9 @@ Required properties:
 	  configuration
 	- the "ref" clock is used to get the rate of the reference clock
 	  provided to the PHY module
- - rockchip,dsi-panel : phandle to MIPI DSI panel node, used to get the display
-			timing of the panel provided to the PHY module.
+ - clock-output-names: from common clock binding.
+	See ../clocks/clock-bindings.txt for details.
+ - #clock-cells : from common clock binding; shall be set to 0.
 
 Optional properties
  - resets : phandle to the reset of MIPI DSI PHY APB clock.
@@ -22,25 +23,25 @@ Example:
 
 For Rockchip RK3368
 
-mipi_dphy: mipi-dphy@ff968000 {
-	compatible = "rockchip,rk3368-mipi-dphy";
-	reg = <0x0 0xff968000 0x0 0x4000>;
-	#phy-cells = <0>;
-	clocks = <&cru SCLK_MIPIDSI_24M>, <&cru PCLK_DPHYTX0>;
-	clock-names = "ref", "pclk";
-	resets = <&cru SRST_MIPIDPHYTX>;
-	reset-names = "apb";
-	rockchip,dsi-panel = <&dsi_panel>;
-};
+	mipi_dphy: mipi-dphy@ff968000 {
+		compatible = "rockchip,rk3368-mipi-dphy";
+		reg = <0x0 0xff968000 0x0 0x4000>;
+		clocks = <&cru SCLK_MIPIDSI_24M>, <&cru PCLK_DPHYTX0>;
+		clock-names = "ref", "pclk";
+		clock-output-names = "mipi_dphy_pll";
+		#clock-cells = <0>;
+		resets = <&cru SRST_MIPIDPHYTX>;
+		reset-names = "apb";
+		#phy-cells = <0>;
+	};
 
 Then the PHY can be used in other nodes such as:
 
-mipi-dsi-host@ff960000 {
-	phys = <&mipi_dphy>;
-	phy-names = "mipi_dphy";
-
-	dsi_panel: panel {
-		dsi,lanes = 4;
+	dsi@ff960000 {
+		...
+		clocks = <&cru PCLK_MIPI_DSI0>, <&mipi_dphy>;
+		clock-names = "pclk", "hs_clk";
+		phys = <&mipi_dphy>;
+		phy-names = "mipi_dphy";
 		...
 	};
-};
diff --git a/drivers/phy/phy-rockchip-inno-mipi-dphy.c b/drivers/phy/phy-rockchip-inno-mipi-dphy.c
index ca878c001db6..c934364f523d 100644
--- a/drivers/phy/phy-rockchip-inno-mipi-dphy.c
+++ b/drivers/phy/phy-rockchip-inno-mipi-dphy.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2017 Rockchip Inc.
+ * Copyright (c) 2017 Rockchip Electronics Co. Ltd.
  *
  * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -11,37 +12,51 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/kernel.h>
 #include <linux/clk.h>
+#include <linux/clk-provider.h>
 #include <linux/delay.h>
 #include <linux/init.h>
-#include <linux/io.h>
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/phy/phy.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/reset.h>
+#include <linux/phy/phy.h>
 
-#include <drm/drm_mipi_dsi.h>
-
-#include <video/mipi_display.h>
-#include <video/of_videomode.h>
-#include <video/videomode.h>
-
-#define DRV_NAME	"inno-mipi-dphy"
+#define UPDATE(x, h, l)	(((x) << (l)) & GENMASK((h), (l)))
 
+/* Innosilicon MIPI D-PHY registers */
 #define INNO_PHY_LANE_CTRL	0x00000
 #define INNO_PHY_POWER_CTRL	0x00004
+#define ANALOG_RESET_MASK	BIT(2)
+#define ANALOG_RESET		BIT(2)
+#define ANALOG_NORMAL		0
+#define LDO_POWER_MASK		BIT(1)
+#define LDO_POWER_DOWN		BIT(1)
+#define LDO_POWER_ON		0
+#define PLL_POWER_MASK		BIT(0)
+#define PLL_POWER_DOWN		BIT(0)
+#define PLL_POWER_ON		0
 #define INNO_PHY_PLL_CTRL_0	0x0000c
+#define FBDIV_HI_MASK		BIT(5)
+#define FBDIV_HI(x)		UPDATE(x, 5, 5)
+#define PREDIV_MASK		GENMASK(4, 0)
+#define PREDIV(x)		UPDATE(x, 4, 0)
 #define INNO_PHY_PLL_CTRL_1	0x00010
+#define FBDIV_LO_MASK		GENMASK(7, 0)
+#define FBDIV_LO(x)		UPDATE(x, 7, 0)
 #define INNO_PHY_DIG_CTRL	0x00080
-#define INNO_PHY_PIN_CTRL	0x00084
+#define DIGITAL_RESET_MASK	BIT(0)
+#define DIGITAL_NORMAL		BIT(0)
+#define DIGITAL_RESET		0
 
 #define INNO_CLOCK_LANE_REG_BASE	0x00100
 #define INNO_DATA_LANE_0_REG_BASE	0x00180
 #define INNO_DATA_LANE_1_REG_BASE	0x00200
 #define INNO_DATA_LANE_2_REG_BASE	0x00280
 #define INNO_DATA_LANE_3_REG_BASE	0x00300
+#define INNO_MIPI_DPHY_MAX_REGISTER	(INNO_DATA_LANE_3_REG_BASE + \
+					 T_TA_WAIT_OFFSET)
 
 #define T_LPX_OFFSET		0x00014
 #define T_HS_PREPARE_OFFSET	0x00018
@@ -56,46 +71,30 @@
 #define T_TA_SURE_OFFSET	0x00044
 #define T_TA_WAIT_OFFSET	0x00048
 
-#define CLK_LANE_EN_MASK	BIT(6)
-#define DATA_LANE_3_EN_MASK	BIT(5)
-#define DATA_LANE_2_EN_MASK	BIT(4)
-#define DATA_LANE_1_EN_MASK	BIT(3)
-#define DATA_LANE_0_EN_MASK	BIT(2)
-#define CLK_LANE_EN		BIT(6)
-#define DATA_LANE_3_EN		BIT(5)
-#define DATA_LANE_2_EN		BIT(4)
-#define DATA_LANE_1_EN		BIT(3)
-#define DATA_LANE_0_EN		BIT(2)
-#define FBDIV_8_MASK		BIT(5)
-#define PREDIV_MASK		(0x1f << 0)
-#define FBDIV_7_0_MASK		(0xff << 0)
-#define FBDIV_8(x)		(((x) & 0x1) << 5)
-#define PREDIV(x)		(((x) & 0x1f) << 0)
-#define FBDIV_7_0(x)		(((x) & 0xff) << 0)
-#define T_LPX_MASK		(0x3f << 0)
-#define T_HS_PREPARE_MASK	(0x7f << 0)
-#define T_HS_ZERO_MASK		(0x3f << 0)
-#define T_HS_TRAIL_MASK		(0x7f << 0)
-#define T_HS_EXIT_MASK		(0x1f << 0)
-#define T_CLK_POST_MASK		(0xf << 0)
-#define T_WAKUP_H_MASK		(0x3 << 0)
-#define T_WAKUP_L_MASK		(0xff << 0)
-#define T_CLK_PRE_MASK		(0xf << 0)
-#define T_TA_GO_MASK		(0x3f << 0)
-#define T_TA_SURE_MASK		(0x3f << 0)
-#define T_TA_WAIT_MASK		(0x3f << 0)
-#define T_LPX(x)		(((x) & 0x3f) << 0)
-#define T_HS_PREPARE(x)		(((x) & 0x7f) << 0)
-#define T_HS_ZERO(x)		(((x) & 0x3f) << 0)
-#define T_HS_TRAIL(x)		(((x) & 0x7f) << 0)
-#define T_HS_EXIT(x)		(((x) & 0x1f) << 0)
-#define T_CLK_POST(x)		(((x) & 0xf) << 0)
-#define T_WAKUP_H(x)		(((x) & 0x3) << 0)
-#define T_WAKUP_L(x)		(((x) & 0xff) << 0)
-#define T_CLK_PRE(x)		(((x) & 0xf) << 0)
-#define T_TA_GO(x)		(((x) & 0x3f) << 0)
-#define T_TA_SURE(x)		(((x) & 0x3f) << 0)
-#define T_TA_WAIT(x)		(((x) & 0x3f) << 0)
+#define T_LPX_MASK		GENMASK(5, 0)
+#define T_LPX(x)		UPDATE(x, 5, 0)
+#define T_HS_PREPARE_MASK	GENMASK(6, 0)
+#define T_HS_PREPARE(x)		UPDATE(x, 6, 0)
+#define T_HS_ZERO_MASK		GENMASK(5, 0)
+#define T_HS_ZERO(x)		UPDATE(x, 5, 0)
+#define T_HS_TRAIL_MASK		GENMASK(6, 0)
+#define T_HS_TRAIL(x)		UPDATE(x, 6, 0)
+#define T_HS_EXIT_MASK		GENMASK(4, 0)
+#define T_HS_EXIT(x)		UPDATE(x, 4, 0)
+#define T_CLK_POST_MASK		GENMASK(3, 0)
+#define T_CLK_POST(x)		UPDATE(x, 3, 0)
+#define T_WAKUP_H_MASK		GENMASK(1, 0)
+#define T_WAKUP_H(x)		UPDATE(x, 1, 0)
+#define T_WAKUP_L_MASK		GENMASK(7, 0)
+#define T_WAKUP_L(x)		UPDATE(x, 7, 0)
+#define T_CLK_PRE_MASK		GENMASK(3, 0)
+#define T_CLK_PRE(x)		UPDATE(x, 3, 0)
+#define T_TA_GO_MASK		GENMASK(5, 0)
+#define T_TA_GO(x)		UPDATE(x, 5, 0)
+#define T_TA_SURE_MASK		GENMASK(5, 0)
+#define T_TA_SURE(x)		UPDATE(x, 5, 0)
+#define T_TA_WAIT_MASK		GENMASK(5, 0)
+#define T_TA_WAIT(x)		UPDATE(x, 5, 0)
 
 enum lane_type {
 	CLOCK_LANE,
@@ -105,14 +104,6 @@ enum lane_type {
 	DATA_LANE_3,
 };
 
-struct t_param {
-	u32 hs_clk_rate;
-	u8 t_hs_prepare;
-	u8 clock_lane_t_hs_zero;
-	u8 data_lane_t_hs_zero;
-	u8 t_hs_trail;
-};
-
 struct mipi_dphy_timing {
 	unsigned int clkmiss;
 	unsigned int clkpost;
@@ -139,79 +130,113 @@ struct mipi_dphy_timing {
 };
 
 struct inno_mipi_dphy_timing {
-	u8 t_lpx;
-	u8 t_hs_prepare;
-	u8 t_hs_zero;
-	u8 t_hs_trail;
-	u8 t_hs_exit;
-	u8 t_clk_post;
-	u8 t_wakup_h;
-	u8 t_wakup_l;
-	u8 t_clk_pre;
-	u8 t_ta_go;
-	u8 t_ta_sure;
-	u8 t_ta_wait;
-};
-
-struct dsi_panel {
-	struct videomode vm;
-	int bpp;
+	u8 lpx;
+	u8 hs_prepare;
+	u8 hs_zero;
+	u8 hs_trail;
+	u8 hs_exit;
+	u8 clk_post;
+	u8 wakup_h;
+	u8 wakup_l;
+	u8 clk_pre;
+	u8 ta_go;
+	u8 ta_sure;
+	u8 ta_wait;
 };
 
 struct inno_mipi_dphy {
 	struct device *dev;
-	struct phy *phy;
-	void __iomem *regs;
 	struct clk *ref_clk;
-	struct clk *pclk;
+	struct regmap *regmap;
 	struct reset_control *rst;
 
-	struct dsi_panel *panel;
-	u32 lanes;
-	u32 lane_mbps;
+	unsigned int lanes;
+	unsigned long lane_rate;
+
+	struct {
+		struct clk_hw hw;
+		u8 prediv;
+		u16 fbdiv;
+	} pll;
 };
 
 static const u32 lane_reg_offset[] = {
-	[CLOCK_LANE] = INNO_CLOCK_LANE_REG_BASE,
+	[CLOCK_LANE]  = INNO_CLOCK_LANE_REG_BASE,
 	[DATA_LANE_0] = INNO_DATA_LANE_0_REG_BASE,
 	[DATA_LANE_1] = INNO_DATA_LANE_1_REG_BASE,
 	[DATA_LANE_2] = INNO_DATA_LANE_2_REG_BASE,
 	[DATA_LANE_3] = INNO_DATA_LANE_3_REG_BASE,
 };
 
-static const struct t_param t_fixed_param_table[] = {
-	{ 110, 0x20, 0x16, 0x02, 0x22},
-	{ 150, 0x06, 0x16, 0x03, 0x45},
-	{ 200, 0x18, 0x17, 0x04, 0x0b},
-	{ 250, 0x05, 0x17, 0x05, 0x16},
-	{ 300, 0x51, 0x18, 0x06, 0x2c},
-	{ 400, 0x64, 0x19, 0x07, 0x33},
-	{ 500, 0x20, 0x1b, 0x07, 0x4e},
-	{ 600, 0x6a, 0x1d, 0x08, 0x3a},
-	{ 700, 0x3e, 0x1e, 0x08, 0x6a},
-	{ 800, 0x21, 0x1f, 0x09, 0x29},
-	{1000, 0x09, 0x20, 0x09, 0x27}
+#define FIXED_PARAM(_freq, _prepare, _clk_zero, _data_zero, _trail)	\
+{	\
+	.max_freq = _freq,	\
+	.hs_prepare = _prepare,	\
+	.clk_lane = {	\
+		.hs_zero = _clk_zero,	\
+	},	\
+	.data_lane = {	\
+		.hs_zero = _data_zero,	\
+	},	\
+	.hs_trail = _trail,	\
+}
+
+static const struct {
+	unsigned long max_freq;
+	u8 hs_prepare;
+	struct {
+		u8 hs_zero;
+	} clk_lane;
+	struct {
+		u8 hs_zero;
+	} data_lane;
+	u8 hs_trail;
+} fixed_param_table[] = {
+	FIXED_PARAM(110,  0x20, 0x16, 0x02, 0x22),
+	FIXED_PARAM(150,  0x06, 0x16, 0x03, 0x45),
+	FIXED_PARAM(200,  0x18, 0x17, 0x04, 0x0b),
+	FIXED_PARAM(250,  0x05, 0x17, 0x05, 0x16),
+	FIXED_PARAM(300,  0x51, 0x18, 0x06, 0x2c),
+	FIXED_PARAM(400,  0x64, 0x19, 0x07, 0x33),
+	FIXED_PARAM(500,  0x20, 0x1b, 0x07, 0x4e),
+	FIXED_PARAM(600,  0x6a, 0x1d, 0x08, 0x3a),
+	FIXED_PARAM(700,  0x3e, 0x1e, 0x08, 0x6a),
+	FIXED_PARAM(800,  0x21, 0x1f, 0x09, 0x29),
+	FIXED_PARAM(1000, 0x09, 0x20, 0x09, 0x27),
 };
 
-static inline void inno_write(struct inno_mipi_dphy *inno, u32 reg, u32 val)
+static inline struct inno_mipi_dphy *hw_to_inno(struct clk_hw *hw)
 {
-	writel_relaxed(val, inno->regs + reg);
+	return container_of(hw, struct inno_mipi_dphy, pll.hw);
 }
 
-static inline u32 inno_read(struct inno_mipi_dphy *inno, u32 reg)
+static inline void inno_mipi_dphy_reset(struct inno_mipi_dphy *inno)
 {
-	return readl_relaxed(inno->regs + reg);
+	/* Reset analog */
+	regmap_update_bits(inno->regmap, INNO_PHY_POWER_CTRL,
+			   ANALOG_RESET_MASK, ANALOG_RESET);
+	udelay(1);
+	regmap_update_bits(inno->regmap, INNO_PHY_POWER_CTRL,
+			   ANALOG_RESET_MASK, ANALOG_NORMAL);
+	/* Reset digital */
+	regmap_update_bits(inno->regmap, INNO_PHY_DIG_CTRL,
+			   DIGITAL_RESET_MASK, DIGITAL_RESET);
+	udelay(1);
+	regmap_update_bits(inno->regmap, INNO_PHY_DIG_CTRL,
+			   DIGITAL_RESET_MASK, DIGITAL_NORMAL);
 }
 
-static inline void inno_update_bits(struct inno_mipi_dphy *inno, u32 reg,
-				    u32 mask, u32 val)
+static inline void inno_mipi_dphy_lane_enable(struct inno_mipi_dphy *inno)
 {
-	u32 tmp, orig;
+	u8 map[] = {0x44, 0x4c, 0x5c, 0x7c};
 
-	orig = inno_read(inno, reg);
-	tmp = orig & ~mask;
-	tmp |= val & mask;
-	inno_write(inno, reg, tmp);
+	regmap_update_bits(inno->regmap, INNO_PHY_LANE_CTRL,
+			   0x7c, map[inno->lanes - 1]);
+}
+
+static inline void inno_mipi_dphy_lane_disable(struct inno_mipi_dphy *inno)
+{
+	regmap_update_bits(inno->regmap, INNO_PHY_LANE_CTRL, 0x7c, 0x00);
 }
 
 static void mipi_dphy_timing_get_default(struct mipi_dphy_timing *timing,
@@ -220,7 +245,7 @@ static void mipi_dphy_timing_get_default(struct mipi_dphy_timing *timing,
 	/* Global Operation Timing Parameters */
 	timing->clkmiss = 0;
 	timing->clkpost = 70 + 52 * period;
-	timing->clkpre = 8;
+	timing->clkpre = 8 * period;
 	timing->clkprepare = 65;
 	timing->clksettle = 95;
 	timing->clktermen = 0;
@@ -229,16 +254,16 @@ static void mipi_dphy_timing_get_default(struct mipi_dphy_timing *timing,
 	timing->dtermen = 0;
 	timing->eot = 0;
 	timing->hsexit = 120;
-	timing->hsprepare = 65 + 5 * period;
-	timing->hszero = 145 + 5 * period;
+	timing->hsprepare = 65 + 4 * period;
+	timing->hszero = 145 + 6 * period;
 	timing->hssettle = 85 + 6 * period;
 	timing->hsskip = 40;
-	timing->hstrail = max(4 * 8 * period, 60 + 4 * 4 * period);
+	timing->hstrail = max(8 * period, 60 + 4 * period);
 	timing->init = 100000;
 	timing->lpx = 60;
 	timing->taget = 5 * timing->lpx;
 	timing->tago = 4 * timing->lpx;
-	timing->tasure = timing->lpx;
+	timing->tasure = 2 * timing->lpx;
 	timing->wakeup = 1000000;
 }
 
@@ -247,85 +272,57 @@ static void inno_mipi_dphy_timing_update(struct inno_mipi_dphy *inno,
 					 struct inno_mipi_dphy_timing *t)
 {
 	u32 base = lane_reg_offset[lane_type];
-	u32 val, mask;
-
-	mask = T_HS_PREPARE_MASK;
-	val = T_HS_PREPARE(t->t_hs_prepare);
-	inno_update_bits(inno, base + T_HS_PREPARE_OFFSET, mask, val);
-
-	mask = T_HS_ZERO_MASK;
-	val = T_HS_ZERO(t->t_hs_zero);
-	inno_update_bits(inno, base + T_HS_ZERO_OFFSET, mask, val);
 
-	mask = T_HS_TRAIL_MASK;
-	val = T_HS_TRAIL(t->t_hs_trail);
-	inno_update_bits(inno, base + T_HS_TRAIL_OFFSET, mask, val);
-
-	mask = T_HS_EXIT_MASK;
-	val = T_HS_EXIT(t->t_hs_exit);
-	inno_update_bits(inno, base + T_HS_EXIT_OFFSET, mask, val);
+	regmap_update_bits(inno->regmap, base + T_HS_PREPARE_OFFSET,
+			   T_HS_PREPARE_MASK, T_HS_PREPARE(t->hs_prepare));
+	regmap_update_bits(inno->regmap, base + T_HS_ZERO_OFFSET,
+			   T_HS_ZERO_MASK, T_HS_ZERO(t->hs_zero));
+	regmap_update_bits(inno->regmap, base + T_HS_TRAIL_OFFSET,
+			   T_HS_TRAIL_MASK, T_HS_TRAIL(t->hs_trail));
+	regmap_update_bits(inno->regmap, base + T_HS_EXIT_OFFSET,
+			   T_HS_EXIT_MASK, T_HS_EXIT(t->hs_exit));
 
 	if (lane_type == CLOCK_LANE) {
-		mask = T_CLK_POST_MASK;
-		val = T_CLK_POST(t->t_clk_post);
-		inno_update_bits(inno, base + T_CLK_POST_OFFSET, mask, val);
-
-		mask = T_CLK_PRE_MASK;
-		val = T_CLK_PRE(t->t_clk_pre);
-		inno_update_bits(inno, base + T_CLK_PRE_OFFSET, mask, val);
+		regmap_update_bits(inno->regmap, base + T_CLK_POST_OFFSET,
+				   T_CLK_POST_MASK, T_CLK_POST(t->clk_post));
+		regmap_update_bits(inno->regmap, base + T_CLK_PRE_OFFSET,
+				   T_CLK_PRE_MASK, T_CLK_PRE(t->clk_pre));
 	}
 
-	mask = T_WAKUP_H_MASK;
-	val = T_WAKUP_H(t->t_wakup_h);
-	inno_update_bits(inno, base + T_WAKUP_H_OFFSET, mask, val);
-
-	mask = T_WAKUP_L_MASK;
-	val = T_WAKUP_L(t->t_wakup_l);
-	inno_update_bits(inno, base + T_WAKUP_L_OFFSET, mask, val);
-
-	mask = T_LPX_MASK;
-	val = T_LPX(t->t_lpx);
-	inno_update_bits(inno, base + T_LPX_OFFSET, mask, val);
-
-	mask = T_TA_GO_MASK;
-	val = T_TA_GO(t->t_ta_go);
-	inno_update_bits(inno, base + T_TA_GO_OFFSET, mask, val);
-
-	mask = T_TA_SURE_MASK;
-	val = T_TA_SURE(t->t_ta_sure);
-	inno_update_bits(inno, base + T_TA_SURE_OFFSET, mask, val);
-
-	mask = T_TA_WAIT_MASK;
-	val = T_TA_WAIT(t->t_ta_wait);
-	inno_update_bits(inno, base + T_TA_WAIT_OFFSET, mask, val);
+	regmap_update_bits(inno->regmap, base + T_WAKUP_H_OFFSET,
+			   T_WAKUP_H_MASK, T_WAKUP_H(t->wakup_h));
+	regmap_update_bits(inno->regmap, base + T_WAKUP_L_OFFSET,
+			   T_WAKUP_L_MASK, T_WAKUP_L(t->wakup_l));
+	regmap_update_bits(inno->regmap, base + T_LPX_OFFSET,
+			   T_LPX_MASK, T_LPX(t->lpx));
+	regmap_update_bits(inno->regmap, base + T_TA_GO_OFFSET,
+			   T_TA_GO_MASK, T_TA_GO(t->ta_go));
+	regmap_update_bits(inno->regmap, base + T_TA_SURE_OFFSET,
+			   T_TA_SURE_MASK, T_TA_SURE(t->ta_sure));
+	regmap_update_bits(inno->regmap, base + T_TA_WAIT_OFFSET,
+			   T_TA_WAIT_MASK, T_TA_WAIT(t->ta_wait));
 }
 
-static int inno_mipi_dphy_get_fixed_param(struct inno_mipi_dphy_timing *t,
-					  u32 hs_clk_rate,
-					  enum lane_type lane_type)
+static void inno_mipi_dphy_get_fixed_param(struct inno_mipi_dphy_timing *t,
+					   unsigned long freq,
+					   enum lane_type lane_type)
 {
-	const struct t_param *param;
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(t_fixed_param_table); i++) {
-		if (hs_clk_rate <= t_fixed_param_table[i].hs_clk_rate)
+	for (i = 0; i < ARRAY_SIZE(fixed_param_table); i++)
+		if (freq <= fixed_param_table[i].max_freq)
 			break;
-	}
-
-	if (i == ARRAY_SIZE(t_fixed_param_table))
-		return -EINVAL;
 
-	param = &t_fixed_param_table[i];
+	if (i == ARRAY_SIZE(fixed_param_table))
+		--i;
 
 	if (lane_type == CLOCK_LANE)
-		t->t_hs_zero = param->clock_lane_t_hs_zero;
+		t->hs_zero = fixed_param_table[i].clk_lane.hs_zero;
 	else
-		t->t_hs_zero = param->data_lane_t_hs_zero;
+		t->hs_zero = fixed_param_table[i].data_lane.hs_zero;
 
-	t->t_hs_prepare = param->t_hs_prepare;
-	t->t_hs_trail = param->t_hs_trail;
-
-	return 0;
+	t->hs_prepare = fixed_param_table[i].hs_prepare;
+	t->hs_trail = fixed_param_table[i].hs_trail;
 }
 
 static void inno_mipi_dphy_lane_timing_init(struct inno_mipi_dphy *inno,
@@ -333,96 +330,100 @@ static void inno_mipi_dphy_lane_timing_init(struct inno_mipi_dphy *inno,
 {
 	struct mipi_dphy_timing timing;
 	struct inno_mipi_dphy_timing data;
-	u32 txbyteclkhs = inno->lane_mbps / 8;	/* MHz */
-	u32 txclkesc = 20;	/* MHz */
-	u32 UI = DIV_ROUND_CLOSEST(NSEC_PER_USEC, inno->lane_mbps);	/* ns */
+	unsigned long txbyteclk, txclkesc, UI;
+	unsigned int esc_clk_div;
 
 	memset(&timing, 0, sizeof(timing));
 	memset(&data, 0, sizeof(data));
 
+	txbyteclk = inno->lane_rate / 8;
+	esc_clk_div = DIV_ROUND_UP(txbyteclk, 20000000);
+	txclkesc = txbyteclk / esc_clk_div;
+	UI = DIV_ROUND_CLOSEST_ULL(NSEC_PER_SEC, inno->lane_rate);
+
 	mipi_dphy_timing_get_default(&timing, UI);
-	inno_mipi_dphy_get_fixed_param(&data, inno->lane_mbps, lane_type);
-
-	/* txbyteclkhs domain */
-	data.t_hs_exit = DIV_ROUND_UP(txbyteclkhs * timing.hsexit,
-				      NSEC_PER_USEC);
-	data.t_clk_post = DIV_ROUND_UP(txbyteclkhs * timing.clkpost,
-				       NSEC_PER_USEC);
-	data.t_clk_pre = DIV_ROUND_UP(txbyteclkhs * timing.clkpre,
-				      NSEC_PER_USEC);
-	data.t_wakup_h = 0x3;
-	data.t_wakup_l = 0xff;
-	data.t_lpx = txbyteclkhs * timing.lpx / NSEC_PER_USEC;
-
-	/* txclkesc domain */
-	data.t_ta_go = DIV_ROUND_UP(txclkesc * timing.tago, NSEC_PER_USEC);
-	data.t_ta_sure = DIV_ROUND_UP(txclkesc * timing.tasure, NSEC_PER_USEC);
-	data.t_ta_wait = DIV_ROUND_UP(txclkesc * timing.taget, NSEC_PER_USEC);
+	inno_mipi_dphy_get_fixed_param(&data, inno->lane_rate / USEC_PER_SEC,
+				       lane_type);
+
+	data.hs_exit = DIV_ROUND_UP(timing.hsexit * txbyteclk, NSEC_PER_SEC);
+	data.clk_post = DIV_ROUND_UP(timing.clkpost * txbyteclk, NSEC_PER_SEC);
+	data.clk_pre = DIV_ROUND_UP(timing.clkpre * txbyteclk, NSEC_PER_SEC);
+	data.wakup_h = 0x3;
+	data.wakup_l = 0xff;
+	data.lpx = DIV_ROUND_UP(txbyteclk * timing.lpx, NSEC_PER_SEC);
+	if (data.lpx >= 2)
+		data.lpx -= 2;
+	data.ta_go = DIV_ROUND_UP(timing.tago * txclkesc, NSEC_PER_SEC);
+	data.ta_sure = DIV_ROUND_UP(timing.tasure * txclkesc, NSEC_PER_SEC);
+	data.ta_wait = DIV_ROUND_UP(timing.taget * txclkesc, NSEC_PER_SEC);
 
 	inno_mipi_dphy_timing_update(inno, lane_type, &data);
+
+#define TIMING_NS(x, freq) (((x) * (DIV_ROUND_CLOSEST(NSEC_PER_SEC, freq))))
+	dev_dbg(inno->dev, "hs-exit=%lu, clk-post=%lu, clk-pre=%lu, lpx=%lu\n",
+		TIMING_NS(data.hs_exit, txbyteclk),
+		TIMING_NS(data.clk_post, txbyteclk),
+		TIMING_NS(data.clk_pre, txbyteclk),
+		TIMING_NS(data.lpx + 2, txbyteclk));
+	dev_dbg(inno->dev, "ta-go=%lu, ta-sure=%lu, ta-wait=%lu\n",
+		TIMING_NS(data.ta_go, txclkesc),
+		TIMING_NS(data.ta_sure, txclkesc),
+		TIMING_NS(data.ta_wait, txclkesc));
 }
 
-static void inno_mipi_dphy_pll_init(struct inno_mipi_dphy *inno)
+static unsigned long inno_mipi_dphy_pll_rate_fixup(unsigned long fin,
+						   unsigned long rate,
+						   u8 *prediv, u16 *fbdiv)
 {
-	struct dsi_panel *panel = inno->panel;
-	unsigned int i, pre;
-	unsigned int mpclk, pllref, tmp;
-	unsigned int target_mbps = 1000;
-	unsigned int max_mbps = 1000;
-	u32 fbdiv = 1, prediv = 1;
-	u32 val, mask;
-
-	mpclk = DIV_ROUND_UP(panel->vm.pixelclock, USEC_PER_SEC);
-	if (mpclk) {
-		/* take 1 / 0.9, since mbps must big than bandwidth of RGB */
-		tmp = mpclk * (panel->bpp / inno->lanes) * 10 / 9;
-		if (tmp < max_mbps)
-			target_mbps = tmp;
-		else
-			dev_err(inno->dev, "DPHY clock frequency is out of range\n");
-	}
-
-	pllref = DIV_ROUND_UP(clk_get_rate(inno->ref_clk) / 2, USEC_PER_SEC);
-	tmp = pllref;
-
-	for (i = 1; i < 6; i++) {
-		if (pllref % i)
+	unsigned long best_freq = 0;
+	unsigned long fout;
+	u8 min_prediv, max_prediv;
+	u8 _prediv, uninitialized_var(best_prediv);
+	u16 _fbdiv, uninitialized_var(best_fbdiv);
+	u32 min_delta = UINT_MAX;
+
+	/*
+	 * The PLL output frequency can be calculated using a simple formula:
+	 * PLL_Output_Frequency = (FREF / PREDIV * FBDIV) / 2
+	 * PLL_Output_Frequency: it is equal to DDR-Clock-Frequency * 2
+	 */
+	fout = 2 * rate;
+
+	/* constraint: 5Mhz < Fref / prediv < 40MHz */
+	min_prediv = DIV_ROUND_UP(fin, 40000000);
+	max_prediv = fin / 5000000;
+
+	for (_prediv = min_prediv; _prediv <= max_prediv; _prediv++) {
+		u64 tmp;
+		u32 delta;
+
+		tmp = (u64)fout * _prediv;
+		do_div(tmp, fin);
+		_fbdiv = tmp;
+		/*
+		 * The all possible settings of feedback divider are
+		 * 12, 13, 14, 16, ~ 511
+		 */
+		if ((_fbdiv == 15) || (_fbdiv < 12) || (_fbdiv > 511))
 			continue;
-		pre = pllref / i;
-		if ((tmp > (target_mbps % pre)) && (target_mbps / pre < 512)) {
-			tmp = target_mbps % pre;
-			prediv = i;
-			fbdiv = target_mbps / pre;
+		tmp = (u64)_fbdiv * fin;
+		do_div(tmp, _prediv);
+
+		delta = abs(fout - tmp);
+		if (delta < min_delta) {
+			best_prediv = _prediv;
+			best_fbdiv = _fbdiv;
+			min_delta = delta;
+			best_freq = tmp;
 		}
-		if (tmp == 0)
-			break;
 	}
 
-	inno->lane_mbps = pllref / prediv * fbdiv;
-	phy_set_bus_width(inno->phy, inno->lane_mbps);
-
-	mask = FBDIV_8_MASK | PREDIV_MASK;
-	val = FBDIV_8(fbdiv >> 8) | PREDIV(prediv);
-	inno_update_bits(inno, INNO_PHY_PLL_CTRL_0, mask, val);
-
-	mask = FBDIV_7_0_MASK;
-	val = FBDIV_7_0(fbdiv);
-	inno_update_bits(inno, INNO_PHY_PLL_CTRL_1, mask, val);
-
-	dev_info(inno->dev, "fin=%d, target_mbps=%d, fout=%d, prediv=%d, fbdiv=%d\n",
-		 pllref, target_mbps, inno->lane_mbps, prediv, fbdiv);
-}
+	if (best_freq) {
+		*prediv = best_prediv;
+		*fbdiv = best_fbdiv;
+	}
 
-static void inno_mipi_dphy_reset(struct inno_mipi_dphy *inno)
-{
-	/* Reset analog */
-	inno_write(inno, INNO_PHY_POWER_CTRL, 0xe0);
-	udelay(10);
-	/* Reset digital */
-	inno_write(inno, INNO_PHY_DIG_CTRL, 0x1e);
-	udelay(10);
-	inno_write(inno, INNO_PHY_DIG_CTRL, 0x1f);
-	udelay(10);
+	return best_freq / 2;
 }
 
 static void inno_mipi_dphy_timing_init(struct inno_mipi_dphy *inno)
@@ -445,217 +446,240 @@ static void inno_mipi_dphy_timing_init(struct inno_mipi_dphy *inno)
 	}
 }
 
-static inline void inno_mipi_dphy_lane_enable(struct inno_mipi_dphy *inno)
+static int inno_mipi_dphy_power_on(struct phy *phy)
 {
-	u32 val = 0;
-	u32 mask = 0;
+	struct inno_mipi_dphy *inno = phy_get_drvdata(phy);
 
-	switch (inno->lanes) {
-	case 4:
-		mask |= DATA_LANE_3_EN_MASK;
-		val |= DATA_LANE_3_EN;
-		/* Fall through */
-	case 3:
-		mask |= DATA_LANE_2_EN_MASK;
-		val |= DATA_LANE_2_EN;
-		/* Fall through */
-	case 2:
-		mask |= DATA_LANE_1_EN_MASK;
-		val |= DATA_LANE_1_EN;
-		/* Fall through */
-	default:
-	case 1:
-		mask |= DATA_LANE_0_EN_MASK | CLK_LANE_EN_MASK;
-		val |= DATA_LANE_0_EN | CLK_LANE_EN;
-		break;
-	}
+	inno_mipi_dphy_lane_enable(inno);
+	inno_mipi_dphy_reset(inno);
+	inno_mipi_dphy_timing_init(inno);
+	udelay(1);
 
-	inno_update_bits(inno, INNO_PHY_LANE_CTRL, mask, val);
+	return 0;
 }
 
-static inline void inno_mipi_dphy_pll_ldo_enable(struct inno_mipi_dphy *inno)
+static int inno_mipi_dphy_power_off(struct phy *phy)
 {
-	inno_write(inno, INNO_PHY_POWER_CTRL, 0xe4);
-	udelay(10);
+	struct inno_mipi_dphy *inno = phy_get_drvdata(phy);
+
+	inno_mipi_dphy_lane_disable(inno);
+
+	return 0;
 }
 
-static int inno_mipi_dphy_power_on(struct phy *phy)
+static const struct phy_ops inno_mipi_dphy_ops = {
+	.power_on  = inno_mipi_dphy_power_on,
+	.power_off = inno_mipi_dphy_power_off,
+	.owner	   = THIS_MODULE,
+};
+
+static long inno_mipi_dphy_pll_clk_round_rate(struct clk_hw *hw,
+					      unsigned long rate,
+					      unsigned long *prate)
 {
-	struct inno_mipi_dphy *inno = phy_get_drvdata(phy);
+	struct inno_mipi_dphy *inno = hw_to_inno(hw);
+	unsigned long fin = *prate;
+	unsigned long fout;
+	u16 fbdiv;
+	u8 prediv;
 
-	clk_prepare_enable(inno->ref_clk);
-	clk_prepare_enable(inno->pclk);
+	fout = inno_mipi_dphy_pll_rate_fixup(fin, rate, &prediv, &fbdiv);
 
-	if (inno->rst) {
-		/* MIPI DSI PHY APB software reset request. */
-		reset_control_assert(inno->rst);
-		usleep_range(20, 40);
-		reset_control_deassert(inno->rst);
-	}
+	dev_dbg(inno->dev, "%s: fin=%lu, req_rate=%lu\n",
+		__func__, *prate, rate);
+	dev_dbg(inno->dev, "%s: fout=%lu, prediv=%u, fbdiv=%u\n",
+		__func__, fout, prediv, fbdiv);
 
-	inno_mipi_dphy_pll_init(inno);
-	inno_mipi_dphy_pll_ldo_enable(inno);
-	inno_mipi_dphy_lane_enable(inno);
-	inno_mipi_dphy_reset(inno);
-	inno_mipi_dphy_timing_init(inno);
-
-	dev_info(inno->dev, "Inno MIPI-DPHY Power-On\n");
+	inno->pll.prediv = prediv;
+	inno->pll.fbdiv = fbdiv;
 
-	return 0;
+	return fout;
 }
 
-static inline void inno_mipi_dphy_lane_disable(struct inno_mipi_dphy *inno)
+static int inno_mipi_dphy_pll_clk_set_rate(struct clk_hw *hw,
+					   unsigned long rate,
+					   unsigned long parent_rate)
 {
-	inno_update_bits(inno, INNO_PHY_LANE_CTRL, 0x7c, 0x00);
-}
+	struct inno_mipi_dphy *inno = hw_to_inno(hw);
 
-static inline void inno_mipi_dphy_pll_ldo_disable(struct inno_mipi_dphy *inno)
-{
-	inno_write(inno, INNO_PHY_POWER_CTRL, 0xe3);
-	udelay(10);
+	dev_dbg(inno->dev, "%s: rate: %lu Hz\n", __func__, rate);
+
+	inno->lane_rate = rate;
+
+	return 0;
 }
 
-static int inno_mipi_dphy_power_off(struct phy *phy)
+static unsigned long
+inno_mipi_dphy_pll_clk_recalc_rate(struct clk_hw *hw, unsigned long prate)
 {
-	struct inno_mipi_dphy *inno = phy_get_drvdata(phy);
+	struct inno_mipi_dphy *inno = hw_to_inno(hw);
 
-	inno_mipi_dphy_lane_disable(inno);
-	inno_mipi_dphy_pll_ldo_disable(inno);
+	dev_dbg(inno->dev, "%s: rate: %lu Hz\n", __func__, inno->lane_rate);
 
-	clk_disable_unprepare(inno->pclk);
-	clk_disable_unprepare(inno->ref_clk);
+	return inno->lane_rate;
+}
 
-	dev_info(inno->dev, "Inno MIPI-DPHY Power-Off\n");
+static int inno_mipi_dphy_pll_clk_enable(struct clk_hw *hw)
+{
+	struct inno_mipi_dphy *inno = hw_to_inno(hw);
+
+	regmap_update_bits(inno->regmap, INNO_PHY_PLL_CTRL_0, FBDIV_HI_MASK |
+			   PREDIV_MASK, FBDIV_HI(inno->pll.fbdiv >> 8) |
+			   PREDIV(inno->pll.prediv));
+	regmap_update_bits(inno->regmap, INNO_PHY_PLL_CTRL_1,
+			   FBDIV_LO_MASK, FBDIV_LO(inno->pll.fbdiv));
+	regmap_update_bits(inno->regmap, INNO_PHY_POWER_CTRL,
+			   PLL_POWER_MASK | LDO_POWER_MASK,
+			   PLL_POWER_ON | LDO_POWER_ON);
 
 	return 0;
 }
 
-static const struct phy_ops inno_mipi_dphy_ops = {
-	.power_on = inno_mipi_dphy_power_on,
-	.power_off = inno_mipi_dphy_power_off,
-	.owner = THIS_MODULE,
-};
-
-static int get_bpp(struct device_node *np)
+static void inno_mipi_dphy_pll_clk_disable(struct clk_hw *hw)
 {
-	u32 format = 0;
-
-	if (of_property_read_u32(np, "dsi,format", &format))
-		return 24;
-
-	switch (format) {
-	case MIPI_DSI_FMT_RGB666_PACKED:
-		return 18;
-	case MIPI_DSI_FMT_RGB565:
-		return 16;
-	case MIPI_DSI_FMT_RGB888:
-	case MIPI_DSI_FMT_RGB666:
-	default:
-		return 24;
-	}
+	struct inno_mipi_dphy *inno = hw_to_inno(hw);
+
+	regmap_update_bits(inno->regmap, INNO_PHY_POWER_CTRL,
+			   PLL_POWER_MASK | LDO_POWER_MASK,
+			   PLL_POWER_DOWN | LDO_POWER_DOWN);
 }
 
-static int inno_mipi_dphy_parse_dt(struct device_node *np,
-				   struct inno_mipi_dphy *inno)
+static const struct clk_ops inno_mipi_dphy_pll_clk_ops = {
+	.enable = inno_mipi_dphy_pll_clk_enable,
+	.disable = inno_mipi_dphy_pll_clk_disable,
+	.round_rate = inno_mipi_dphy_pll_clk_round_rate,
+	.set_rate = inno_mipi_dphy_pll_clk_set_rate,
+	.recalc_rate = inno_mipi_dphy_pll_clk_recalc_rate,
+};
+
+static int inno_mipi_dphy_pll_register(struct inno_mipi_dphy *inno)
 {
-	struct device_node *panel_node;
-	struct dsi_panel *panel;
+	struct device *dev = inno->dev;
+	struct device_node *np = dev->of_node;
+	struct clk *clk;
+	const char *parent_name;
+	struct clk_init_data init;
 	int ret;
 
-	panel_node = of_parse_phandle(np, "rockchip,dsi-panel", 0);
-	if (!panel_node) {
-		dev_err(inno->dev, "Missing 'rockchip,dsi-panel' property");
-		return -ENODEV;
-	}
+	parent_name = __clk_get_name(inno->ref_clk);
 
-	panel = devm_kzalloc(inno->dev, sizeof(*panel), GFP_KERNEL);
-	if (!panel) {
-		ret = -ENOMEM;
-		goto put_panel_node;
+	ret = of_property_read_string(np, "clock-output-names", &init.name);
+	if (ret < 0) {
+		dev_err(dev, "Missing clock-output-names property: %d\n", ret);
+		return ret;
 	}
 
-	ret = of_get_videomode(panel_node, &panel->vm, 0);
-	if (ret < 0)
-		goto put_panel_node;
-
-	panel->bpp = get_bpp(panel_node);
+	init.ops = &inno_mipi_dphy_pll_clk_ops;
+	init.parent_names = (const char * const *)&parent_name;
+	init.num_parents = 1;
+	init.flags = 0;
 
-	if (of_property_read_u32(panel_node, "dsi,lanes", &inno->lanes))
-		inno->lanes = 4;
+	inno->pll.hw.init = &init;
+	clk = devm_clk_register(dev, &inno->pll.hw);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		dev_err(dev, "failed to register PLL: %d\n", ret);
+		return ret;
+	}
 
-	of_node_put(panel_node);
+	return of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
 
-	inno->panel = panel;
+static int inno_mipi_dphy_parse_dt(struct device_node *np,
+				   struct inno_mipi_dphy *inno)
+{
+	if (of_property_read_u32(np, "inno,lanes", &inno->lanes))
+		inno->lanes = 4;
 
 	return 0;
-
-put_panel_node:
-	of_node_put(panel_node);
-	return ret;
 }
 
+static struct regmap_config inno_mipi_dphy_regmap_cfg = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+	.max_register = INNO_MIPI_DPHY_MAX_REGISTER,
+};
+
 static int inno_mipi_dphy_probe(struct platform_device *pdev)
 {
-	struct device_node *np = pdev->dev.of_node;
+	struct device *dev = &pdev->dev;
 	struct inno_mipi_dphy *inno;
 	struct phy_provider *phy_provider;
+	struct phy *phy;
 	struct resource *res;
+	void __iomem *regs;
 	int ret;
 
-	inno = devm_kzalloc(&pdev->dev, sizeof(*inno), GFP_KERNEL);
+	inno = devm_kzalloc(dev, sizeof(*inno), GFP_KERNEL);
 	if (!inno)
 		return -ENOMEM;
 
-	inno->dev = &pdev->dev;
+	inno->dev = dev;
 	platform_set_drvdata(pdev, inno);
 
-	ret = inno_mipi_dphy_parse_dt(np, inno);
+	ret = inno_mipi_dphy_parse_dt(dev->of_node, inno);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to parse DT\n");
+		dev_err(dev, "failed to parse DT\n");
 		return ret;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	inno->regs = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(inno->regs))
-		return PTR_ERR(inno->regs);
+	regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(regs))
+		return PTR_ERR(regs);
+
+	inno->regmap = devm_regmap_init_mmio_clk(dev, "pclk", regs,
+						 &inno_mipi_dphy_regmap_cfg);
+	if (IS_ERR(inno->regmap)) {
+		ret = PTR_ERR(inno->regmap);
+		dev_err(dev, "failed to init regmap: %d\n", ret);
+		return ret;
+	}
 
-	inno->ref_clk = devm_clk_get(&pdev->dev, "ref");
+	inno->ref_clk = devm_clk_get(dev, "ref");
 	if (IS_ERR(inno->ref_clk)) {
-		dev_err(&pdev->dev, "failed to get mipi dphy ref clk\n");
+		dev_err(dev, "failed to get reference clock\n");
 		return PTR_ERR(inno->ref_clk);
 	}
 
-	clk_set_rate(inno->ref_clk, 24000000);
-
-	inno->pclk = devm_clk_get(&pdev->dev, "pclk");
-	if (IS_ERR(inno->pclk)) {
-		dev_err(&pdev->dev, "failed to get mipi dphy pclk\n");
-		return PTR_ERR(inno->pclk);
-	}
-
-	inno->rst = devm_reset_control_get_optional(&pdev->dev, "apb");
+	inno->rst = devm_reset_control_get(dev, "apb");
 	if (IS_ERR(inno->rst)) {
-		dev_info(&pdev->dev, "No reset control specified\n");
-		inno->rst = NULL;
+		dev_err(dev, "failed to get system reset control\n");
+		return PTR_ERR(inno->rst);
 	}
 
-	inno->phy = devm_phy_create(&pdev->dev, NULL, &inno_mipi_dphy_ops);
-	if (IS_ERR(inno->phy)) {
-		dev_err(&pdev->dev, "failed to create MIPI D-PHY\n");
-		return PTR_ERR(inno->phy);
+	phy = devm_phy_create(dev, NULL, &inno_mipi_dphy_ops);
+	if (IS_ERR(phy)) {
+		dev_err(dev, "failed to create MIPI D-PHY\n");
+		return PTR_ERR(phy);
 	}
 
-	phy_set_drvdata(inno->phy, inno);
+	phy_set_drvdata(phy, inno);
 
-	phy_provider = devm_of_phy_provider_register(&pdev->dev,
-						     of_phy_simple_xlate);
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
 	if (IS_ERR(phy_provider)) {
-		dev_err(&pdev->dev, "failed to register phy provider\n");
+		dev_err(dev, "failed to register phy provider\n");
 		return PTR_ERR(phy_provider);
 	}
 
-	dev_info(&pdev->dev, "Inno MIPI-DPHY Driver Probe\n");
+	clk_prepare_enable(inno->ref_clk);
+
+	ret = inno_mipi_dphy_pll_register(inno);
+	if (ret) {
+		clk_disable_unprepare(inno->ref_clk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int inno_mipi_dphy_remove(struct platform_device *pdev)
+{
+	struct inno_mipi_dphy *inno = platform_get_drvdata(pdev);
+
+	of_clk_del_provider(inno->dev->of_node);
+	clk_disable_unprepare(inno->ref_clk);
 
 	return 0;
 }
@@ -668,14 +692,16 @@ static const struct of_device_id inno_mipi_dphy_of_match[] = {
 MODULE_DEVICE_TABLE(of, inno_mipi_dphy_of_match);
 
 static struct platform_driver inno_mipi_dphy_driver = {
-	.probe	= inno_mipi_dphy_probe,
 	.driver = {
-		.name	= DRV_NAME,
-		.of_match_table	= inno_mipi_dphy_of_match,
-	}
+		.name = "inno-mipi-dphy",
+		.of_match_table	= of_match_ptr(inno_mipi_dphy_of_match),
+	},
+	.probe	= inno_mipi_dphy_probe,
+	.remove = inno_mipi_dphy_remove,
 };
 
 module_platform_driver(inno_mipi_dphy_driver);
 
+MODULE_AUTHOR("Wyon Bi <bivvy.bi@rock-chips.com>");
 MODULE_DESCRIPTION("Innosilicon MIPI D-PHY Driver");
 MODULE_LICENSE("GPL v2");
-- 
2.34.1