[PATCH 18/45] media: qcom: camss: integrate IFE devices

[Gjorgji . Rosikopulos . gjorgji . rosikopulos]

From: Atanas Filipov <atanas.filipov@xxxxxxxxxxxxxxxx>

Integrate IFE devices with CAMSS probe, power management, media entity
registration, and performance management. Keep the legacy VFE path
unchanged.

Signed-off-by: Atanas Filipov <atanas.filipov@xxxxxxxxxxxxxxxx>
---
 drivers/media/platform/qcom/camss/camss.c | 471 +++++++++++++++++++++-
 1 file changed, 460 insertions(+), 11 deletions(-)

diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index 1f662b1caf5a..7c355003c957 100644
--- a/drivers/media/platform/qcom/camss/camss.c
+++ b/drivers/media/platform/qcom/camss/camss.c
@@ -32,6 +32,9 @@
 #define CAMSS_CLOCK_MARGIN_NUMERATOR 105
 #define CAMSS_CLOCK_MARGIN_DENOMINATOR 100
 
+/* Number of pixels fetched per clock cycle */
+#define CAMSS_NUM_OF_PIXELS_PER_CYCLE	2
+
 static const struct parent_dev_ops vfe_parent_dev_ops;
 
 static const struct camss_subdev_resources csiphy_res_8x16[] = {
@@ -4653,6 +4656,108 @@ s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
 	return v4l2_get_link_freq(sensor_pad, bpp, 2 * lanes);
 }
 
