diff mbox series

[v3,1/4] media: camss: csiphy: Move to hardcode CSI Clock Lane number

Message ID 20211118124819.1902427-2-robert.foss@linaro.org (mailing list archive)
State Superseded
Headers show
Series Remove clock-lanes DT property from CAMSS | expand

Commit Message

Robert Foss Nov. 18, 2021, 12:48 p.m. UTC 
QCOM ISPs do not support having a programmable CSI Clock Lane number.

In order to accurately reflect this, the different CSIPHY HW versions
need to have their own register layer for computing lane masks.

Signed-off-by: Robert Foss <robert.foss@linaro.org>
---
 .../qcom/camss/camss-csiphy-2ph-1-0.c         | 19 +++++++++++++++--
 .../qcom/camss/camss-csiphy-3ph-1-0.c         | 17 ++++++++++++++-
 .../media/platform/qcom/camss/camss-csiphy.c  | 21 +------------------
 .../media/platform/qcom/camss/camss-csiphy.h  |  7 +++++++
 4 files changed, 41 insertions(+), 23 deletions(-)

Comments

Bjorn Andersson Nov. 19, 2021, 4:01 a.m. UTC | #1 
On Thu 18 Nov 06:48 CST 2021, Robert Foss wrote:

> QCOM ISPs do not support having a programmable CSI Clock Lane number.
> 
> In order to accurately reflect this, the different CSIPHY HW versions
> need to have their own register layer for computing lane masks.
> 
> Signed-off-by: Robert Foss <robert.foss@linaro.org>

Reviewed-by: Bjorn Andersson <bjorn.andersson@linaro.org>

Regards,
Bjorn

