From 35a788ded3c4c8f0f57b2d19a13045b4ee737fd0 Mon Sep 17 00:00:00 2001
From: WeiYong Bi <bivvy.bi@rock-chips.com>
Date: Tue, 14 Mar 2017 17:25:27 +0800
Subject: [PATCH] phy: Add support for INNO MIPI D-PHY

The INNO MIPI D-PHY is built in witch a standard digital interface
to talk to any third part Host controller.That is part of Rockchip SoCs,
like rk3368.

Change-Id: I9806882e0e3fb6b20348015d0f34923d1bc46b89
Signed-off-by: WeiYong Bi <bivvy.bi@rock-chips.com>
---
 .../phy/phy-rockchip-inno-mipi-dphy.txt       |  38 +
 drivers/phy/Kconfig                           |   7 +
 drivers/phy/Makefile                          |   1 +
 drivers/phy/phy-rockchip-inno-mipi-dphy.c     | 739 ++++++++++++++++++
 4 files changed, 785 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt
 create mode 100644 drivers/phy/phy-rockchip-inno-mipi-dphy.c

diff --git a/Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt b/Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt
new file mode 100644
index 000000000000..238e3405f8eb
--- /dev/null
+++ b/Documentation/devicetree/bindings/phy/phy-rockchip-inno-mipi-dphy.txt
@@ -0,0 +1,38 @@
+ROCKCHIP MIPI DPHY WITH INNO IP BLOCK
+
+Required properties:
+ - compatible : should be "rockchip,rk3368-mipi-dphy";
+ - reg : the address offset of register for mipi-dphy configuration.
+ - #phy-cells : must be 0. See ./phy-bindings.txt for details.
+ - clocks and clock-names:
+	- the "pclk" clock is required by the phy module, used to register
+	  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.
+
+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";
+	rockchip,dsi-panel = <&dsi_panel>;
+};
+
+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;
+		...
+	};
+};
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index d1c72214f6e7..b09bec07db4b 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -352,6 +352,13 @@ config PHY_ROCKCHIP_DP
 	help
 	  Enable this to support the Rockchip Display Port PHY.
 
+config PHY_ROCKCHIP_INNO_MIPI_DPHY
+	tristate "Rockchip INNO MIPI D-PHY Driver"
+	depends on ARCH_ROCKCHIP && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the Rockchip MIPI D-PHY with Innosilicon IP block.
+
 config PHY_ROCKCHIP_TYPEC
 	tristate "Rockchip TYPEC PHY Driver"
 	depends on ARCH_ROCKCHIP && OF
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index 518551c85cc0..1cfe7b7049ca 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -37,6 +37,7 @@ obj-$(CONFIG_PHY_QCOM_APQ8064_SATA)	+= phy-qcom-apq8064-sata.o
 obj-$(CONFIG_PHY_ROCKCHIP_USB) += phy-rockchip-usb.o
 obj-$(CONFIG_PHY_ROCKCHIP_INNO_USB2)	+= phy-rockchip-inno-usb2.o
 obj-$(CONFIG_PHY_ROCKCHIP_INNO_USB3)	+= phy-rockchip-inno-usb3.o
+obj-$(CONFIG_PHY_ROCKCHIP_INNO_MIPI_DPHY)	+= phy-rockchip-inno-mipi-dphy.o
 obj-$(CONFIG_PHY_ROCKCHIP_EMMC) += phy-rockchip-emmc.o
 obj-$(CONFIG_PHY_ROCKCHIP_DP)		+= phy-rockchip-dp.o
 obj-$(CONFIG_PHY_ROCKCHIP_TYPEC)	+= phy-rockchip-typec.o