+static int camss_get_num_lanes(struct media_entity *entity,
+			       unsigned int *lanes)
+{
+	struct media_pad *pad;
+
+	while (1) {
+		struct v4l2_subdev *sd;
+
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			return -ENODEV;
+
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			return -ENODEV;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+		if (sd->grp_id == CSIPHY_GRP_ID) {
+			struct csiphy_lanes_cfg *lane_cfg;
+			struct csiphy_device *csiphy;
+
+			csiphy = v4l2_get_subdevdata(sd);
+			if (!csiphy->cfg.csi2)
+				return -ENODEV;
+
+			lane_cfg = &csiphy->cfg.csi2->lane_cfg;
+			if (!lane_cfg->num_data)
+				return -EINVAL;
+
+			*lanes = lane_cfg->num_data;
+			return 0;
+		}
+
+		entity = pad->entity;
+	}
+}
+
+static int camss_get_pixel_clock_from_link_freq(struct media_entity *entity,
+						unsigned int bpp,
+						u64 *pixel_clock)
+{
+	struct media_pad *sensor_pad;
+	unsigned int lanes;
+	s64 link_freq;
+	int ret;
+
+	if (!bpp)
+		return -EINVAL;
+
+	sensor_pad = camss_find_sensor_pad(entity);
+	if (!sensor_pad)
+		return -ENODEV;
+
+	ret = camss_get_num_lanes(entity, &lanes);
+	if (ret)
+		return ret;
+
+	link_freq = v4l2_get_link_freq(sensor_pad, bpp, 2 * lanes);
+	if (link_freq < 0)
+		return link_freq;
+
+	*pixel_clock = div_u64(link_freq * 2 * lanes, bpp);
+
+	return 0;
+}
+
+/*
+ * camss_get_pixel_clock - Get pixel clock rate from sensor
+ * @entity: Media entity in the current pipeline
+ * @bpp: Number of bits per pixel for the current format
+ * @pixel_clock: Received pixel clock value
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int camss_get_pixel_clock_bpp(struct media_entity *entity, unsigned int bpp,
+			      u64 *pixel_clock)
+{
+	struct media_pad *sensor_pad;
+	struct v4l2_subdev *subdev;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	ret = camss_get_pixel_clock_from_link_freq(entity, bpp, pixel_clock);
+	if (!ret)
+		return 0;
+
+	sensor_pad = camss_find_sensor_pad(entity);
+	if (!sensor_pad)
+		return -ENODEV;
+
+	subdev = media_entity_to_v4l2_subdev(sensor_pad->entity);
+
+	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
+
+	if (!ctrl)
+		return -EINVAL;
+
+	*pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);
+
+	return 0;
+}
+
 /*
  * camss_get_pixel_clock - Get pixel clock rate from sensor
  * @entity: Media entity in the current pipeline
@@ -4746,6 +4851,156 @@ static const struct parent_dev_ops vfe_parent_dev_ops = {
 	.get_base_address = vfe_parent_dev_ops_get_base_address
 };
 
+static const u32 cam_cc_sbi_clk_rates[] = {
+	19200000, 350000000, 475000000, 576000000, 680000000,
+};
+
+static int camss_set_perf_level(struct camss *camss, struct ife_device *ife)
+{
+	enum camss_perf_mode perf_max = CAMSS_PERF_MAX_MODES - 1;
+	u64 freq_min, pixel_clock = 0;
+	u32 sink_pad = ife->sink_pad;
+	int pixelformat, fmt_index, i;
+	u8 bits_per_pixel = 0;
+	int ret;
+
+	fmt_index = camss_get_pixfmt_index(ife->pads[sink_pad].fmt.code,
+					   ife->pads[sink_pad].desc->sup_fmts.formats,
+					   ife->pads[sink_pad].desc->sup_fmts.nformats);
+	if (fmt_index < 0) {
+		dev_err(camss->dev, "%s Can't find requested format index: 0x%x\n", __func__,
+			ife->pads[sink_pad].fmt.code);
+		return fmt_index;
+	}
+
+	pixelformat = ife->pads[sink_pad].desc->sup_fmts.formats[fmt_index].pixelformat;
+
+	ret = camss_get_pixfmt_depth(pixelformat, Y_PLANE, &bits_per_pixel);
+	if (ret) {
+		dev_err(camss->dev, "%s Unknown pixel format: 0x%x\n", __func__,
+			pixelformat);
+		return ret;
+	}
+
+	ret = camss_get_pixel_clock_bpp(&ife->sd.entity, bits_per_pixel,
+					&pixel_clock);
+	if (ret) {
+		camss->perf_mode = CAMSS_PERF_MODE_NOM;
+		return 0;
+	}
+
+	freq_min = pixel_clock * bits_per_pixel / CAMSS_NUM_OF_PIXELS_PER_CYCLE / BITS_PER_BYTE;
+	camss->perf_mode = perf_max;
+
+	for (i = 0; i < ARRAY_SIZE(cam_cc_sbi_clk_rates); i++) {
+		if (freq_min <= cam_cc_sbi_clk_rates[i]) {
+			camss->perf_mode = min_t(u8, i, perf_max);
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int camss_core_get(struct camss *camss, u8 entity_id)
+{
+	int ret;
+
+	if (atomic_inc_return(&camss->perf_ref_count) > 1)
+		return 0;
+
+	ret = camss_set_perf_level(camss, &camss->ife[entity_id]);
+	if (ret)
+		dev_warn(camss->dev, "Cannot configure the performance level\n");
+
+	ret = pm_runtime_resume_and_get(camss->dev);
+	if (ret < 0) {
+		dev_err(camss->dev, "Core domain cannot be acquired\n");
+		atomic_dec(&camss->perf_ref_count);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int camss_core_put(struct camss *camss)
+{
+	int ret;
+
+	if (atomic_dec_if_positive(&camss->perf_ref_count))
+		return 0;
+
+	ret = pm_runtime_put_sync(camss->dev);
+	if (ret < 0) {
+		dev_err(camss->dev, "Core domain cannot be released\n");
+		return ret;
+	}
+
+	camss->perf_mode = CAMSS_PERF_MODE_MINSVS;
+
+	return 0;
+}
+
+static int ife_parent_dev_ops_get(struct camss *camss, int id)
+{
+	int ret = -EINVAL;
+
+	if (id < camss->res->ife_num + camss->res->ife_lite_num) {
+		struct ife_device *ife = &camss->ife[id];
+
+		ret = ife_get(ife);
+	}
+
+	return ret;
+}
+
+static int ife_parent_dev_ops_put(struct camss *camss, int id)
+{
+	if (id < camss->res->ife_num + camss->res->ife_lite_num) {
+		struct ife_device *ife = &camss->ife[id];
+
+		ife_put(ife);
+	}
+
+	return 0;
+}
+
+static void __iomem
+*ife_parent_dev_ops_get_base_address(struct camss *camss, int id)
+{
+	if (id < camss->res->ife_num + camss->res->ife_lite_num) {
+		struct ife_device *ife = &camss->ife[id];
+
+		return ife->top_ctx.base;
+	}
+
+	return NULL;
+}
+
+static const struct parent_dev_ops ife_parent_dev_ops __maybe_unused = {
+	.get = ife_parent_dev_ops_get,
+	.put = ife_parent_dev_ops_put,
+	.get_base_address = ife_parent_dev_ops_get_base_address
+};
+
+int camss_attach_host_data(struct camss *camss, struct v4l2_subdev *sd,
+			   bool is_sensor, int id)
+{
+	struct camss_host_priv_data *host_data;
+	struct device *dev = camss->dev;
+
+	host_data = devm_kzalloc(dev, sizeof(*host_data), GFP_KERNEL);
+	if (!host_data)
+		return -ENOMEM;
+
+	host_data->is_sensor = is_sensor;
+	host_data->id = id;
+
+	v4l2_set_subdev_hostdata(sd, host_data);
+
+	return 0;
+}
+
 /*
  * camss_parse_endpoint_node - Parse port endpoint node
  * @dev: CAMSS device
@@ -4833,6 +5088,46 @@ static int camss_parse_ports(struct camss *camss)
 	return ret;
 }
 
+static int camss_init_csid_subdevices(struct camss *camss)
+{
+	const struct camss_resources *res = camss->res;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < res->csid_num; i++) {
+		ret = msm_csid_subdev_init(camss, &camss->csid[i],
+					   &res->csid_res[i], i);
+		if (ret < 0) {
+			dev_err(camss->dev, "Failed to init csid%d sub-device: %d\n",
+				i, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int camss_init_ife_subdevices(struct camss *camss)
+{
+	const struct camss_resources *res = camss->res;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < camss->ife_total_num; i++) {
+		void __iomem *csid_base = camss->csid[i].base;
+
+		ret = camss_ife_subdev_init(camss, &camss->ife[i],
+					    &res->ife_res[i], i, csid_base);
+		if (ret < 0) {
+			dev_err(camss->dev, "Fail to init ife%d sub-device: %d\n",
+				i, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
 /*
  * camss_init_subdevices - Initialize subdev structures and resources
  * @camss: CAMSS device
@@ -4882,6 +5177,11 @@ static int camss_init_subdevices(struct camss *camss)
 		}
 	}
 
+	if (camss->ife_total_num > res->csid_num) {
+		dev_err(camss->dev, "IFE resources require one CSID per IFE\n");
+		return -EINVAL;
+	}
+
 	/* Get optional CSID wrapper regs shared between CSID devices */
 	if (res->csid_wrapper_res) {
 		char *reg = res->csid_wrapper_res->reg;
@@ -4893,15 +5193,20 @@ static int camss_init_subdevices(struct camss *camss)
 		camss->csid_wrapper_base = base;
 	}
 
