[PATCH 0/2] cadence: csirx: Add Cadence dphy support

[PATCH 0/2] cadence: csirx: Add Cadence dphy support

[Krzysztof Witos]

The main goal is to provide Cadence DPHY support for CSI2RX driver.
It is implemented basing on the DSITX DPHY solution.

Krzysztof Witos (2):
  csirx updated doc for csirx with dphy supported
  added support for csirx dphy

 .../devicetree/bindings/media/cdns,csi2rx.txt      |  32 +-
 drivers/media/platform/cadence/cdns-csi2rx.c       | 342 +++++++++++++++++++--
 2 files changed, 341 insertions(+), 33 deletions(-)

-- 
2.15.0

[PATCH 1/2] csirx updated doc for csirx with dphy supported

[Krzysztof Witos]

Signed-off-by: Krzysztof Witos <kwitos@cadence.com>
---
 .../devicetree/bindings/media/cdns,csi2rx.txt      | 32 +++++++++++++++++++---
 1 file changed, 28 insertions(+), 4 deletions(-)

diff --git a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
index 6b02a0657ad9..753b967863c0 100644
--- a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
+++ b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
@@ -13,8 +13,7 @@ Required properties:
     * p_clk: register bank clock
     * pixel_if[0-3]_clk: pixel stream output clock, one for each stream
                          implemented in hardware, between 0 and 3
-
-Optional properties:
+    * hs_clock: dphy module clock
   - phys: phandle to the external D-PHY, phy-names must be provided
   - phy-names: must contain "dphy", if the implementation uses an
                external D-PHY
@@ -38,18 +37,43 @@ Required subnodes:
            in the design.Since there is only one endpoint per port,
            the endpoints are not numbered.
 
+Cadence DPHY
+============
+
+Cadence DPHY block.
+
+Required properties:
+- compatible: should be set to "cdns,dphy".
+- reg: physical base address and length of the DPHY registers.
+- clocks: DPHY reference clocks.
+- clock-names: must contain "psm" and "hs_clk".
+- #phy-cells: must be set to 0.
+
 
 Example:
 
