media: qcom: camss: Add link support for TPG

TPG is connected to the csid as an entity, the link
needs to be adapted.

Signed-off-by: Wenmeng Liu <wenmeng.liu@oss.qualcomm.com>
Reviewed-by: Bryan O'Donoghue <bryan.odonoghue@linaro.org>
Tested-by: Bryan O'Donoghue <bryan.odonoghue@linaro.org> # Dell Inpsiron14p
Signed-off-by: Bryan O'Donoghue <bod@kernel.org>

diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
index ed1820488c9878df91c173cd4ff0209dfa1e3a8c..48459b46a981bc7504cdde7d6f39fcc4a1e273de 100644 (file)
--- a/drivers/media/platform/qcom/camss/camss-csid.c
+++ b/drivers/media/platform/qcom/camss/camss-csid.c
@@ -35,6 +35,8 @@
 #define                HW_VERSION_REVISION     16
 #define                HW_VERSION_GENERATION   28
 
+#define LANE_CFG_BITWIDTH 4
+
 #define MSM_CSID_NAME "msm_csid"
 
 const char * const csid_testgen_modes[] = {
@@ -1215,18 +1217,22 @@ void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
 }
 
 /*
- * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
- * @lane_cfg - CSI2 lane configuration
+ * csid_get_lane_assign - Calculate lane assign by csiphy/tpg lane num
+ * @lane_cfg: CSI2 lane configuration
+ * @num_lanes: lane num
  *
  * Return lane assign
  */
-static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
+static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg, int num_lanes)
 {
        u32 lane_assign = 0;
+       int pos;
        int i;
 
-       for (i = 0; i < lane_cfg->num_data; i++)
-               lane_assign |= lane_cfg->data[i].pos << (i * 4);
+       for (i = 0; i < num_lanes; i++) {
+               pos = lane_cfg ? lane_cfg->data[i].pos : i;
+               lane_assign |= pos << (i * LANE_CFG_BITWIDTH);
+       }
 
        return lane_assign;
 }
@@ -1251,6 +1257,7 @@ static int csid_link_setup(struct media_entity *entity,
        if ((local->flags & MEDIA_PAD_FL_SINK) &&
            (flags & MEDIA_LNK_FL_ENABLED)) {
                struct v4l2_subdev *sd;
+               struct tpg_device *tpg;
                struct csid_device *csid;
                struct csiphy_device *csiphy;
                struct csiphy_lanes_cfg *lane_cfg;
@@ -1265,18 +1272,28 @@ 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 (sd->grp_id == TPG_GRP_ID) {
+                       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_lane_assign(NULL, csid->phy.lane_cnt);
+                       csid->tpg_linked = true;
+               } else {
+                       csiphy = v4l2_get_subdevdata(sd);
 
-               csid->phy.csiphy_id = csiphy->id;
+                       /* If a sensor is not linked to CSIPHY */
+                       /* do no allow a link from CSIPHY to CSID */
+                       if (!csiphy->cfg.csi2)
+                               return -EPERM;
 
-               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);
+                       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->num_data);
+                       csid->tpg_linked = false;
+               }
        }
        /* 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-csid.h b/drivers/media/platform/qcom/camss/camss-csid.h
index aedc96ed84b2fcc3f352160dcfd31554a671d0fc..5296b10f6bac839a3faa1039bdbf0fbbbe9456ac 100644 (file)
--- a/drivers/media/platform/qcom/camss/camss-csid.h
+++ b/drivers/media/platform/qcom/camss/camss-csid.h
@@ -161,6 +161,7 @@ struct csid_device {
        int num_supplies;
        struct completion reset_complete;
        struct csid_testgen_config testgen;
+       bool tpg_linked;
        struct csid_phy_config phy;
        struct v4l2_mbus_framefmt fmt[MSM_CSID_PADS_NUM];
        struct v4l2_ctrl_handler ctrls;

diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.c b/drivers/media/platform/qcom/camss/camss-csiphy.c
index 78a1b568dbae6a9303ac88fad6c31ecba31b5c9f..539ac4888b608b3cc789728365e7e8a39fc54e78 100644 (file)
--- a/drivers/media/platform/qcom/camss/camss-csiphy.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.c
@@ -793,6 +793,7 @@ int msm_csiphy_register_entity(struct csiphy_device *csiphy,
        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
        snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
                 MSM_CSIPHY_NAME, csiphy->id);
+       sd->grp_id = CSIPHY_GRP_ID;
        v4l2_set_subdevdata(sd, csiphy);
 
        ret = csiphy_init_formats(sd, NULL);

diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.h b/drivers/media/platform/qcom/camss/camss-csiphy.h
index 2d5054819df7f9069611bcdf287846b1d20afc92..9d9657b82f748d02bf6be6139480cfbd0c5001c9 100644 (file)
--- a/drivers/media/platform/qcom/camss/camss-csiphy.h
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.h
@@ -21,6 +21,8 @@
 #define MSM_CSIPHY_PAD_SRC 1
 #define MSM_CSIPHY_PADS_NUM 2
 
+#define CSIPHY_GRP_ID 1
+
 struct csiphy_lane {
        u8 pos;
        u8 pol;

diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index 506d84dd3b970e810f883505215e242ad0504d66..bd351ceadda751ed399e9b35f9cb272b3078b19e 100644 (file)
--- a/drivers/media/platform/qcom/camss/camss.c
+++ b/drivers/media/platform/qcom/camss/camss.c
@@ -4748,6 +4748,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;
+                       }
+               }
+       }
+
        /* note: SM8250 requires VFE to be initialized before CSID */
        for (i = 0; i < camss->res->vfe_num; i++) {
                ret = msm_vfe_subdev_init(camss, &camss->vfe[i],
@@ -4836,6 +4849,23 @@ static int camss_link_entities(struct camss *camss)
                }
        }
 