-	for (i = 0; i < camss->res->csid_num; i++) {
-		ret = msm_csid_subdev_init(camss, &camss->csid[i],
-					   &res->csid_res[i], i);
-		if (ret < 0) {
-			dev_err(camss->dev,
-				"Failed to init csid%d sub-device: %d\n",
-				i, ret);
+	if (res->embedded_csid) {
+		ret = camss_init_ife_subdevices(camss);
+		if (ret)
+			return ret;
+	}
+
+	ret = camss_init_csid_subdevices(camss);
+	if (ret)
+		return ret;
+
+	if (!res->embedded_csid) {
+		ret = camss_init_ife_subdevices(camss);
+		if (ret)
 			return ret;
-		}
 	}
 
 	ret = msm_ispif_subdev_init(camss, res->ispif_res);
@@ -4942,6 +5247,9 @@ static int camss_link_entities(struct camss *camss)
 	int i, j, k;
 	int ret;
 
+	if (camss->ife_total_num)
+		return camss->res->link_entities(camss);
+
 	for (i = 0; i < camss->res->csiphy_num; i++) {
 		for (j = 0; j < camss->res->csid_num; j++) {
 			ret = media_create_pad_link(&camss->csiphy[i].subdev.entity,
@@ -5126,8 +5434,24 @@ static int camss_register_entities(struct camss *camss)
 		}
 	}
 
+	for (i = 0; i < camss->ife_total_num; i++) {
+		ret = ife_register_entities(&camss->ife[i],
+					    &camss->v4l2_dev);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to register ife%d entities: %d\n",
+				i, ret);
+			goto err_reg_ife;
+		}
+	}
+
 	return 0;
 