+dphy1:  dphy@fd0d0000 {
+	compatible = "cdns,dphy";
+	reg = <0x0 0xfd0d0000 0x0 0x1000>;
+	clocks = <&dphy_psm_clk>;
+	clock-names = "psm";
+	clock-output-names = "hs_clk";
+	#phy-cells = <0>;
+
 csi2rx: csi-bridge@0d060000 {
 	compatible = "cdns,csi2rx";
 	reg = <0x0d060000 0x1000>;
 	clocks = <&byteclock>, <&byteclock>
 		 <&coreclock>, <&coreclock>,
-		 <&coreclock>, <&coreclock>;
+		 <&coreclock>, <&coreclock>,
+		 <&dphy_rx_byte_hs_clk>;
 	clock-names = "sys_clk", "p_clk",
 		      "pixel_if0_clk", "pixel_if1_clk",
-		      "pixel_if2_clk", "pixel_if3_clk";
+		      "pixel_if2_clk", "pixel_if3_clk",
+		      "hs_clk";	
+
+	phys = <&dphy1>;
+	phy-names = "dphy";
 
 	ports {
 		#address-cells = <1>;
-- 
2.15.0

Re: [PATCH 1/2] csirx updated doc for csirx with dphy supported

[Rob Herring]

On Fri, Jun 08, 2018 at 11:33:03AM +0100, Krzysztof Witos wrote:

Commit message?

And the subject prefix should be "dt-bindings: media: ..."

> Signed-off-by: Krzysztof Witos <kwitos@cadence.com>
> ---
>  .../devicetree/bindings/media/cdns,csi2rx.txt      | 32 +++++++++++++++++++---
>  1 file changed, 28 insertions(+), 4 deletions(-)
> 
> diff --git a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> index 6b02a0657ad9..753b967863c0 100644
> --- a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> +++ b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> @@ -13,8 +13,7 @@ Required properties:
>      * p_clk: register bank clock
>      * pixel_if[0-3]_clk: pixel stream output clock, one for each stream
>                           implemented in hardware, between 0 and 3
> -
> -Optional properties:

phys is no longer optional? That's not backwards compatible and needs an 
explanation if in fact intentional.

> +    * hs_clock: dphy module clock
>    - phys: phandle to the external D-PHY, phy-names must be provided
>    - phy-names: must contain "dphy", if the implementation uses an
>                 external D-PHY
> @@ -38,18 +37,43 @@ Required subnodes:
>             in the design.Since there is only one endpoint per port,
>             the endpoints are not numbered.
>  
> +Cadence DPHY
> +============
> +
> +Cadence DPHY block.
> +
> +Required properties:
> +- compatible: should be set to "cdns,dphy".
> +- reg: physical base address and length of the DPHY registers.
> +- clocks: DPHY reference clocks.
> +- clock-names: must contain "psm" and "hs_clk".
> +- #phy-cells: must be set to 0.
> +
>  
>  Example:
>  
> +dphy1:  dphy@fd0d0000 {
> +	compatible = "cdns,dphy";
> +	reg = <0x0 0xfd0d0000 0x0 0x1000>;
> +	clocks = <&dphy_psm_clk>;
> +	clock-names = "psm";
> +	clock-output-names = "hs_clk";
> +	#phy-cells = <0>;
> +
>  csi2rx: csi-bridge@0d060000 {
>  	compatible = "cdns,csi2rx";
>  	reg = <0x0d060000 0x1000>;
>  	clocks = <&byteclock>, <&byteclock>
>  		 <&coreclock>, <&coreclock>,
> -		 <&coreclock>, <&coreclock>;
> +		 <&coreclock>, <&coreclock>,
> +		 <&dphy_rx_byte_hs_clk>;
>  	clock-names = "sys_clk", "p_clk",
>  		      "pixel_if0_clk", "pixel_if1_clk",
> -		      "pixel_if2_clk", "pixel_if3_clk";
> +		      "pixel_if2_clk", "pixel_if3_clk",
> +		      "hs_clk";	
> +
> +	phys = <&dphy1>;
> +	phy-names = "dphy";
>  
>  	ports {
>  		#address-cells = <1>;
> -- 
> 2.15.0
> 
> --
> To unsubscribe from this list: send the line "unsubscribe devicetree" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html

RE: [PATCH 1/2] csirx updated doc for csirx with dphy supported

[Krzysztof Witos]

Hi Rob,

> On Fri, Jun 08, 2018 at 11:33:03AM +0100, Krzysztof Witos wrote:
> 
> Commit message?
> 
> And the subject prefix should be "dt-bindings: media: ..."

Yes, sorry for this mess with commit names.

> 
> > Signed-off-by: Krzysztof Witos <kwitos@cadence.com>
> > ---
> >  .../devicetree/bindings/media/cdns,csi2rx.txt      | 32
> +++++++++++++++++++---
> >  1 file changed, 28 insertions(+), 4 deletions(-)
> >
> > diff --git a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> > b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> > index 6b02a0657ad9..753b967863c0 100644
> > --- a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> > +++ b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> > @@ -13,8 +13,7 @@ Required properties:
> >      * p_clk: register bank clock
> >      * pixel_if[0-3]_clk: pixel stream output clock, one for each stream
> >                           implemented in hardware, between 0 and 3
> > -
> > -Optional properties:
> 
> phys is no longer optional? That's not backwards compatible and needs an
> explanation if in fact intentional.
> 

Thank you for pointing this out. I have to review it and probably will try to change it
to not break this backward compatibility.

Thanks,
Krzysiek

Re: [PATCH 1/2] csirx updated doc for csirx with dphy supported

[Rob Herring]

On Fri, Jun 08, 2018 at 11:33:03AM +0100, Krzysztof Witos wrote:
> Signed-off-by: Krzysztof Witos <kwitos@cadence.com>
> ---
>  .../devicetree/bindings/media/cdns,csi2rx.txt      | 32 +++++++++++++++++++---
>  1 file changed, 28 insertions(+), 4 deletions(-)
> 
> diff --git a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> index 6b02a0657ad9..753b967863c0 100644
> --- a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> +++ b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> @@ -13,8 +13,7 @@ Required properties:
>      * p_clk: register bank clock
>      * pixel_if[0-3]_clk: pixel stream output clock, one for each stream
>                           implemented in hardware, between 0 and 3
> -
> -Optional properties:
> +    * hs_clock: dphy module clock
>    - phys: phandle to the external D-PHY, phy-names must be provided
>    - phy-names: must contain "dphy", if the implementation uses an
>                 external D-PHY
> @@ -38,18 +37,43 @@ Required subnodes:
>             in the design.Since there is only one endpoint per port,
>             the endpoints are not numbered.
>  
> +Cadence DPHY
> +============
> +
> +Cadence DPHY block.
> +
> +Required properties:
> +- compatible: should be set to "cdns,dphy".

Also, needs to be more specific. Only 1 version of this h/w?

RE: [PATCH 1/2] csirx updated doc for csirx with dphy supported

[Krzysztof Witos]

Hi Rob, 

Thanks for review.

> On Fri, Jun 08, 2018 at 11:33:03AM +0100, Krzysztof Witos wrote:
> > Signed-off-by: Krzysztof Witos <kwitos@cadence.com>
> > ---
> >  .../devicetree/bindings/media/cdns,csi2rx.txt      | 32
> +++++++++++++++++++---
> >  1 file changed, 28 insertions(+), 4 deletions(-)
> >
> > diff --git a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> > index 6b02a0657ad9..753b967863c0 100644
> > --- a/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> > +++ b/Documentation/devicetree/bindings/media/cdns,csi2rx.txt
> > @@ -13,8 +13,7 @@ Required properties:
> >      * p_clk: register bank clock
> >      * pixel_if[0-3]_clk: pixel stream output clock, one for each stream
> >                           implemented in hardware, between 0 and 3
> > -
> > -Optional properties:
> > +    * hs_clock: dphy module clock
> >    - phys: phandle to the external D-PHY, phy-names must be provided
> >    - phy-names: must contain "dphy", if the implementation uses an
> >                 external D-PHY
> > @@ -38,18 +37,43 @@ Required subnodes:
> >             in the design.Since there is only one endpoint per port,
> >             the endpoints are not numbered.
> >
> > +Cadence DPHY
> > +============
> > +
> > +Cadence DPHY block.
> > +
> > +Required properties:
> > +- compatible: should be set to "cdns,dphy".
> 
> Also, needs to be more specific. Only 1 version of this h/w?

Right, I will try to get more specific name for this h/w.

Best Regards,
Krzysiek

[PATCH 2/2] added support for csirx dphy

[Krzysztof Witos]

Signed-off-by: Krzysztof Witos <kwitos@cadence.com>
---
 drivers/media/platform/cadence/cdns-csi2rx.c | 342 ++++++++++++++++++++++++---
 1 file changed, 313 insertions(+), 29 deletions(-)

diff --git a/drivers/media/platform/cadence/cdns-csi2rx.c b/drivers/media/platform/cadence/cdns-csi2rx.c
index a0f02916006b..9251ea6015f0 100644
--- a/drivers/media/platform/cadence/cdns-csi2rx.c
+++ b/drivers/media/platform/cadence/cdns-csi2rx.c
@@ -2,14 +2,16 @@
 /*
  * Driver for Cadence MIPI-CSI2 RX Controller v1.3
  *
- * Copyright (C) 2017 Cadence Design Systems Inc.
+ * Copyright (C) 2017,2018 Cadence Design Systems Inc.
  */
 
 #include <linux/clk.h>
+#include <linux/iopoll.h>
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/of_graph.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
@@ -44,6 +46,33 @@
 #define CSI2RX_LANES_MAX	4
 #define CSI2RX_STREAMS_MAX	4
 
+/* DPHY registers */
+#define DPHY_PMA_CMN(reg)       (reg)
+#define DPHY_PMA_LCLK(reg)      (0x100 + (reg))
+#define DPHY_PMA_LDATA(lane, reg)   (0x200 + ((lane) * 0x100) + (reg))
+#define DPHY_PMA_RCLK(reg)      (0x600 + (reg))
+#define DPHY_PMA_RDATA(lane, reg)   (0x700 + ((lane) * 0x100) + (reg))
+#define DPHY_PCS(reg)           (0xb00 + (reg))
+
+#define DPHY_CMN_SSM            DPHY_PMA_CMN(0x20)
+#define DPHY_CMN_SSM_EN         BIT(0)
+#define DPHY_CMN_RX_MODE_EN     BIT(10)
+
+#define DPHY_CMN_PWM            DPHY_PMA_CMN(0x40)
+#define DPHY_CMN_PWM_DIV(x)     ((x) << 20)
+#define DPHY_CMN_PWM_LOW(x)     ((x) << 10)
+#define DPHY_CMN_PWM_HIGH(x)        (x)
+
+#define DPHY_CMN_PLL_CFG	DPHY_PMA_CMN(0xE8)
+#define PLL_LOCKED		BIT(2)
+
+#define DPHY_PSM_CFG            DPHY_PCS(0x4)
+#define DPHY_PSM_CFG_FROM_REG       BIT(0)
+#define DPHY_PSM_CLK_DIV(x)     ((x) << 1)
+
+#define DPHY_BAND_CTRL          DPHY_PCS(0x0)
+#define DPHY_BAND_LEFT_VAL(x)	(x)
+
 enum csi2rx_pads {
 	CSI2RX_PAD_SINK,
 	CSI2RX_PAD_SOURCE_STREAM0,
@@ -67,7 +96,7 @@ struct csi2rx_priv {
 	struct clk			*sys_clk;
 	struct clk			*p_clk;
 	struct clk			*pixel_clk[CSI2RX_STREAMS_MAX];
-	struct phy			*dphy;
+	struct clk			*hs_clk;
 
 	u8				lanes[CSI2RX_LANES_MAX];
 	u8				num_lanes;
@@ -83,8 +112,175 @@ struct csi2rx_priv {
 	struct v4l2_async_subdev	asd;
 	struct v4l2_subdev		*source_subdev;
 	int				source_pad;
+	struct cdns_dphy		*dphy;
+};
+
+struct cdns_dphy_cfg {
+	unsigned int nlanes;
+};
+
+struct cdns_dphy;
+
+enum cdns_dphy_clk_lane_cfg {
+	DPHY_CLK_CFG_LEFT_DRIVES_ALL = 0,
+	DPHY_CLK_CFG_LEFT_DRIVES_RIGHT = 1,
+	DPHY_CLK_CFG_LEFT_DRIVES_LEFT = 2,
+	DPHY_CLK_CFG_RIGHT_DRIVES_ALL = 3
+};
+
+struct cdns_dphy_ops {
+	int (*probe)(struct cdns_dphy *dphy);
+	void (*remove)(struct cdns_dphy *dphy);
+	void (*set_psm_div)(struct cdns_dphy *dphy, u8 div);
+	void (*set_pll_cfg)(struct cdns_dphy *dphy);
+	void (*set_clk_lane_cfg)(struct cdns_dphy *dphy,
+		enum cdns_dphy_clk_lane_cfg cfg);
+	void (*is_pll_locked)(struct cdns_dphy *dphy);
+	void (*set_band_ctrl)(struct cdns_dphy *dphy, u8 value);
+};
+
+struct cdns_dphy {
+	struct cdns_dphy_cfg cfg;
+	void __iomem *regs;
+	struct clk *psm_clk;
+	const struct cdns_dphy_ops *ops;
 };
 
+static int cdns_dphy_set_band_ctrl(struct cdns_dphy *dphy,
+	struct csi2rx_priv *csirx)
+{
+	u8 band_value;
+	u32 hs_freq_mhz = clk_get_rate(csirx->hs_clk);
+
+	if (hs_freq_mhz >= 80 && hs_freq_mhz < 100)
+		band_value = 0;
+	else if (hs_freq_mhz >= 100 && hs_freq_mhz < 120)
+		band_value = 1;
+	else if (hs_freq_mhz >= 120 && hs_freq_mhz < 160)
+		band_value = 2;
+	else if (hs_freq_mhz >= 160 && hs_freq_mhz < 200)
+		band_value = 3;
+	else if (hs_freq_mhz >= 200 && hs_freq_mhz < 240)
+		band_value = 4;
+	else if (hs_freq_mhz >= 240 && hs_freq_mhz < 280)
+		band_value = 5;
+	else if (hs_freq_mhz >= 280 && hs_freq_mhz < 320)
+		band_value = 6;
+	else if (hs_freq_mhz >= 320 && hs_freq_mhz < 360)
+		band_value = 7;
+	else if (hs_freq_mhz >= 360 && hs_freq_mhz < 400)
+		band_value = 8;
+	else if (hs_freq_mhz >= 400 && hs_freq_mhz < 480)
+		band_value = 9;
+	else if (hs_freq_mhz >= 480 && hs_freq_mhz < 560)
+		band_value = 10;
+	else if (hs_freq_mhz >= 560 && hs_freq_mhz < 640)
+		band_value = 11;
+	else if (hs_freq_mhz >= 640 && hs_freq_mhz < 720)
+		band_value = 12;
+	else if (hs_freq_mhz >= 720 && hs_freq_mhz < 800)
+		band_value = 13;
+	else if (hs_freq_mhz >= 800 && hs_freq_mhz < 880)
+		band_value = 14;
+	else if (hs_freq_mhz >= 880 && hs_freq_mhz < 1040)
+		band_value = 15;
+	else if (hs_freq_mhz >= 1040 && hs_freq_mhz < 1200)
+		band_value = 16;
+	else if (hs_freq_mhz >= 1200 && hs_freq_mhz < 1350)
+		band_value = 17;
+	else if (hs_freq_mhz >= 1350 && hs_freq_mhz < 1500)
+		band_value = 18;
+	else if (hs_freq_mhz >= 1500 && hs_freq_mhz < 1750)
+		band_value = 19;
+	else if (hs_freq_mhz >= 1750 && hs_freq_mhz < 2000)
+		band_value = 20;
+	else if (hs_freq_mhz >= 2000 && hs_freq_mhz < 2250)
+		band_value = 21;
+	else if (hs_freq_mhz >= 2250 && hs_freq_mhz <= 2500)
+		band_value = 22;
+	else
+		return -EINVAL;
+
+	if (dphy->ops->set_band_ctrl)
+		dphy->ops->set_band_ctrl(dphy, band_value);
+
+	return 0;
+}
+
+static int cdns_dphy_setup_psm(struct cdns_dphy *dphy)
+{
+	unsigned long psm_clk_hz = clk_get_rate(dphy->psm_clk);
+	unsigned long psm_div;
+
+	if (!psm_clk_hz || psm_clk_hz > 100000000)
+		return -EINVAL;
+
+	psm_div = DIV_ROUND_CLOSEST(psm_clk_hz, 1000000);
+	if (dphy->ops->set_psm_div)
+		dphy->ops->set_psm_div(dphy, psm_div);
+
+	return 0;
+}
+
+static void cdns_dphy_set_clk_lane_cfg(struct cdns_dphy *dphy,
+	enum cdns_dphy_clk_lane_cfg cfg)
+{
+	if (dphy->ops->set_clk_lane_cfg)
+		dphy->ops->set_clk_lane_cfg(dphy, cfg);
+}
+
+static void cdns_dphy_set_pll_cfg(struct cdns_dphy *dphy)
+{
+	if (dphy->ops->set_pll_cfg)
+		dphy->ops->set_pll_cfg(dphy);
+}
+
+static void cdns_dphy_is_pll_locked(struct cdns_dphy *dphy)
+{
+	if (dphy->ops->is_pll_locked)
+		dphy->ops->is_pll_locked(dphy);
+}
+
+static void cdns_csirx_dphy_init(struct csi2rx_priv *csi2rx,
+	const struct cdns_dphy_cfg *dphy_cfg)
+{
+
+	/*
+	 * Configure the band control settings.
+	 */
+	cdns_dphy_set_band_ctrl(csi2rx->dphy, csi2rx);
+
+	/*
+	 * Configure the internal PSM clk divider so that the DPHY has a
+	 * 1MHz clk (or something close).
+	 */
+	WARN_ON_ONCE(cdns_dphy_setup_psm(csi2rx->dphy));
+
+	/*
+	 * Configure attach clk lanes to data lanes: the DPHY has 2 clk lanes
+	 * and 8 data lanes, each clk lane can be attache different set of
+	 * data lanes. The 2 groups are named 'left' and 'right', so here we
+	 * just say that we want the 'left' clk lane to drive the 'left' data
+	 * lanes.
+	 */
+	cdns_dphy_set_clk_lane_cfg(csi2rx->dphy,
+		DPHY_CLK_CFG_LEFT_DRIVES_LEFT);
+
+	/*
+	 * Configure the DPHY PLL that will be used to generate the TX byte
+	 * clk.
+	 */
+	cdns_dphy_set_pll_cfg(csi2rx->dphy);
+
+	/*  Start RX state machine. */
+	writel(DPHY_CMN_SSM_EN | DPHY_CMN_RX_MODE_EN,
+		csi2rx->dphy->regs + DPHY_CMN_SSM);
+
+	/* Checking if PLL is locked */
+	cdns_dphy_is_pll_locked(csi2rx->dphy);
+
+}
+
 static inline
 struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
 {
@@ -103,6 +299,7 @@ static void csi2rx_reset(struct csi2rx_priv *csi2rx)
 
 static int csi2rx_start(struct csi2rx_priv *csi2rx)
 {
+	struct cdns_dphy_cfg dphy_cfg;
 	unsigned int i;
 	unsigned long lanes_used = 0;
 	u32 reg;
@@ -135,6 +332,8 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
 
 	writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG);
 
+	cdns_csirx_dphy_init(csi2rx, &dphy_cfg);
+
 	ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true);
 	if (ret)
 		goto err_disable_pclk;
@@ -300,19 +499,10 @@ static int csi2rx_get_resources(struct csi2rx_priv *csi2rx,
 		return PTR_ERR(csi2rx->p_clk);
 	}
 
-	csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy");
-	if (IS_ERR(csi2rx->dphy)) {
-		dev_err(&pdev->dev, "Couldn't get external D-PHY\n");
-		return PTR_ERR(csi2rx->dphy);
-	}
-
-	/*
-	 * FIXME: Once we'll have external D-PHY support, the check
-	 * will need to be removed.
-	 */
-	if (csi2rx->dphy) {
-		dev_err(&pdev->dev, "External D-PHY not supported yet\n");
-		return -EINVAL;
+	csi2rx->hs_clk = devm_clk_get(&pdev->dev, "hs_clk");
+	if (IS_ERR(csi2rx->hs_clk)) {
+		dev_err(&pdev->dev, "Couldn't get hs clock\n");
+		return PTR_ERR(csi2rx->hs_clk);
 	}
 
 	clk_prepare_enable(csi2rx->p_clk);
@@ -333,17 +523,6 @@ static int csi2rx_get_resources(struct csi2rx_priv *csi2rx,
 		return -EINVAL;
 	}
 
-	csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false;
-
-	/*
-	 * FIXME: Once we'll have internal D-PHY support, the check
-	 * will need to be removed.
-	 */
-	if (csi2rx->has_internal_dphy) {
-		dev_err(&pdev->dev, "Internal D-PHY not supported yet\n");
-		return -EINVAL;
-	}
-
 	for (i = 0; i < csi2rx->max_streams; i++) {
 		char clk_name[16];
 
@@ -412,6 +591,107 @@ static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx)
 						   &csi2rx->notifier);
 }
 
+static void cdns_dphy_ref_set_pll_cfg(struct cdns_dphy *dphy)
+{
+	writel(DPHY_CMN_PWM_HIGH(6) | DPHY_CMN_PWM_LOW(0x101) |
+		DPHY_CMN_PWM_DIV(0x8),
+		dphy->regs + DPHY_CMN_PWM);
+}
+
+static void cdns_dphy_ref_set_band_ctrl(struct cdns_dphy *dphy, u8 value)
+{
+	writel(DPHY_BAND_LEFT_VAL(value),
+		dphy->regs + DPHY_BAND_CTRL);
+}
+
+static void cdns_dphy_ref_set_psm_div(struct cdns_dphy *dphy, u8 div)
+{
+	writel(DPHY_PSM_CFG_FROM_REG | DPHY_PSM_CLK_DIV(div),
+		dphy->regs + DPHY_PSM_CFG);
+}
+
+static void cdns_dphy_ref_is_pll_locked(struct cdns_dphy *dphy)
+{
+	u32 status;
+
+	WARN_ON_ONCE(readl_poll_timeout(dphy->regs + DPHY_CMN_PLL_CFG, status,
+					status & PLL_LOCKED, 100, 100));
+}
+
+/*
+ * This is the reference implementation of DPHY hooks. Specific integration of
+ * this IP may have to re-implement some of them depending on how they decided
+ * to wire things in the SoC.
+ */
+static const struct cdns_dphy_ops ref_dphy_ops = {
+	.set_pll_cfg = cdns_dphy_ref_set_pll_cfg,
+	.set_psm_div = cdns_dphy_ref_set_psm_div,
+	.set_band_ctrl = cdns_dphy_ref_set_band_ctrl,
+	.is_pll_locked = cdns_dphy_ref_is_pll_locked
+};
+
+static const struct of_device_id cdns_dphy_of_match[] = {
+	{ .compatible = "cdns,dphy", .data = &ref_dphy_ops },
+	{ /* sentinel */ },
+};
+
+static struct cdns_dphy *cdns_dphy_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	struct cdns_dphy *dphy;
+	struct of_phandle_args args;
+	struct resource res;
+	int ret;
+
+	ret = of_parse_phandle_with_args(pdev->dev.of_node, "phys",
+		"#phy-cells", 0, &args);
+	if (ret)
+		return ERR_PTR(-ENOENT);
+
+	match = of_match_node(cdns_dphy_of_match, args.np);
+	if (!match || !match->data)
+		return ERR_PTR(-EINVAL);
+
+	dphy = devm_kzalloc(&pdev->dev, sizeof(*dphy), GFP_KERNEL);
+	if (!dphy)
+		return ERR_PTR(-ENOMEM);
+
+	dphy->ops = match->data;
+
+	ret = of_address_to_resource(args.np, 0, &res);
+	if (ret)
+		return ERR_PTR(ret);
+
+	dphy->regs = devm_ioremap_resource(&pdev->dev, &res);
+	if (IS_ERR(dphy->regs))
+		return ERR_CAST(dphy->regs);
+
+	dphy->psm_clk = of_clk_get_by_name(args.np, "psm");
+	if (IS_ERR(dphy->psm_clk))
+		return ERR_CAST(dphy->psm_clk);
+
+	if (dphy->ops->probe) {
+		ret = dphy->ops->probe(dphy);
+	if (ret)
+		goto err_put_psm_clk;
+	}
+
+	return dphy;
+
+err_put_psm_clk:
+	clk_put(dphy->psm_clk);
+
+	return ERR_PTR(ret);
+}
+
+static void cdns_dphy_remove(struct cdns_dphy *dphy)
+{
+	if (dphy->ops->remove)
+		dphy->ops->remove(dphy);
+
+	clk_put(dphy->psm_clk);
+}
+
 static int csi2rx_probe(struct platform_device *pdev)
 {
 	struct csi2rx_priv *csi2rx;
@@ -455,10 +735,13 @@ static int csi2rx_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto err_free_priv;
 
+	csi2rx->dphy = cdns_dphy_probe(pdev);
+	if (IS_ERR(csi2rx->dphy))
+		return PTR_ERR(csi2rx->dphy);
+
 	dev_info(&pdev->dev,
-		 "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
-		 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams,
-		 csi2rx->has_internal_dphy ? "internal" : "no");
+		 "Probed CSI2RX with %u/%u lanes, %u streams\n",
+		 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams);
 
 	return 0;
 
@@ -472,6 +755,7 @@ static int csi2rx_remove(struct platform_device *pdev)
 	struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev);
 
 	v4l2_async_unregister_subdev(&csi2rx->subdev);
+	cdns_dphy_remove(csi2rx->dphy);
 	kfree(csi2rx);
 
 	return 0;
-- 
2.15.0

Re: [PATCH 2/2] added support for csirx dphy

[Maxime Ripard]

[-- Attachment #1: Type: text/plain, Size: 8429 bytes --]

Hi,

On Fri, Jun 08, 2018 at 11:33:04AM +0100, Krzysztof Witos wrote:
> Signed-off-by: Krzysztof Witos <kwitos@cadence.com>

A commit log explaining what you're doing here would be nice.

> ---
>  drivers/media/platform/cadence/cdns-csi2rx.c | 342 ++++++++++++++++++++++++---
>  1 file changed, 313 insertions(+), 29 deletions(-)
> 
> diff --git a/drivers/media/platform/cadence/cdns-csi2rx.c b/drivers/media/platform/cadence/cdns-csi2rx.c
> index a0f02916006b..9251ea6015f0 100644
> --- a/drivers/media/platform/cadence/cdns-csi2rx.c
> +++ b/drivers/media/platform/cadence/cdns-csi2rx.c
> @@ -2,14 +2,16 @@
>  /*
>   * Driver for Cadence MIPI-CSI2 RX Controller v1.3
>   *
> - * Copyright (C) 2017 Cadence Design Systems Inc.
> + * Copyright (C) 2017,2018 Cadence Design Systems Inc.
>   */
>  
>  #include <linux/clk.h>
> +#include <linux/iopoll.h>
>  #include <linux/delay.h>
>  #include <linux/io.h>
>  #include <linux/module.h>
>  #include <linux/of.h>
> +#include <linux/of_address.h>
>  #include <linux/of_graph.h>
>  #include <linux/phy/phy.h>
>  #include <linux/platform_device.h>
> @@ -44,6 +46,33 @@
>  #define CSI2RX_LANES_MAX	4
>  #define CSI2RX_STREAMS_MAX	4
>  
> +/* DPHY registers */
> +#define DPHY_PMA_CMN(reg)       (reg)
> +#define DPHY_PMA_LCLK(reg)      (0x100 + (reg))
> +#define DPHY_PMA_LDATA(lane, reg)   (0x200 + ((lane) * 0x100) + (reg))
> +#define DPHY_PMA_RCLK(reg)      (0x600 + (reg))
> +#define DPHY_PMA_RDATA(lane, reg)   (0x700 + ((lane) * 0x100) + (reg))
> +#define DPHY_PCS(reg)           (0xb00 + (reg))
> +
> +#define DPHY_CMN_SSM            DPHY_PMA_CMN(0x20)
> +#define DPHY_CMN_SSM_EN         BIT(0)
> +#define DPHY_CMN_RX_MODE_EN     BIT(10)
> +
> +#define DPHY_CMN_PWM            DPHY_PMA_CMN(0x40)
> +#define DPHY_CMN_PWM_DIV(x)     ((x) << 20)
> +#define DPHY_CMN_PWM_LOW(x)     ((x) << 10)
> +#define DPHY_CMN_PWM_HIGH(x)        (x)
> +
> +#define DPHY_CMN_PLL_CFG	DPHY_PMA_CMN(0xE8)
> +#define PLL_LOCKED		BIT(2)
> +
> +#define DPHY_PSM_CFG            DPHY_PCS(0x4)
> +#define DPHY_PSM_CFG_FROM_REG       BIT(0)
> +#define DPHY_PSM_CLK_DIV(x)     ((x) << 1)
> +
> +#define DPHY_BAND_CTRL          DPHY_PCS(0x0)
> +#define DPHY_BAND_LEFT_VAL(x)	(x)
> +
>  enum csi2rx_pads {
>  	CSI2RX_PAD_SINK,
>  	CSI2RX_PAD_SOURCE_STREAM0,
> @@ -67,7 +96,7 @@ struct csi2rx_priv {
>  	struct clk			*sys_clk;
>  	struct clk			*p_clk;
>  	struct clk			*pixel_clk[CSI2RX_STREAMS_MAX];
> -	struct phy			*dphy;
> +	struct clk			*hs_clk;
>  
>  	u8				lanes[CSI2RX_LANES_MAX];
>  	u8				num_lanes;
> @@ -83,8 +112,175 @@ struct csi2rx_priv {
>  	struct v4l2_async_subdev	asd;
>  	struct v4l2_subdev		*source_subdev;
>  	int				source_pad;
> +	struct cdns_dphy		*dphy;
> +};
> +
> +struct cdns_dphy_cfg {
> +	unsigned int nlanes;
> +};
> +
> +struct cdns_dphy;
> +
> +enum cdns_dphy_clk_lane_cfg {
> +	DPHY_CLK_CFG_LEFT_DRIVES_ALL = 0,
> +	DPHY_CLK_CFG_LEFT_DRIVES_RIGHT = 1,
> +	DPHY_CLK_CFG_LEFT_DRIVES_LEFT = 2,
> +	DPHY_CLK_CFG_RIGHT_DRIVES_ALL = 3
> +};
> +
> +struct cdns_dphy_ops {
> +	int (*probe)(struct cdns_dphy *dphy);
> +	void (*remove)(struct cdns_dphy *dphy);
> +	void (*set_psm_div)(struct cdns_dphy *dphy, u8 div);
> +	void (*set_pll_cfg)(struct cdns_dphy *dphy);
> +	void (*set_clk_lane_cfg)(struct cdns_dphy *dphy,
> +		enum cdns_dphy_clk_lane_cfg cfg);
> +	void (*is_pll_locked)(struct cdns_dphy *dphy);
> +	void (*set_band_ctrl)(struct cdns_dphy *dphy, u8 value);
> +};
> +
> +struct cdns_dphy {
> +	struct cdns_dphy_cfg cfg;
> +	void __iomem *regs;
> +	struct clk *psm_clk;
> +	const struct cdns_dphy_ops *ops;
>  };
>  
> +static int cdns_dphy_set_band_ctrl(struct cdns_dphy *dphy,
> +	struct csi2rx_priv *csirx)
> +{
> +	u8 band_value;
> +	u32 hs_freq_mhz = clk_get_rate(csirx->hs_clk);
> +
> +	if (hs_freq_mhz >= 80 && hs_freq_mhz < 100)
> +		band_value = 0;
> +	else if (hs_freq_mhz >= 100 && hs_freq_mhz < 120)
> +		band_value = 1;
> +	else if (hs_freq_mhz >= 120 && hs_freq_mhz < 160)
> +		band_value = 2;
> +	else if (hs_freq_mhz >= 160 && hs_freq_mhz < 200)
> +		band_value = 3;
> +	else if (hs_freq_mhz >= 200 && hs_freq_mhz < 240)
> +		band_value = 4;
> +	else if (hs_freq_mhz >= 240 && hs_freq_mhz < 280)
> +		band_value = 5;
> +	else if (hs_freq_mhz >= 280 && hs_freq_mhz < 320)
> +		band_value = 6;
> +	else if (hs_freq_mhz >= 320 && hs_freq_mhz < 360)
> +		band_value = 7;
> +	else if (hs_freq_mhz >= 360 && hs_freq_mhz < 400)
> +		band_value = 8;
> +	else if (hs_freq_mhz >= 400 && hs_freq_mhz < 480)
> +		band_value = 9;
> +	else if (hs_freq_mhz >= 480 && hs_freq_mhz < 560)
> +		band_value = 10;
> +	else if (hs_freq_mhz >= 560 && hs_freq_mhz < 640)
> +		band_value = 11;
> +	else if (hs_freq_mhz >= 640 && hs_freq_mhz < 720)
> +		band_value = 12;
> +	else if (hs_freq_mhz >= 720 && hs_freq_mhz < 800)
> +		band_value = 13;
> +	else if (hs_freq_mhz >= 800 && hs_freq_mhz < 880)
> +		band_value = 14;
> +	else if (hs_freq_mhz >= 880 && hs_freq_mhz < 1040)
> +		band_value = 15;
> +	else if (hs_freq_mhz >= 1040 && hs_freq_mhz < 1200)
> +		band_value = 16;
> +	else if (hs_freq_mhz >= 1200 && hs_freq_mhz < 1350)
> +		band_value = 17;
> +	else if (hs_freq_mhz >= 1350 && hs_freq_mhz < 1500)
> +		band_value = 18;
> +	else if (hs_freq_mhz >= 1500 && hs_freq_mhz < 1750)
> +		band_value = 19;
> +	else if (hs_freq_mhz >= 1750 && hs_freq_mhz < 2000)
> +		band_value = 20;
> +	else if (hs_freq_mhz >= 2000 && hs_freq_mhz < 2250)
> +		band_value = 21;
> +	else if (hs_freq_mhz >= 2250 && hs_freq_mhz <= 2500)
> +		band_value = 22;
> +	else
> +		return -EINVAL;
> +
> +	if (dphy->ops->set_band_ctrl)
> +		dphy->ops->set_band_ctrl(dphy, band_value);
> +
> +	return 0;
> +}
> +
> +static int cdns_dphy_setup_psm(struct cdns_dphy *dphy)
> +{
> +	unsigned long psm_clk_hz = clk_get_rate(dphy->psm_clk);
> +	unsigned long psm_div;
> +
> +	if (!psm_clk_hz || psm_clk_hz > 100000000)
> +		return -EINVAL;
> +
> +	psm_div = DIV_ROUND_CLOSEST(psm_clk_hz, 1000000);
> +	if (dphy->ops->set_psm_div)
> +		dphy->ops->set_psm_div(dphy, psm_div);
> +
> +	return 0;
> +}
> +
> +static void cdns_dphy_set_clk_lane_cfg(struct cdns_dphy *dphy,
> +	enum cdns_dphy_clk_lane_cfg cfg)
> +{
> +	if (dphy->ops->set_clk_lane_cfg)
> +		dphy->ops->set_clk_lane_cfg(dphy, cfg);
> +}
> +
> +static void cdns_dphy_set_pll_cfg(struct cdns_dphy *dphy)
> +{
> +	if (dphy->ops->set_pll_cfg)
> +		dphy->ops->set_pll_cfg(dphy);
> +}
> +
> +static void cdns_dphy_is_pll_locked(struct cdns_dphy *dphy)
> +{
> +	if (dphy->ops->is_pll_locked)
> +		dphy->ops->is_pll_locked(dphy);
> +}
> +
> +static void cdns_csirx_dphy_init(struct csi2rx_priv *csi2rx,
> +	const struct cdns_dphy_cfg *dphy_cfg)
> +{
> +
> +	/*
> +	 * Configure the band control settings.
> +	 */
> +	cdns_dphy_set_band_ctrl(csi2rx->dphy, csi2rx);
> +
> +	/*
> +	 * Configure the internal PSM clk divider so that the DPHY has a
> +	 * 1MHz clk (or something close).
> +	 */
> +	WARN_ON_ONCE(cdns_dphy_setup_psm(csi2rx->dphy));
> +
> +	/*
> +	 * Configure attach clk lanes to data lanes: the DPHY has 2 clk lanes
> +	 * and 8 data lanes, each clk lane can be attache different set of
> +	 * data lanes. The 2 groups are named 'left' and 'right', so here we
> +	 * just say that we want the 'left' clk lane to drive the 'left' data
> +	 * lanes.
> +	 */
> +	cdns_dphy_set_clk_lane_cfg(csi2rx->dphy,
> +		DPHY_CLK_CFG_LEFT_DRIVES_LEFT);
> +
> +	/*
> +	 * Configure the DPHY PLL that will be used to generate the TX byte
> +	 * clk.
> +	 */
> +	cdns_dphy_set_pll_cfg(csi2rx->dphy);
> +
> +	/*  Start RX state machine. */
> +	writel(DPHY_CMN_SSM_EN | DPHY_CMN_RX_MODE_EN,
> +		csi2rx->dphy->regs + DPHY_CMN_SSM);
> +
> +	/* Checking if PLL is locked */
> +	cdns_dphy_is_pll_locked(csi2rx->dphy);
> +
> +}
> +

That part looks like it's pretty much the same thing than the PHY
support in the DSI bridge found there:
https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/tree/drivers/gpu/drm/bridge/cdns-dsi.c#n493

This code shouldn't be duplicated, but shared, ideally through the phy
framework. That would require some changes to that framework though.

Maxime

-- 
Maxime Ripard, Bootlin (formerly Free Electrons)
Embedded Linux and Kernel engineering
https://bootlin.com

[-- Attachment #2: signature.asc --]
[-- Type: application/pgp-signature, Size: 833 bytes --]

RE: [PATCH 2/2] added support for csirx dphy

[Krzysztof Witos]

Hi Maxime,

Thank you very much for the review. Yes, I understand your point. 
This needs to be changed.

> Hi,
> 
> On Fri, Jun 08, 2018 at 11:33:04AM +0100, Krzysztof Witos wrote:
> > Signed-off-by: Krzysztof Witos <kwitos@cadence.com>
> 
> A commit log explaining what you're doing here would be nice.
> 
> > ---
> >  drivers/media/platform/cadence/cdns-csi2rx.c | 342
> > ++++++++++++++++++++++++---
> >  1 file changed, 313 insertions(+), 29 deletions(-)
> >
> > diff --git a/drivers/media/platform/cadence/cdns-csi2rx.c
> > b/drivers/media/platform/cadence/cdns-csi2rx.c
> > index a0f02916006b..9251ea6015f0 100644
> > --- a/drivers/media/platform/cadence/cdns-csi2rx.c
> > +++ b/drivers/media/platform/cadence/cdns-csi2rx.c
> > @@ -2,14 +2,16 @@
> >  /*
> >   * Driver for Cadence MIPI-CSI2 RX Controller v1.3
> >   *
> > - * Copyright (C) 2017 Cadence Design Systems Inc.
> > + * Copyright (C) 2017,2018 Cadence Design Systems Inc.
> >   */
> >
> >  #include <linux/clk.h>
> > +#include <linux/iopoll.h>
> >  #include <linux/delay.h>
> >  #include <linux/io.h>
> >  #include <linux/module.h>
> >  #include <linux/of.h>
> > +#include <linux/of_address.h>
> >  #include <linux/of_graph.h>
> >  #include <linux/phy/phy.h>
> >  #include <linux/platform_device.h>
> > @@ -44,6 +46,33 @@
> >  #define CSI2RX_LANES_MAX	4
> >  #define CSI2RX_STREAMS_MAX	4
> >
> > +/* DPHY registers */
> > +#define DPHY_PMA_CMN(reg)       (reg)
> > +#define DPHY_PMA_LCLK(reg)      (0x100 + (reg))
> > +#define DPHY_PMA_LDATA(lane, reg)   (0x200 + ((lane) * 0x100) + (reg))
> > +#define DPHY_PMA_RCLK(reg)      (0x600 + (reg))
> > +#define DPHY_PMA_RDATA(lane, reg)   (0x700 + ((lane) * 0x100) + (reg))
> > +#define DPHY_PCS(reg)           (0xb00 + (reg))
> > +
> > +#define DPHY_CMN_SSM            DPHY_PMA_CMN(0x20)
> > +#define DPHY_CMN_SSM_EN         BIT(0)
> > +#define DPHY_CMN_RX_MODE_EN     BIT(10)
> > +
> > +#define DPHY_CMN_PWM            DPHY_PMA_CMN(0x40)
> > +#define DPHY_CMN_PWM_DIV(x)     ((x) << 20)
> > +#define DPHY_CMN_PWM_LOW(x)     ((x) << 10)
> > +#define DPHY_CMN_PWM_HIGH(x)        (x)
> > +
> > +#define DPHY_CMN_PLL_CFG	DPHY_PMA_CMN(0xE8)
> > +#define PLL_LOCKED		BIT(2)
> > +
> > +#define DPHY_PSM_CFG            DPHY_PCS(0x4)
> > +#define DPHY_PSM_CFG_FROM_REG       BIT(0)
> > +#define DPHY_PSM_CLK_DIV(x)     ((x) << 1)
> > +
> > +#define DPHY_BAND_CTRL          DPHY_PCS(0x0)
> > +#define DPHY_BAND_LEFT_VAL(x)	(x)
> > +
> >  enum csi2rx_pads {
> >  	CSI2RX_PAD_SINK,
> >  	CSI2RX_PAD_SOURCE_STREAM0,
> > @@ -67,7 +96,7 @@ struct csi2rx_priv {
> >  	struct clk			*sys_clk;
> >  	struct clk			*p_clk;
> >  	struct clk			*pixel_clk[CSI2RX_STREAMS_MAX];
> > -	struct phy			*dphy;
> > +	struct clk			*hs_clk;
> >
> >  	u8				lanes[CSI2RX_LANES_MAX];
> >  	u8				num_lanes;
> > @@ -83,8 +112,175 @@ struct csi2rx_priv {
> >  	struct v4l2_async_subdev	asd;
> >  	struct v4l2_subdev		*source_subdev;
> >  	int				source_pad;
> > +	struct cdns_dphy		*dphy;
> > +};
> > +
> > +struct cdns_dphy_cfg {
> > +	unsigned int nlanes;
> > +};
> > +
> > +struct cdns_dphy;
> > +
> > +enum cdns_dphy_clk_lane_cfg {
> > +	DPHY_CLK_CFG_LEFT_DRIVES_ALL = 0,
> > +	DPHY_CLK_CFG_LEFT_DRIVES_RIGHT = 1,
> > +	DPHY_CLK_CFG_LEFT_DRIVES_LEFT = 2,
> > +	DPHY_CLK_CFG_RIGHT_DRIVES_ALL = 3
> > +};
> > +
> > +struct cdns_dphy_ops {
> > +	int (*probe)(struct cdns_dphy *dphy);
> > +	void (*remove)(struct cdns_dphy *dphy);
> > +	void (*set_psm_div)(struct cdns_dphy *dphy, u8 div);
> > +	void (*set_pll_cfg)(struct cdns_dphy *dphy);
> > +	void (*set_clk_lane_cfg)(struct cdns_dphy *dphy,
> > +		enum cdns_dphy_clk_lane_cfg cfg);
> > +	void (*is_pll_locked)(struct cdns_dphy *dphy);
> > +	void (*set_band_ctrl)(struct cdns_dphy *dphy, u8 value); };
> > +
> > +struct cdns_dphy {
> > +	struct cdns_dphy_cfg cfg;
> > +	void __iomem *regs;
> > +	struct clk *psm_clk;
> > +	const struct cdns_dphy_ops *ops;
> >  };
> >
> > +static int cdns_dphy_set_band_ctrl(struct cdns_dphy *dphy,
> > +	struct csi2rx_priv *csirx)
> > +{
> > +	u8 band_value;
> > +	u32 hs_freq_mhz = clk_get_rate(csirx->hs_clk);
> > +
> > +	if (hs_freq_mhz >= 80 && hs_freq_mhz < 100)
> > +		band_value = 0;
> > +	else if (hs_freq_mhz >= 100 && hs_freq_mhz < 120)
> > +		band_value = 1;
> > +	else if (hs_freq_mhz >= 120 && hs_freq_mhz < 160)
> > +		band_value = 2;
> > +	else if (hs_freq_mhz >= 160 && hs_freq_mhz < 200)
> > +		band_value = 3;
> > +	else if (hs_freq_mhz >= 200 && hs_freq_mhz < 240)
> > +		band_value = 4;
> > +	else if (hs_freq_mhz >= 240 && hs_freq_mhz < 280)
> > +		band_value = 5;
> > +	else if (hs_freq_mhz >= 280 && hs_freq_mhz < 320)
> > +		band_value = 6;
> > +	else if (hs_freq_mhz >= 320 && hs_freq_mhz < 360)
> > +		band_value = 7;
> > +	else if (hs_freq_mhz >= 360 && hs_freq_mhz < 400)
> > +		band_value = 8;
> > +	else if (hs_freq_mhz >= 400 && hs_freq_mhz < 480)
> > +		band_value = 9;
> > +	else if (hs_freq_mhz >= 480 && hs_freq_mhz < 560)
> > +		band_value = 10;
> > +	else if (hs_freq_mhz >= 560 && hs_freq_mhz < 640)
> > +		band_value = 11;
> > +	else if (hs_freq_mhz >= 640 && hs_freq_mhz < 720)
> > +		band_value = 12;
> > +	else if (hs_freq_mhz >= 720 && hs_freq_mhz < 800)
> > +		band_value = 13;
> > +	else if (hs_freq_mhz >= 800 && hs_freq_mhz < 880)
> > +		band_value = 14;
> > +	else if (hs_freq_mhz >= 880 && hs_freq_mhz < 1040)
> > +		band_value = 15;
> > +	else if (hs_freq_mhz >= 1040 && hs_freq_mhz < 1200)
> > +		band_value = 16;
> > +	else if (hs_freq_mhz >= 1200 && hs_freq_mhz < 1350)
> > +		band_value = 17;
> > +	else if (hs_freq_mhz >= 1350 && hs_freq_mhz < 1500)
> > +		band_value = 18;
> > +	else if (hs_freq_mhz >= 1500 && hs_freq_mhz < 1750)
> > +		band_value = 19;
> > +	else if (hs_freq_mhz >= 1750 && hs_freq_mhz < 2000)
> > +		band_value = 20;
> > +	else if (hs_freq_mhz >= 2000 && hs_freq_mhz < 2250)
> > +		band_value = 21;
> > +	else if (hs_freq_mhz >= 2250 && hs_freq_mhz <= 2500)
> > +		band_value = 22;
> > +	else
> > +		return -EINVAL;
> > +
> > +	if (dphy->ops->set_band_ctrl)
> > +		dphy->ops->set_band_ctrl(dphy, band_value);
> > +
> > +	return 0;
> > +}
> > +
> > +static int cdns_dphy_setup_psm(struct cdns_dphy *dphy) {
> > +	unsigned long psm_clk_hz = clk_get_rate(dphy->psm_clk);
> > +	unsigned long psm_div;
> > +
> > +	if (!psm_clk_hz || psm_clk_hz > 100000000)
> > +		return -EINVAL;
> > +
> > +	psm_div = DIV_ROUND_CLOSEST(psm_clk_hz, 1000000);
> > +	if (dphy->ops->set_psm_div)
> > +		dphy->ops->set_psm_div(dphy, psm_div);
> > +
> > +	return 0;
> > +}
> > +
> > +static void cdns_dphy_set_clk_lane_cfg(struct cdns_dphy *dphy,
> > +	enum cdns_dphy_clk_lane_cfg cfg)
> > +{
> > +	if (dphy->ops->set_clk_lane_cfg)
> > +		dphy->ops->set_clk_lane_cfg(dphy, cfg); }
> > +
> > +static void cdns_dphy_set_pll_cfg(struct cdns_dphy *dphy) {
> > +	if (dphy->ops->set_pll_cfg)
> > +		dphy->ops->set_pll_cfg(dphy);
> > +}
> > +
> > +static void cdns_dphy_is_pll_locked(struct cdns_dphy *dphy) {
> > +	if (dphy->ops->is_pll_locked)
> > +		dphy->ops->is_pll_locked(dphy);
> > +}
> > +
> > +static void cdns_csirx_dphy_init(struct csi2rx_priv *csi2rx,
> > +	const struct cdns_dphy_cfg *dphy_cfg) {
> > +
> > +	/*
> > +	 * Configure the band control settings.
> > +	 */
> > +	cdns_dphy_set_band_ctrl(csi2rx->dphy, csi2rx);
> > +
> > +	/*
> > +	 * Configure the internal PSM clk divider so that the DPHY has a
> > +	 * 1MHz clk (or something close).
> > +	 */
> > +	WARN_ON_ONCE(cdns_dphy_setup_psm(csi2rx->dphy));
> > +
> > +	/*
> > +	 * Configure attach clk lanes to data lanes: the DPHY has 2 clk lanes
> > +	 * and 8 data lanes, each clk lane can be attache different set of
> > +	 * data lanes. The 2 groups are named 'left' and 'right', so here we
> > +	 * just say that we want the 'left' clk lane to drive the 'left' data
> > +	 * lanes.
> > +	 */
> > +	cdns_dphy_set_clk_lane_cfg(csi2rx->dphy,
> > +		DPHY_CLK_CFG_LEFT_DRIVES_LEFT);
> > +
> > +	/*
> > +	 * Configure the DPHY PLL that will be used to generate the TX byte
> > +	 * clk.
> > +	 */
> > +	cdns_dphy_set_pll_cfg(csi2rx->dphy);
> > +
> > +	/*  Start RX state machine. */
> > +	writel(DPHY_CMN_SSM_EN | DPHY_CMN_RX_MODE_EN,
> > +		csi2rx->dphy->regs + DPHY_CMN_SSM);
> > +
> > +	/* Checking if PLL is locked */
> > +	cdns_dphy_is_pll_locked(csi2rx->dphy);
> > +
> > +}
> > +
> 
> That part looks like it's pretty much the same thing than the PHY support in
> the DSI bridge found there:
> https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/tree/drivers
> /gpu/drm/bridge/cdns-dsi.c#n493
> 
> This code shouldn't be duplicated, but shared, ideally through the phy
> framework. That would require some changes to that framework though.
> 
> Maxime
> 
> --
> Maxime Ripard, Bootlin (formerly Free Electrons) Embedded Linux and Kernel
> engineering https://bootlin.com


Best Regards,
Krzysztof