TPG is connected to the csid as an entity, the link
needs to be adapted.
Signed-off-by: Wenmeng Liu <quic_wenmliu@xxxxxxxxxxx>
---
drivers/media/platform/qcom/camss/camss-csid.c | 44 +++++++++++++++++-----
drivers/media/platform/qcom/camss/camss.c | 52 ++++++++++++++++++++++++++
2 files changed, 87 insertions(+), 9 deletions(-)
diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
index 5284b5857368c37c202cd89dad6ae8042b637537..1ee4c4cc61cb32ce731dd8123522cc729d1ae3bb 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.c
+++ b/drivers/media/platform/qcom/camss/camss-csid.c
@@ -1226,6 +1226,23 @@ void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
*id = csid->id;
}
+/*
+ * csid_get_csiphy_tpg_lane_assign - Calculate lane assign by tpg lane num
+ * @num - tpg lane num
+ *
+ * Return lane assign
+ */
+static u32 csid_get_csiphy_tpg_lane_assign(int num)
+{
+ u32 lane_assign = 0;
+ int i;
+
+ for (i = (num - 1); i >= 0; i--)
+ lane_assign |= i << (i * 4);
+
+ return lane_assign;
+}
+
/*
* csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
* @lane_cfg - CSI2 lane configuration
@@ -1266,6 +1283,7 @@ static int csid_link_setup(struct media_entity *entity,
struct csid_device *csid;
struct csiphy_device *csiphy;
struct csiphy_lanes_cfg *lane_cfg;
+ struct tpg_device *tpg;
sd = media_entity_to_v4l2_subdev(entity);
csid = v4l2_get_subdevdata(sd);
@@ -1277,18 +1295,26 @@ static int csid_link_setup(struct media_entity *entity,
return -EBUSY;
sd = media_entity_to_v4l2_subdev(remote->entity);
- csiphy = v4l2_get_subdevdata(sd);
+ if (strnstr(sd->name, MSM_TPG_NAME, strlen(MSM_TPG_NAME))) {
+ tpg = v4l2_get_subdevdata(sd);
- /* If a sensor is not linked to CSIPHY */
- /* do no allow a link from CSIPHY to CSID */
- if (!csiphy->cfg.csi2)
- return -EPERM;
+ csid->phy.lane_cnt = tpg->res->lane_cnt;
+ csid->phy.csiphy_id = tpg->id;
+ csid->phy.lane_assign = csid_get_csiphy_tpg_lane_assign(csid->phy.lane_cnt);
+ } else {
+ csiphy = v4l2_get_subdevdata(sd);
+
+ /* If a sensor is not linked to CSIPHY */
+ /* do no allow a link from CSIPHY to CSID */
+ if (!csiphy->cfg.csi2)
+ return -EPERM;
- csid->phy.csiphy_id = csiphy->id;
+ csid->phy.csiphy_id = csiphy->id;
- lane_cfg = &csiphy->cfg.csi2->lane_cfg;
- csid->phy.lane_cnt = lane_cfg->num_data;
- csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
+ lane_cfg = &csiphy->cfg.csi2->lane_cfg;
+ csid->phy.lane_cnt = lane_cfg->num_data;
+ csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
+ }
}
/* Decide which virtual channels to enable based on which source pads are enabled */
if (local->flags & MEDIA_PAD_FL_SOURCE) {
diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index af5c9326736f9c8576816c91b73ad3e1d3a49dbf..34f71039038e881ced9c9f06bd70915b5c5f610f 100644
--- a/drivers/media/platform/qcom/camss/camss.c
+++ b/drivers/media/platform/qcom/camss/camss.c
@@ -3913,6 +3913,19 @@ static int camss_init_subdevices(struct camss *camss)
}
}
+ if (camss->tpg) {
+ for (i = 0; i < camss->res->tpg_num; i++) {
+ ret = msm_tpg_subdev_init(camss, &camss->tpg[i],
+ &res->tpg_res[i], i);
+ if (ret < 0) {
+ dev_err(camss->dev,
+ "Failed to init tpg%d sub-device: %d\n",
+ i, ret);
+ return ret;
+ }
+ }
+ }