+err_reg_ife:
+	for (i--; i >= 0; i--)
+		ife_unregister_entities(&camss->ife[i]);
+
+	i = camss->res->vfe_num;
 err_reg_vfe:
 	for (i--; i >= 0; i--)
 		msm_vfe_unregister_entities(&camss->vfe[i]);
@@ -5180,6 +5504,9 @@ static void camss_unregister_entities(struct camss *camss)
 
 	for (i = 0; i < camss->res->vfe_num; i++)
 		msm_vfe_unregister_entities(&camss->vfe[i]);
+
+	for (i = 0; i < camss->ife_total_num; i++)
+		ife_unregister_entities(&camss->ife[i]);
 }
 
 static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
@@ -5193,6 +5520,20 @@ static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
 	struct csiphy_device *csiphy = &camss->csiphy[id];
 
 	csiphy->cfg.csi2 = &csd->interface.csi2;
+
+	if (camss->ife_total_num) {
+		int ret;
+
+		ret = camss_attach_host_data(camss, subdev, true, id);
+		if (ret) {
+			dev_err(camss->dev,
+				"Failed to alloc host private data: %d\n", ret);
+			return ret;
+		}
+
+		return 0;
+	}
+
 	subdev->host_priv = csiphy;
 
 	return 0;
@@ -5210,6 +5551,16 @@ static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
 		unsigned int i;
 		int ret;
 
+		if (camss->ife_total_num) {
+			struct camss_host_priv_data *host_data;
+
+			host_data = v4l2_get_subdev_hostdata(sd);
+			if (!host_data || !host_data->is_sensor)
+				continue;
+
+			csiphy = &camss->csiphy[host_data->id];
+		}
+
 		if (!csiphy)
 			continue;
 
@@ -5247,6 +5598,53 @@ static const struct media_device_ops camss_media_ops = {
 	.link_notify = v4l2_pipeline_link_notify,
 };
 