+       for (i = 0; i < camss->res->tpg_num; i++) {
+               for (j = 0; j < camss->res->csid_num; j++) {
+                       ret = media_create_pad_link(&camss->tpg[i].subdev.entity,
+                                                   MSM_TPG_PAD_SRC,
+                                                   &camss->csid[j].subdev.entity,
+                                                   MSM_CSID_PAD_SINK,
+                                                   0);
+                       if (ret < 0) {
+                               camss_link_err(camss,
+                                              camss->tpg[i].subdev.entity.name,
+                                              camss->csid[j].subdev.entity.name,
+                                              ret);
+                               return ret;
+                       }
+               }
+       }
+
        if (camss->ispif) {
                for (i = 0; i < camss->res->csid_num; i++) {
                        for (j = 0; j < camss->ispif->line_num; j++) {
@@ -4940,6 +4970,19 @@ static int camss_register_entities(struct camss *camss)
                }
        }
 
+       if (camss->tpg) {
+               for (i = 0; i < camss->res->tpg_num; i++) {
+                       ret = msm_tpg_register_entity(&camss->tpg[i],
+                                                     &camss->v4l2_dev);
+                       if (ret < 0) {
+                               dev_err(camss->dev,
+                                       "Failed to register tpg%d entity: %d\n",
+                                       i, ret);
+                               goto err_reg_tpg;
+                       }
+               }
+       }
+
        for (i = 0; i < camss->res->csid_num; i++) {
                ret = msm_csid_register_entity(&camss->csid[i],
                                               &camss->v4l2_dev);
@@ -4983,6 +5026,13 @@ err_reg_csid:
        for (i--; i >= 0; i--)
                msm_csid_unregister_entity(&camss->csid[i]);
 
+       i = camss->res->tpg_num;
+err_reg_tpg:
+       if (camss->tpg) {
+               for (i--; i >= 0; i--)
+                       msm_tpg_unregister_entity(&camss->tpg[i]);
+       }
+
        i = camss->res->csiphy_num;
 err_reg_csiphy:
        for (i--; i >= 0; i--)
@@ -5004,6 +5054,11 @@ static void camss_unregister_entities(struct camss *camss)
        for (i = 0; i < camss->res->csiphy_num; i++)
                msm_csiphy_unregister_entity(&camss->csiphy[i]);
 
+       if (camss->tpg) {
+               for (i = 0; i < camss->res->tpg_num; i++)
+                       msm_tpg_unregister_entity(&camss->tpg[i]);
+       }
+
        for (i = 0; i < camss->res->csid_num; i++)
                msm_csid_unregister_entity(&camss->csid[i]);