diff --git a/drivers/phy/phy-rockchip-inno-mipi-dphy.c b/drivers/phy/phy-rockchip-inno-mipi-dphy.c
new file mode 100644
index 000000000000..9a3a670d3154
--- /dev/null
+++ b/drivers/phy/phy-rockchip-inno-mipi-dphy.c
@@ -0,0 +1,739 @@
+/*
+ * Copyright (c) 2017 Rockchip Inc.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.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 <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 INNO_PHY_LANE_CTRL	0x00000
+#define INNO_PHY_POWER_CTRL	0x00004
+#define INNO_PHY_PLL_CTRL_0	0x0000c
+#define INNO_PHY_PLL_CTRL_1	0x00010
+#define INNO_PHY_DIG_CTRL	0x00080
+#define INNO_PHY_PIN_CTRL	0x00084
+
+#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 T_LPX_OFFSET		0x00014
+#define T_HS_PREPARE_OFFSET	0x00018
+#define T_HS_ZERO_OFFSET	0x0001c
+#define T_HS_TRAIL_OFFSET	0x00020
+#define T_HS_EXIT_OFFSET	0x00024
+#define T_CLK_POST_OFFSET	0x00028
+#define T_WAKUP_H_OFFSET	0x00030
+#define T_WAKUP_L_OFFSET	0x00034
+#define T_CLK_PRE_OFFSET	0x00038
+#define T_TA_GO_OFFSET		0x00040
+#define T_TA_SURE_OFFSET	0x00044
+#define T_TA_WAIT_OFFSET	0x00048
+
+/* INNO_PHY_LANE_CTRL */
+#define M_CLK_LANE_EN		BIT(6)
+#define M_DATA_LANE_3_EN	BIT(5)
+#define M_DATA_LANE_2_EN	BIT(4)
+#define M_DATA_LANE_1_EN	BIT(3)
+#define M_DATA_LANE_0_EN	BIT(2)
+#define V_CLK_LANE_EN		BIT(6)
+#define V_DATA_LANE_3_EN	BIT(5)
+#define V_DATA_LANE_2_EN	BIT(4)
+#define V_DATA_LANE_1_EN	BIT(3)
+#define V_DATA_LANE_0_EN	BIT(2)
+/* INNO_PHY_PLL_CTRL_0 */
+#define M_FBDIV_8		BIT(5)
+#define M_PREDIV		(0x1f << 0)
+#define V_FBDIV_8(x)		((x) << 5)
+#define V_PREDIV(x)		((x) << 0)
+/* INNO_PHY_PLL_CTRL_1 */
+#define M_FBDIV_7_0		(0xff << 0)
+#define V_FBDIV_7_0(x)		((x) << 0)
+
+#define M_T_LPX			(0x3f << 0)
+#define V_T_LPX(x)		((x) << 0)
+#define M_T_HS_PREPARE		(0x7f << 0)
+#define V_T_HS_PREPARE(x)	((x) << 0)
+#define M_T_HS_ZERO		(0x3f << 0)
+#define V_T_HS_ZERO(x)		((x) << 0)
+#define M_T_HS_TRAIL		(0x7f << 0)
+#define V_T_HS_TRAIL(x)		((x) << 0)
+#define M_T_HS_EXIT		(0x1f << 0)
+#define V_T_HS_EXIT(x)		((x) << 0)
+#define M_T_CLK_POST		(0xf << 0)
+#define V_T_CLK_POST(x)		((x) << 0)
+#define M_T_WAKUP_H		(0x3 << 0)
+#define V_T_WAKUP_H(x)		((x) << 0)
+#define M_T_WAKUP_L		(0xff << 0)
+#define V_T_WAKUP_L(x)		((x) << 0)
+#define M_T_CLK_PRE		(0xf << 0)
+#define V_T_CLK_PRE(x)		((x) << 0)
+#define M_T_TA_GO		(0x3f << 0)
+#define V_T_TA_GO(x)		((x) << 0)
+#define M_T_TA_SURE		(0x3f << 0)
+#define V_T_TA_SURE(x)		((x) << 0)
+#define M_T_TA_WAIT		(0x3f << 0)
+#define V_T_TA_WAIT(x)		((x) << 0)
+
+enum lane_type {
+	CLOCK_LANE,
+	DATA_LANE_0,
+	DATA_LANE_1,
+	DATA_LANE_2,
+	DATA_LANE_3,
+};
+
+static const u32 lane_reg_offset[] = {
+	[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,
+};
+
+enum hs_clk_range {
+	HS_CLK_RANGE_80_110_MHZ,
+	HS_CLK_RANGE_110_150_MHZ,
+	HS_CLK_RANGE_150_200_MHZ,
+	HS_CLK_RANGE_200_250_MHZ,
+	HS_CLK_RANGE_250_300_MHZ,
+	HS_CLK_RANGE_300_400_MHZ,
+	HS_CLK_RANGE_400_500_MHZ,
+	HS_CLK_RANGE_500_600_MHZ,
+	HS_CLK_RANGE_600_700_MHZ,
+	HS_CLK_RANGE_700_800_MHZ,
+	HS_CLK_RANGE_800_1000_MHZ,
+};
+
+static const u8 t_hs_prepare_val[] = {
+	[HS_CLK_RANGE_80_110_MHZ] = 0x20,
+	[HS_CLK_RANGE_110_150_MHZ] = 0x06,
+	[HS_CLK_RANGE_150_200_MHZ] = 0x18,
+	[HS_CLK_RANGE_200_250_MHZ] = 0x05,
+	[HS_CLK_RANGE_250_300_MHZ] = 0x51,
+	[HS_CLK_RANGE_300_400_MHZ] = 0x64,
+	[HS_CLK_RANGE_400_500_MHZ] = 0x20,
+	[HS_CLK_RANGE_500_600_MHZ] = 0x6a,
+	[HS_CLK_RANGE_600_700_MHZ] = 0x3e,
+	[HS_CLK_RANGE_700_800_MHZ] = 0x21,
+	[HS_CLK_RANGE_800_1000_MHZ] = 0x09,
+};
+
+static const u8 clock_lane_t_hs_zero_val[] = {
+	[HS_CLK_RANGE_80_110_MHZ] = 0x16,
+	[HS_CLK_RANGE_110_150_MHZ] = 0x16,
+	[HS_CLK_RANGE_150_200_MHZ] = 0x17,
+	[HS_CLK_RANGE_200_250_MHZ] = 0x17,
+	[HS_CLK_RANGE_250_300_MHZ] = 0x18,
+	[HS_CLK_RANGE_300_400_MHZ] = 0x19,
+	[HS_CLK_RANGE_400_500_MHZ] = 0x1b,
+	[HS_CLK_RANGE_500_600_MHZ] = 0x1d,
+	[HS_CLK_RANGE_600_700_MHZ] = 0x1e,
+	[HS_CLK_RANGE_700_800_MHZ] = 0x1f,
+	[HS_CLK_RANGE_800_1000_MHZ] = 0x20,
+};
+
+static const u8 data_lane_t_hs_zero_val[] = {
+	[HS_CLK_RANGE_80_110_MHZ] = 2,
+	[HS_CLK_RANGE_110_150_MHZ] = 3,
+	[HS_CLK_RANGE_150_200_MHZ] = 4,
+	[HS_CLK_RANGE_200_250_MHZ] = 5,
+	[HS_CLK_RANGE_250_300_MHZ] = 6,
+	[HS_CLK_RANGE_300_400_MHZ] = 7,
+	[HS_CLK_RANGE_400_500_MHZ] = 7,
+	[HS_CLK_RANGE_500_600_MHZ] = 8,
+	[HS_CLK_RANGE_600_700_MHZ] = 8,
+	[HS_CLK_RANGE_700_800_MHZ] = 9,
+	[HS_CLK_RANGE_800_1000_MHZ] = 9,
+};
+
+static const u8 t_hs_trail_val[] = {
+	[HS_CLK_RANGE_80_110_MHZ] = 0x22,
+	[HS_CLK_RANGE_110_150_MHZ] = 0x45,
+	[HS_CLK_RANGE_150_200_MHZ] = 0x0b,
+	[HS_CLK_RANGE_200_250_MHZ] = 0x16,
+	[HS_CLK_RANGE_250_300_MHZ] = 0x2c,
+	[HS_CLK_RANGE_300_400_MHZ] = 0x33,
+	[HS_CLK_RANGE_400_500_MHZ] = 0x4e,
+	[HS_CLK_RANGE_500_600_MHZ] = 0x3a,
+	[HS_CLK_RANGE_600_700_MHZ] = 0x6a,
+	[HS_CLK_RANGE_700_800_MHZ] = 0x29,
+	[HS_CLK_RANGE_800_1000_MHZ] = 0x27,
+};
+
+struct mipi_dphy_timing {
+	unsigned int clkmiss;
+	unsigned int clkpost;
+	unsigned int clkpre;
+	unsigned int clkprepare;
+	unsigned int clksettle;
+	unsigned int clktermen;
+	unsigned int clktrail;
+	unsigned int clkzero;
+	unsigned int dtermen;
+	unsigned int eot;
+	unsigned int hsexit;
+	unsigned int hsprepare;
+	unsigned int hszero;
+	unsigned int hssettle;
+	unsigned int hsskip;
+	unsigned int hstrail;
+	unsigned int init;
+	unsigned int lpx;
+	unsigned int taget;
+	unsigned int tago;
+	unsigned int tasure;
+	unsigned int wakeup;
+};
+
+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;
+};
+
+struct inno_mipi_dphy {
+	struct device *dev;
+	void __iomem *regs;
+	struct clk *ref_clk;
+	struct clk *pclk;
+
+	struct dsi_panel *panel;
+	u32 lanes;
+	u32 lane_mbps;
+};
+
+static inline void inno_write(struct inno_mipi_dphy *inno, u32 reg, u32 val)
+{
+	writel_relaxed(val, inno->regs + reg);
+}
+
+static inline u32 inno_read(struct inno_mipi_dphy *inno, u32 reg)
+{
+	return readl_relaxed(inno->regs + reg);
+}
+
+static inline void inno_update_bits(struct inno_mipi_dphy *inno, u32 reg,
+				    u32 mask, u32 val)
+{
+	u32 tmp, orig;
+
+	orig = inno_read(inno, reg);
+	tmp = orig & ~mask;
+	tmp |= val & mask;
+	inno_write(inno, reg, tmp);
+}
+
+static void mipi_dphy_timing_get_default(struct mipi_dphy_timing *timing,
+					 unsigned long period)
+{
+	/* Global Operation Timing Parameters */
+	timing->clkmiss = 0;
+	timing->clkpost = 70000 + 52 * period;
+	timing->clkpre = 8000;
+	timing->clkprepare = 65000;
+	timing->clksettle = 95000;
+	timing->clktermen = 0;
+	timing->clktrail = 80000;
+	timing->clkzero = 260000;
+	timing->dtermen = 0;
+	timing->eot = 0;
+	timing->hsexit = 120000;
+	timing->hsprepare = 65000 + 5 * period;
+	timing->hszero = 145000 + 5 * period;
+	timing->hssettle = 85000 + 6 * period;
+	timing->hsskip = 40000;
+	timing->hstrail = max(4 * 8 * period, 60000 + 4 * 4 * period);
+	timing->init = 100000000;
+	timing->lpx = 60000;
+	timing->taget = 5 * timing->lpx;
+	timing->tago = 4 * timing->lpx;
+	timing->tasure = timing->lpx;
+	timing->wakeup = 1000000000;
+}
+
+static u32 fre_to_period(u32 fre)
+{
+	u32 integer = 0;
+	u32 decimals = 0;
+
+	integer = 1000000000UL / fre;
+	decimals = 1000000000UL % fre;
+	if (decimals <= 40000000)
+		decimals = (decimals * 100) / (fre / 10);
+	else if (decimals <= 400000000)
+		decimals = (decimals * 10) / (fre / 100);
+	else
+		decimals = decimals / (fre / 1000);
+	integer = integer * 1000 + decimals;
+
+	return integer;
+}
+
+static void inno_mipi_dphy_timing_update(struct inno_mipi_dphy *inno,
+					 enum lane_type lane_type,
+					 struct inno_mipi_dphy_timing *t)
+{
+	u32 base = lane_reg_offset[lane_type];
+	u32 val, mask;
+
+	mask = M_T_HS_PREPARE;
+	val = V_T_HS_PREPARE(t->t_hs_prepare);
+	inno_update_bits(inno, base + T_HS_PREPARE_OFFSET, mask, val);
+
+	mask = M_T_HS_ZERO;
+	val = V_T_HS_ZERO(t->t_hs_zero);
+	inno_update_bits(inno, base + T_HS_ZERO_OFFSET, mask, val);
+
+	mask = M_T_HS_TRAIL;
+	val = V_T_HS_TRAIL(t->t_hs_trail);
+	inno_update_bits(inno, base + T_HS_TRAIL_OFFSET, mask, val);
+
+	mask = M_T_HS_EXIT;
+	val = V_T_HS_EXIT(t->t_hs_exit);
+	inno_update_bits(inno, base + T_HS_EXIT_OFFSET, mask, val);
+
+	if (lane_type == CLOCK_LANE) {
+		mask = M_T_CLK_POST;
+		val = V_T_CLK_POST(t->t_clk_post);
+		inno_update_bits(inno, base + T_CLK_POST_OFFSET, mask, val);
+
+		mask = M_T_CLK_PRE;
+		val = V_T_CLK_PRE(t->t_clk_pre);
+		inno_update_bits(inno, base + T_CLK_PRE_OFFSET, mask, val);
+	}
+
+	mask = M_T_WAKUP_H;
+	val = V_T_WAKUP_H(t->t_wakup_h);
+	inno_update_bits(inno, base + T_WAKUP_H_OFFSET, mask, val);
+
+	mask = M_T_WAKUP_L;
+	val = V_T_WAKUP_L(t->t_wakup_l);
+	inno_update_bits(inno, base + T_WAKUP_L_OFFSET, mask, val);
+
+	mask = M_T_LPX;
+	val = V_T_LPX(t->t_lpx);
+	inno_update_bits(inno, base + T_LPX_OFFSET, mask, val);
+
+	mask = M_T_TA_GO;
+	val = V_T_TA_GO(t->t_ta_go);
+	inno_update_bits(inno, base + T_TA_GO_OFFSET, mask, val);
+
+	mask = M_T_TA_SURE;
+	val = V_T_TA_SURE(t->t_ta_sure);
+	inno_update_bits(inno, base + T_TA_SURE_OFFSET, mask, val);
+
+	mask = M_T_TA_WAIT;
+	val = V_T_TA_WAIT(t->t_ta_wait);
+	inno_update_bits(inno, base + T_TA_WAIT_OFFSET, mask, val);
+}
+
+static enum hs_clk_range inno_mipi_dphy_get_hs_clk_range(u32 lane_rate)
+{
+	u32 range = lane_rate / USEC_PER_SEC;
+
+	if (range < 110)
+		return HS_CLK_RANGE_80_110_MHZ;
+	else if (range < 150)
+		return HS_CLK_RANGE_110_150_MHZ;
+	else if (range < 200)
+		return HS_CLK_RANGE_150_200_MHZ;
+	else if (range < 250)
+		return HS_CLK_RANGE_200_250_MHZ;
+	else if (range < 300)
+		return HS_CLK_RANGE_250_300_MHZ;
+	else if (range < 400)
+		return HS_CLK_RANGE_400_500_MHZ;
+	else if (range < 500)
+		return HS_CLK_RANGE_400_500_MHZ;
+	else if (range < 600)
+		return HS_CLK_RANGE_500_600_MHZ;
+	else if (range < 700)
+		return HS_CLK_RANGE_600_700_MHZ;
+	else if (range < 800)
+		return HS_CLK_RANGE_700_800_MHZ;
+	else
+		return HS_CLK_RANGE_800_1000_MHZ;
+}
+
+static void inno_mipi_dphy_lane_timing_init(struct inno_mipi_dphy *inno,
+					    enum lane_type lane_type)
+{
+	struct mipi_dphy_timing timing;
+	struct inno_mipi_dphy_timing data;
+	u32 txbyteclkhs = inno->lane_mbps * USEC_PER_SEC / 8;
+	u32 txclkesc = 20000000;
+	u32 UI = fre_to_period(inno->lane_mbps * USEC_PER_SEC);
+	u32 unit;
+	enum hs_clk_range range;
+
+	memset(&timing, 0, sizeof(timing));
+	memset(&data, 0, sizeof(data));
+
+	mipi_dphy_timing_get_default(&timing, UI);
+
+	range = inno_mipi_dphy_get_hs_clk_range(inno->lane_mbps * USEC_PER_SEC);
+
+	if (lane_type == CLOCK_LANE)
+		data.t_hs_zero = clock_lane_t_hs_zero_val[range];
+	else
+		data.t_hs_zero = data_lane_t_hs_zero_val[range];
+
+	data.t_hs_prepare = t_hs_prepare_val[range];
+	data.t_hs_trail = t_hs_trail_val[range];
+
+	/* txbyteclkhs domain */
+	unit = fre_to_period(txbyteclkhs);
+	data.t_hs_exit = DIV_ROUND_UP(timing.hsexit, unit);
+	data.t_clk_post = DIV_ROUND_UP(timing.clkpost, unit);
+	data.t_clk_pre = DIV_ROUND_UP(timing.clkpre, unit);
+	data.t_wakup_h = 0x3;
+	data.t_wakup_l = 0xff;
+	data.t_lpx = DIV_ROUND_UP(timing.lpx, unit) - 2;
+
+	/* txclkesc domain */
+	unit = fre_to_period(txclkesc);
+	data.t_ta_go =  DIV_ROUND_UP(timing.tago, unit);
+	data.t_ta_sure = DIV_ROUND_UP(timing.tasure, unit);
+	data.t_ta_wait = DIV_ROUND_UP(timing.taget, unit);
+
+	inno_mipi_dphy_timing_update(inno, lane_type, &data);
+}
+
+static void inno_mipi_dphy_pll_init(struct inno_mipi_dphy *inno)
+{
+	struct dsi_panel *panel = inno->panel;
+	unsigned int i, pre;
+	unsigned long 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++) {
+		pre = pllref / i;
+		if ((tmp > (target_mbps % pre)) && (target_mbps / pre < 512)) {
+			tmp = target_mbps % pre;
+			prediv = i;
+			fbdiv = target_mbps / pre;
+		}
+		if (tmp == 0)
+			break;
+	}
+
+	inno->lane_mbps = pllref / prediv * fbdiv;
+
+	mask = M_FBDIV_8 | M_PREDIV;
+	val = V_FBDIV_8(fbdiv >> 8) | V_PREDIV(prediv);
+	inno_update_bits(inno, INNO_PHY_PLL_CTRL_0, mask, val);
+
+	mask = M_FBDIV_7_0;
+	val = V_FBDIV_7_0(fbdiv);
+	inno_update_bits(inno, INNO_PHY_PLL_CTRL_1, mask, val);
+
+	dev_info(inno->dev, "fin=%ld, fout=%d, prediv=%d, fbdiv=%d\n",
+		 pllref, inno->lane_mbps, prediv, 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);
+}
+
+static void inno_mipi_dphy_timing_init(struct inno_mipi_dphy *inno)
+{
+	switch (inno->lanes) {
+	case 4:
+		inno_mipi_dphy_lane_timing_init(inno, DATA_LANE_3);
+		/* Fall through */
+	case 3:
+		inno_mipi_dphy_lane_timing_init(inno, DATA_LANE_2);
+		/* Fall through */
+	case 2:
+		inno_mipi_dphy_lane_timing_init(inno, DATA_LANE_1);
+		/* Fall through */
+	case 1:
+	default:
+		inno_mipi_dphy_lane_timing_init(inno, DATA_LANE_0);
+		inno_mipi_dphy_lane_timing_init(inno, CLOCK_LANE);
+		break;
+	}
+}
+
+static inline void inno_mipi_dphy_lane_enable(struct inno_mipi_dphy *inno)
+{
+	u32 val = 0;
+	u32 mask = 0;
+
+	switch (inno->lanes) {
+	case 4:
+		mask |= M_DATA_LANE_3_EN;
+		val |= V_DATA_LANE_3_EN;
+		/* Fall through */
+	case 3:
+		mask |= M_DATA_LANE_2_EN;
+		val |= V_DATA_LANE_2_EN;
+		/* Fall through */
+	case 2:
+		mask |= M_DATA_LANE_1_EN;
+		val |= V_DATA_LANE_1_EN;
+		/* Fall through */
+	default:
+	case 1:
+		mask |= M_DATA_LANE_0_EN | M_CLK_LANE_EN;
+		val |= V_DATA_LANE_0_EN | V_CLK_LANE_EN;
+		break;
+	}
+
+	inno_update_bits(inno, INNO_PHY_LANE_CTRL, mask, val);
+}
+
+static inline void inno_mipi_dphy_pll_ldo_enable(struct inno_mipi_dphy *inno)
+{
+	inno_write(inno, INNO_PHY_POWER_CTRL, 0xe4);
+	udelay(10);
+}
+
+static int inno_mipi_dphy_power_on(struct phy *phy)
+{
+	struct inno_mipi_dphy *inno = phy_get_drvdata(phy);
+
+	clk_prepare_enable(inno->ref_clk);
+	clk_prepare_enable(inno->pclk);
+
+	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");
+
+	return 0;
+}
+
+static inline void inno_mipi_dphy_lane_disable(struct inno_mipi_dphy *inno)
+{
+	inno_update_bits(inno, INNO_PHY_LANE_CTRL, 0x7c, 0x00);
+}
+
+static inline void inno_mipi_dphy_pll_ldo_disable(struct inno_mipi_dphy *inno)
+{
+	inno_write(inno, INNO_PHY_POWER_CTRL, 0xe3);
+	udelay(10);
+}
+
+static int inno_mipi_dphy_power_off(struct phy *phy)
+{
+	struct inno_mipi_dphy *inno = phy_get_drvdata(phy);
+
+	inno_mipi_dphy_lane_disable(inno);
+	inno_mipi_dphy_pll_ldo_disable(inno);
+
+	clk_disable_unprepare(inno->pclk);
+	clk_disable_unprepare(inno->ref_clk);
+
+	dev_info(inno->dev, "Inno MIPI-DPHY Power-Off\n");
+
+	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)
+{
+	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;
+	}
+}
+
+static int inno_mipi_dphy_parse_dt(struct device_node *np,
+				   struct inno_mipi_dphy *inno)
+{
+	struct device_node *panel_node;
+	struct dsi_panel *panel;
+	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;
+	}
+
+	panel = devm_kzalloc(inno->dev, sizeof(*panel), GFP_KERNEL);
+	if (!panel) {
+		ret = -ENOMEM;
+		goto put_panel_node;
+	}
+
+	ret = of_get_videomode(panel_node, &panel->vm, 0);
+	if (ret < 0)
+		goto put_panel_node;
+
+	panel->bpp = get_bpp(panel_node);
+
+	if (of_property_read_u32(panel_node, "dsi,lanes", &inno->lanes))
+		inno->lanes = 4;
+
+	of_node_put(panel_node);
+
+	inno->panel = panel;
+
+	return 0;
+
+put_panel_node:
+	of_node_put(panel_node);
+	return ret;
+}
+
+static int inno_mipi_dphy_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct inno_mipi_dphy *inno;
+	struct phy *phy;
+	struct phy_provider *phy_provider;
+	struct resource *res;
+	int ret;
+
+	inno = devm_kzalloc(&pdev->dev, sizeof(*inno), GFP_KERNEL);
+	if (!inno)
+		return -ENOMEM;
+
+	inno->dev = &pdev->dev;
+
+	ret = inno_mipi_dphy_parse_dt(np, inno);
+	if (ret) {
+		dev_err(&pdev->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);
+
+	inno->ref_clk = devm_clk_get(&pdev->dev, "ref");
+	if (IS_ERR(inno->ref_clk)) {
+		dev_err(&pdev->dev, "failed to get mipi dphy ref clk\n");
+		return PTR_ERR(inno->ref_clk);
+	}
+
+	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);
+	};
+
+	phy = devm_phy_create(&pdev->dev, NULL, &inno_mipi_dphy_ops);
+	if (IS_ERR(phy)) {
+		dev_err(&pdev->dev, "failed to create MIPI D-PHY\n");
+		return PTR_ERR(phy);
+	}
+
+	phy_set_drvdata(phy, inno);
+
+	phy_provider = devm_of_phy_provider_register(&pdev->dev,
+						     of_phy_simple_xlate);
+	if (IS_ERR(phy_provider)) {
+		dev_err(&pdev->dev, "failed to register phy provider\n");
+		return PTR_ERR(phy_provider);
+	}
+
+	dev_info(&pdev->dev, "Inno MIPI-DPHY Driver Probe\n");
+
+	return 0;
+}
+
+static const struct of_device_id inno_mipi_dphy_of_match[] = {
+	{ .compatible = "rockchip,rk3368-mipi-dphy", },
+	{ /* Sentinel */ }
+};
+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,
+	}
+};
+
+module_platform_driver(inno_mipi_dphy_driver);
+
+MODULE_DESCRIPTION("Innosilicon MIPI D-PHY Driver");
+MODULE_LICENSE("GPL v2");
-- 
2.34.1