> ---
>  .../qcom/camss/camss-csiphy-2ph-1-0.c         | 19 +++++++++++++++--
>  .../qcom/camss/camss-csiphy-3ph-1-0.c         | 17 ++++++++++++++-
>  .../media/platform/qcom/camss/camss-csiphy.c  | 21 +------------------
>  .../media/platform/qcom/camss/camss-csiphy.h  |  7 +++++++
>  4 files changed, 41 insertions(+), 23 deletions(-)
> 
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
> index 30b454c369ab..cd4a8c369234 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
> @@ -16,6 +16,7 @@
>  
>  #define CAMSS_CSI_PHY_LNn_CFG2(n)		(0x004 + 0x40 * (n))
>  #define CAMSS_CSI_PHY_LNn_CFG3(n)		(0x008 + 0x40 * (n))
> +#define		CAMSS_CSI_PHY_LN_CLK		1
>  #define CAMSS_CSI_PHY_GLBL_RESET		0x140
>  #define CAMSS_CSI_PHY_GLBL_PWR_CFG		0x144
>  #define CAMSS_CSI_PHY_GLBL_IRQ_CMD		0x164
> @@ -26,6 +27,19 @@
>  #define CAMSS_CSI_PHY_GLBL_T_INIT_CFG0		0x1ec
>  #define CAMSS_CSI_PHY_T_WAKEUP_CFG0		0x1f4
>  
> +static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
> +{
> +	u8 lane_mask;
> +	int i;
> +
> +	lane_mask = 1 << CAMSS_CSI_PHY_LN_CLK;
> +
> +	for (i = 0; i < lane_cfg->num_data; i++)
> +		lane_mask |= 1 << lane_cfg->data[i].pos;
> +
> +	return lane_mask;
> +}
> +
>  static void csiphy_hw_version_read(struct csiphy_device *csiphy,
>  				   struct device *dev)
>  {
> @@ -105,7 +119,7 @@ static void csiphy_lanes_enable(struct csiphy_device *csiphy,
>  
>  	for (i = 0; i <= c->num_data; i++) {
>  		if (i == c->num_data)
> -			l = c->clk.pos;
> +			l = CAMSS_CSI_PHY_LN_CLK;
>  		else
>  			l = c->data[i].pos;
>  
> @@ -129,7 +143,7 @@ static void csiphy_lanes_disable(struct csiphy_device *csiphy,
>  
>  	for (i = 0; i <= c->num_data; i++) {
>  		if (i == c->num_data)
> -			l = c->clk.pos;
> +			l = CAMSS_CSI_PHY_LN_CLK;
>  		else
>  			l = c->data[i].pos;
>  
> @@ -167,6 +181,7 @@ static irqreturn_t csiphy_isr(int irq, void *dev)
>  }
>  
>  const struct csiphy_hw_ops csiphy_ops_2ph_1_0 = {
> +	.get_lane_mask = csiphy_get_lane_mask,
>  	.hw_version_read = csiphy_hw_version_read,
>  	.reset = csiphy_reset,
>  	.lanes_enable = csiphy_lanes_enable,
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
> index e318c822ab04..cde6b3a10b9e 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
> @@ -43,6 +43,7 @@
>  #define CSIPHY_3PH_LNn_CSI_LANE_CTRL15_SWI_SOT_SYMBOL	0xb8
>  
>  #define CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(n)	(0x800 + 0x4 * (n))
> +#define CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE	BIT(7)
>  #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_COMMON_PWRDN_B	BIT(0)
>  #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_SHOW_REV_ID	BIT(1)
>  #define CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(n)	(0x8b0 + 0x4 * (n))
> @@ -320,6 +321,19 @@ static void csiphy_gen2_config_lanes(struct csiphy_device *csiphy,
>  	}
>  }
>  
> +static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
> +{
> +	u8 lane_mask;
> +	int i;
> +
> +	lane_mask = CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE;
> +
> +	for (i = 0; i < lane_cfg->num_data; i++)
> +		lane_mask |= 1 << lane_cfg->data[i].pos;
> +
> +	return lane_mask;
> +}
> +
>  static void csiphy_lanes_enable(struct csiphy_device *csiphy,
>  				struct csiphy_config *cfg,
>  				s64 link_freq, u8 lane_mask)
> @@ -331,7 +345,7 @@ static void csiphy_lanes_enable(struct csiphy_device *csiphy,
>  
>  	settle_cnt = csiphy_settle_cnt_calc(link_freq, csiphy->timer_clk_rate);
>  
> -	val = BIT(c->clk.pos);
> +	val = CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE;
>  	for (i = 0; i < c->num_data; i++)
>  		val |= BIT(c->data[i].pos * 2);
>  
> @@ -397,6 +411,7 @@ static void csiphy_lanes_disable(struct csiphy_device *csiphy,
>  }
>  
>  const struct csiphy_hw_ops csiphy_ops_3ph_1_0 = {
> +	.get_lane_mask = csiphy_get_lane_mask,
>  	.hw_version_read = csiphy_hw_version_read,
>  	.reset = csiphy_reset,
>  	.lanes_enable = csiphy_lanes_enable,
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.c b/drivers/media/platform/qcom/camss/camss-csiphy.c
> index 24eec16197e7..ac7e96e6b7cd 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy.c
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy.c
> @@ -229,25 +229,6 @@ static int csiphy_set_power(struct v4l2_subdev *sd, int on)
>  	return 0;
>  }
>  
> -/*
> - * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
> - * @lane_cfg - CSI2 lane configuration
> - *
> - * Return lane mask
> - */
> -static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
> -{
> -	u8 lane_mask;
> -	int i;
> -
> -	lane_mask = 1 << lane_cfg->clk.pos;
> -
> -	for (i = 0; i < lane_cfg->num_data; i++)
> -		lane_mask |= 1 << lane_cfg->data[i].pos;
> -
> -	return lane_mask;
> -}
> -
>  /*
>   * csiphy_stream_on - Enable streaming on CSIPHY module
>   * @csiphy: CSIPHY device
> @@ -261,7 +242,7 @@ static int csiphy_stream_on(struct csiphy_device *csiphy)
>  {
>  	struct csiphy_config *cfg = &csiphy->cfg;
>  	s64 link_freq;
> -	u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
> +	u8 lane_mask = csiphy->ops->get_lane_mask(&cfg->csi2->lane_cfg);
>  	u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
>  				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
>  	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.h b/drivers/media/platform/qcom/camss/camss-csiphy.h
> index d71b8bc6ec00..1c14947f92d3 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy.h
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy.h
> @@ -45,6 +45,13 @@ struct csiphy_config {
>  struct csiphy_device;
>  
>  struct csiphy_hw_ops {
> +	/*
> +	 * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
> +	 * @lane_cfg - CSI2 lane configuration
> +	 *
> +	 * Return lane mask
> +	 */
> +	u8 (*get_lane_mask)(struct csiphy_lanes_cfg *lane_cfg);
>  	void (*hw_version_read)(struct csiphy_device *csiphy,
>  				struct device *dev);
>  	void (*reset)(struct csiphy_device *csiphy);
> -- 
> 2.32.0
>
Bjorn Andersson Dec. 6, 2021, 3:04 p.m. UTC | #2 
On Thu 18 Nov 04:48 PST 2021, Robert Foss wrote:

> QCOM ISPs do not support having a programmable CSI Clock Lane number.
> 
> In order to accurately reflect this, the different CSIPHY HW versions
> need to have their own register layer for computing lane masks.
> 
> Signed-off-by: Robert Foss <robert.foss@linaro.org>

Reviewed-by: Bjorn Andersson <bjorn.andersson@linaro.org>

Regards,
Bjorn

> ---
>  .../qcom/camss/camss-csiphy-2ph-1-0.c         | 19 +++++++++++++++--
>  .../qcom/camss/camss-csiphy-3ph-1-0.c         | 17 ++++++++++++++-
>  .../media/platform/qcom/camss/camss-csiphy.c  | 21 +------------------
>  .../media/platform/qcom/camss/camss-csiphy.h  |  7 +++++++
>  4 files changed, 41 insertions(+), 23 deletions(-)
> 
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
> index 30b454c369ab..cd4a8c369234 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
> @@ -16,6 +16,7 @@
>  
>  #define CAMSS_CSI_PHY_LNn_CFG2(n)		(0x004 + 0x40 * (n))
>  #define CAMSS_CSI_PHY_LNn_CFG3(n)		(0x008 + 0x40 * (n))
> +#define		CAMSS_CSI_PHY_LN_CLK		1
>  #define CAMSS_CSI_PHY_GLBL_RESET		0x140
>  #define CAMSS_CSI_PHY_GLBL_PWR_CFG		0x144
>  #define CAMSS_CSI_PHY_GLBL_IRQ_CMD		0x164
> @@ -26,6 +27,19 @@
>  #define CAMSS_CSI_PHY_GLBL_T_INIT_CFG0		0x1ec
>  #define CAMSS_CSI_PHY_T_WAKEUP_CFG0		0x1f4
>  
> +static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
> +{
> +	u8 lane_mask;
> +	int i;
> +
> +	lane_mask = 1 << CAMSS_CSI_PHY_LN_CLK;
> +
> +	for (i = 0; i < lane_cfg->num_data; i++)
> +		lane_mask |= 1 << lane_cfg->data[i].pos;
> +
> +	return lane_mask;
> +}
> +
>  static void csiphy_hw_version_read(struct csiphy_device *csiphy,
>  				   struct device *dev)
>  {
> @@ -105,7 +119,7 @@ static void csiphy_lanes_enable(struct csiphy_device *csiphy,
>  
>  	for (i = 0; i <= c->num_data; i++) {
>  		if (i == c->num_data)
> -			l = c->clk.pos;
> +			l = CAMSS_CSI_PHY_LN_CLK;
>  		else
>  			l = c->data[i].pos;
>  
> @@ -129,7 +143,7 @@ static void csiphy_lanes_disable(struct csiphy_device *csiphy,
>  
>  	for (i = 0; i <= c->num_data; i++) {
>  		if (i == c->num_data)
> -			l = c->clk.pos;
> +			l = CAMSS_CSI_PHY_LN_CLK;
>  		else
>  			l = c->data[i].pos;
>  
> @@ -167,6 +181,7 @@ static irqreturn_t csiphy_isr(int irq, void *dev)
>  }
>  
>  const struct csiphy_hw_ops csiphy_ops_2ph_1_0 = {
> +	.get_lane_mask = csiphy_get_lane_mask,
>  	.hw_version_read = csiphy_hw_version_read,
>  	.reset = csiphy_reset,
>  	.lanes_enable = csiphy_lanes_enable,
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
> index e318c822ab04..cde6b3a10b9e 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
> @@ -43,6 +43,7 @@
>  #define CSIPHY_3PH_LNn_CSI_LANE_CTRL15_SWI_SOT_SYMBOL	0xb8
>  
>  #define CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(n)	(0x800 + 0x4 * (n))
> +#define CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE	BIT(7)
>  #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_COMMON_PWRDN_B	BIT(0)
>  #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_SHOW_REV_ID	BIT(1)
>  #define CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(n)	(0x8b0 + 0x4 * (n))
> @@ -320,6 +321,19 @@ static void csiphy_gen2_config_lanes(struct csiphy_device *csiphy,
>  	}
>  }
>  
> +static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
> +{
> +	u8 lane_mask;
> +	int i;
> +
> +	lane_mask = CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE;
> +
> +	for (i = 0; i < lane_cfg->num_data; i++)
> +		lane_mask |= 1 << lane_cfg->data[i].pos;
> +
> +	return lane_mask;
> +}
> +
>  static void csiphy_lanes_enable(struct csiphy_device *csiphy,
>  				struct csiphy_config *cfg,
>  				s64 link_freq, u8 lane_mask)
> @@ -331,7 +345,7 @@ static void csiphy_lanes_enable(struct csiphy_device *csiphy,
>  
>  	settle_cnt = csiphy_settle_cnt_calc(link_freq, csiphy->timer_clk_rate);
>  
> -	val = BIT(c->clk.pos);
> +	val = CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE;
>  	for (i = 0; i < c->num_data; i++)
>  		val |= BIT(c->data[i].pos * 2);
>  
> @@ -397,6 +411,7 @@ static void csiphy_lanes_disable(struct csiphy_device *csiphy,
>  }
>  
>  const struct csiphy_hw_ops csiphy_ops_3ph_1_0 = {
> +	.get_lane_mask = csiphy_get_lane_mask,
>  	.hw_version_read = csiphy_hw_version_read,
>  	.reset = csiphy_reset,
>  	.lanes_enable = csiphy_lanes_enable,
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.c b/drivers/media/platform/qcom/camss/camss-csiphy.c
> index 24eec16197e7..ac7e96e6b7cd 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy.c
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy.c
> @@ -229,25 +229,6 @@ static int csiphy_set_power(struct v4l2_subdev *sd, int on)
>  	return 0;
>  }
>  
> -/*
> - * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
> - * @lane_cfg - CSI2 lane configuration
> - *
> - * Return lane mask
> - */
> -static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
> -{
> -	u8 lane_mask;
> -	int i;
> -
> -	lane_mask = 1 << lane_cfg->clk.pos;
> -
> -	for (i = 0; i < lane_cfg->num_data; i++)
> -		lane_mask |= 1 << lane_cfg->data[i].pos;
> -
> -	return lane_mask;
> -}
> -
>  /*
>   * csiphy_stream_on - Enable streaming on CSIPHY module
>   * @csiphy: CSIPHY device
> @@ -261,7 +242,7 @@ static int csiphy_stream_on(struct csiphy_device *csiphy)
>  {
>  	struct csiphy_config *cfg = &csiphy->cfg;
>  	s64 link_freq;
> -	u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
> +	u8 lane_mask = csiphy->ops->get_lane_mask(&cfg->csi2->lane_cfg);
>  	u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
>  				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
>  	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
> diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.h b/drivers/media/platform/qcom/camss/camss-csiphy.h
> index d71b8bc6ec00..1c14947f92d3 100644
> --- a/drivers/media/platform/qcom/camss/camss-csiphy.h
> +++ b/drivers/media/platform/qcom/camss/camss-csiphy.h
> @@ -45,6 +45,13 @@ struct csiphy_config {
>  struct csiphy_device;
>  
>  struct csiphy_hw_ops {
> +	/*
> +	 * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
> +	 * @lane_cfg - CSI2 lane configuration
> +	 *
> +	 * Return lane mask
> +	 */
> +	u8 (*get_lane_mask)(struct csiphy_lanes_cfg *lane_cfg);
>  	void (*hw_version_read)(struct csiphy_device *csiphy,
>  				struct device *dev);
>  	void (*reset)(struct csiphy_device *csiphy);
> -- 
> 2.32.0
>
diff mbox series

Patch

diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
index 30b454c369ab..cd4a8c369234 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
@@ -16,6 +16,7 @@ 
 
 #define CAMSS_CSI_PHY_LNn_CFG2(n)		(0x004 + 0x40 * (n))
 #define CAMSS_CSI_PHY_LNn_CFG3(n)		(0x008 + 0x40 * (n))
+#define		CAMSS_CSI_PHY_LN_CLK		1
 #define CAMSS_CSI_PHY_GLBL_RESET		0x140
 #define CAMSS_CSI_PHY_GLBL_PWR_CFG		0x144
 #define CAMSS_CSI_PHY_GLBL_IRQ_CMD		0x164
@@ -26,6 +27,19 @@ 
 #define CAMSS_CSI_PHY_GLBL_T_INIT_CFG0		0x1ec
 #define CAMSS_CSI_PHY_T_WAKEUP_CFG0		0x1f4
 
+static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
+{
+	u8 lane_mask;
+	int i;
+
+	lane_mask = 1 << CAMSS_CSI_PHY_LN_CLK;
+
+	for (i = 0; i < lane_cfg->num_data; i++)
+		lane_mask |= 1 << lane_cfg->data[i].pos;
+
+	return lane_mask;
+}
+
 static void csiphy_hw_version_read(struct csiphy_device *csiphy,
 				   struct device *dev)
 {
@@ -105,7 +119,7 @@  static void csiphy_lanes_enable(struct csiphy_device *csiphy,
 
 	for (i = 0; i <= c->num_data; i++) {
 		if (i == c->num_data)
-			l = c->clk.pos;
+			l = CAMSS_CSI_PHY_LN_CLK;
 		else
 			l = c->data[i].pos;
 
@@ -129,7 +143,7 @@  static void csiphy_lanes_disable(struct csiphy_device *csiphy,
 
 	for (i = 0; i <= c->num_data; i++) {
 		if (i == c->num_data)
-			l = c->clk.pos;
+			l = CAMSS_CSI_PHY_LN_CLK;
 		else
 			l = c->data[i].pos;
 
@@ -167,6 +181,7 @@  static irqreturn_t csiphy_isr(int irq, void *dev)
 }
 
 const struct csiphy_hw_ops csiphy_ops_2ph_1_0 = {
+	.get_lane_mask = csiphy_get_lane_mask,
 	.hw_version_read = csiphy_hw_version_read,
 	.reset = csiphy_reset,
 	.lanes_enable = csiphy_lanes_enable,
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
index e318c822ab04..cde6b3a10b9e 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
@@ -43,6 +43,7 @@ 
 #define CSIPHY_3PH_LNn_CSI_LANE_CTRL15_SWI_SOT_SYMBOL	0xb8
 
 #define CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(n)	(0x800 + 0x4 * (n))
+#define CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE	BIT(7)
 #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_COMMON_PWRDN_B	BIT(0)
 #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_SHOW_REV_ID	BIT(1)
 #define CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(n)	(0x8b0 + 0x4 * (n))
@@ -320,6 +321,19 @@  static void csiphy_gen2_config_lanes(struct csiphy_device *csiphy,
 	}
 }
 
+static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
+{
+	u8 lane_mask;
+	int i;
+
+	lane_mask = CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE;
+
+	for (i = 0; i < lane_cfg->num_data; i++)
+		lane_mask |= 1 << lane_cfg->data[i].pos;
+
+	return lane_mask;
+}
+
 static void csiphy_lanes_enable(struct csiphy_device *csiphy,
 				struct csiphy_config *cfg,
 				s64 link_freq, u8 lane_mask)
@@ -331,7 +345,7 @@  static void csiphy_lanes_enable(struct csiphy_device *csiphy,
 
 	settle_cnt = csiphy_settle_cnt_calc(link_freq, csiphy->timer_clk_rate);
 
-	val = BIT(c->clk.pos);
+	val = CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE;
 	for (i = 0; i < c->num_data; i++)
 		val |= BIT(c->data[i].pos * 2);
 
@@ -397,6 +411,7 @@  static void csiphy_lanes_disable(struct csiphy_device *csiphy,
 }
 
 const struct csiphy_hw_ops csiphy_ops_3ph_1_0 = {
+	.get_lane_mask = csiphy_get_lane_mask,
 	.hw_version_read = csiphy_hw_version_read,
 	.reset = csiphy_reset,
 	.lanes_enable = csiphy_lanes_enable,
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.c b/drivers/media/platform/qcom/camss/camss-csiphy.c
index 24eec16197e7..ac7e96e6b7cd 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.c
@@ -229,25 +229,6 @@  static int csiphy_set_power(struct v4l2_subdev *sd, int on)
 	return 0;
 }
 
-/*
- * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
- * @lane_cfg - CSI2 lane configuration
- *
- * Return lane mask
- */
-static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
-{
-	u8 lane_mask;
-	int i;
-
-	lane_mask = 1 << lane_cfg->clk.pos;
-
-	for (i = 0; i < lane_cfg->num_data; i++)
-		lane_mask |= 1 << lane_cfg->data[i].pos;
-
-	return lane_mask;
-}
-
 /*
  * csiphy_stream_on - Enable streaming on CSIPHY module
  * @csiphy: CSIPHY device
@@ -261,7 +242,7 @@  static int csiphy_stream_on(struct csiphy_device *csiphy)
 {
 	struct csiphy_config *cfg = &csiphy->cfg;
 	s64 link_freq;
-	u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
+	u8 lane_mask = csiphy->ops->get_lane_mask(&cfg->csi2->lane_cfg);
 	u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
 				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
 	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.h b/drivers/media/platform/qcom/camss/camss-csiphy.h
index d71b8bc6ec00..1c14947f92d3 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy.h
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.h
@@ -45,6 +45,13 @@  struct csiphy_config {
 struct csiphy_device;
 
 struct csiphy_hw_ops {
+	/*
+	 * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
+	 * @lane_cfg - CSI2 lane configuration
+	 *
+	 * Return lane mask
+	 */
+	u8 (*get_lane_mask)(struct csiphy_lanes_cfg *lane_cfg);
 	void (*hw_version_read)(struct csiphy_device *csiphy,
 				struct device *dev);
 	void (*reset)(struct csiphy_device *csiphy);