+static int camss_configure_ife_pd(struct camss *camss)
+{
+	struct device *dev = camss->dev;
+	int i;
+	int ret;
+
+	camss->ife_genpd = devm_kcalloc(dev, camss->genpd_num,
+					sizeof(*camss->ife_genpd), GFP_KERNEL);
+	if (!camss->ife_genpd)
+		return -ENOMEM;
+
+	camss->ife_genpd_link = devm_kcalloc(dev, camss->genpd_num,
+					     sizeof(*camss->ife_genpd_link),
+					     GFP_KERNEL);
+	if (!camss->ife_genpd_link)
+		return -ENOMEM;
+
+	for (i = 0; i < camss->genpd_num; i++) {
+		camss->ife_genpd[i] = dev_pm_domain_attach_by_id(dev, i);
+		if (IS_ERR(camss->ife_genpd[i])) {
+			ret = PTR_ERR(camss->ife_genpd[i]);
+			goto fail_pm;
+		}
+	}
+
+	if (camss->genpd_num > camss->res->ife_num) {
+		i = camss->genpd_num - 1;
+		camss->ife_genpd_link[i] =
+			device_link_add(dev, camss->ife_genpd[i],
+					DL_FLAG_STATELESS | DL_FLAG_PM_RUNTIME |
+					DL_FLAG_RPM_ACTIVE);
+		if (!camss->ife_genpd_link[i]) {
+			ret = -EINVAL;
+			i = camss->genpd_num;
+			goto fail_pm;
+		}
+	}
+
+	return 0;
+
+fail_pm:
+	while (i--)
+		dev_pm_domain_detach(camss->ife_genpd[i], true);
+
+	return ret;
+}
+
 static int camss_configure_pd(struct camss *camss)
 {
 	const struct camss_resources *res = camss->res;
@@ -5271,6 +5669,9 @@ static int camss_configure_pd(struct camss *camss)
 	if (camss->genpd_num == 1)
 		return 0;
 
+	if (camss->ife_total_num)
+		return camss_configure_ife_pd(camss);
+
 	/* count the # of VFEs which have flagged power-domain */
 	for (vfepd_num = i = 0; i < camss->res->vfe_num; i++) {
 		if (res->vfe_res[i].vfe.has_pd)
@@ -5351,6 +5752,20 @@ static void camss_genpd_subdevice_cleanup(struct camss *camss)
 		msm_vfe_genpd_cleanup(&camss->vfe[i]);
 }
 
+static void camss_ife_genpd_cleanup(struct camss *camss)
+{
+	int i;
+
+	if (camss->genpd_num > camss->res->ife_num) {
+		i = camss->genpd_num - 1;
+		if (camss->ife_genpd_link[i])
+			device_link_del(camss->ife_genpd_link[i]);
+	}
+
+	for (i = 0; i < camss->genpd_num; i++)
+		dev_pm_domain_detach(camss->ife_genpd[i], true);
+}
+
 static void camss_genpd_cleanup(struct camss *camss)
 {
 	if (camss->genpd_num == 1)
@@ -5358,6 +5773,11 @@ static void camss_genpd_cleanup(struct camss *camss)
 
 	camss_genpd_subdevice_cleanup(camss);
 
+	if (camss->ife_total_num) {
+		camss_ife_genpd_cleanup(camss);
+		return;
+	}
+
 	if (camss->genpd_link)
 		device_link_del(camss->genpd_link);
 
@@ -5382,8 +5802,23 @@ static int camss_probe(struct platform_device *pdev)
 
 	camss->res = of_device_get_match_data(dev);
 
+	if (camss->res->vfe_num &&
+	    (camss->res->ife_num || camss->res->ife_lite_num)) {
+		dev_err(dev, "VFE and IFE resources are mutually exclusive\n");
+		return -EINVAL;
+	}
+
+	if ((camss->res->ife_num || camss->res->ife_lite_num) &&
+	    !camss->res->link_entities) {
+		dev_err(dev, "IFE resources require entity link operations\n");
+		return -EINVAL;
+	}
+
 	atomic_set(&camss->ref_count, 0);
+	atomic_set(&camss->perf_ref_count, 0);
 	camss->dev = dev;
+	camss->core_get = camss_core_get;
+	camss->core_put = camss_core_put;
 	platform_set_drvdata(pdev, camss);
 
 	camss->csiphy = devm_kcalloc(dev, camss->res->csiphy_num,
@@ -5417,6 +5852,14 @@ static int camss_probe(struct platform_device *pdev)
 	if (!camss->vfe)
 		return -ENOMEM;
 
+	camss->ife_total_num = camss->res->ife_num + camss->res->ife_lite_num;
+	if (camss->ife_total_num) {
+		camss->ife = devm_kcalloc(dev, camss->ife_total_num,
+					  sizeof(*camss->ife), GFP_KERNEL);
+		if (!camss->ife)
+			return -ENOMEM;
+	}
+
 	ret = camss_icc_get(camss);
 	if (ret < 0) {
 		dev_err(dev, "Failed to configure ICC: %d\n", ret);
@@ -5430,8 +5873,10 @@ static int camss_probe(struct platform_device *pdev)
 	}
 
 	ret = camss_init_subdevices(camss);
-	if (ret < 0)
+	if (ret < 0) {
+		dev_err(dev, "Failed to init subdevices: %d\n", ret);
 		goto err_genpd_cleanup;
+	}
 
 	ret = dma_set_mask_and_coherent(dev, 0xffffffff);
 	if (ret) {
@@ -5463,12 +5908,16 @@ static int camss_probe(struct platform_device *pdev)
 	}
 
 	ret = camss_register_entities(camss);
-	if (ret < 0)
+	if (ret < 0) {
+		dev_err(dev, "Failed to register entities: %d\n", ret);
 		goto err_v4l2_device_unregister;
+	}
 
 	ret = camss_link_entities(camss);
-	if (ret < 0)
+	if (ret < 0) {
+		dev_err(dev, "Failed to link entities: %d\n", ret);
 		goto err_register_subdevs;
+	}
 
 	ret = media_device_register(&camss->media_dev);
 	if (ret < 0) {
-- 
2.34.1