media: camss: Use optimal clock frequency rates

Use standard V4L2 control to get pixel clock rate from a sensor
linked in the media controller pipeline. Then calculate clock
rates on CSIPHY, CSID and VFE to use the lowest possible.

If the currnet pixel clock rate of the sensor cannot be read then
use the highest possible. This case covers also the CSID test
generator usage.

If VFE is already powered on by another pipeline, check that the
current VFE clock rate is high enough for the new pipeline.
If not return busy error code as VFE clock rate cannot be changed
while VFE is running.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
Signed-off-by: Hans Verkuil <hans.verkuil@cisco.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab@s-opensource.com>

1 files changed, 114 insertions, 25 deletions

diff --git a/drivers/media/platform/qcom/camss-8x16/camss-csid.c b/drivers/media/platform/qcom/camss-8x16/camss-csid.c
index 5c09e83e4a2b..792c14a9f59c 100644
--- a/drivers/media/platform/qcom/camss-8x16/camss-csid.c
+++ b/drivers/media/platform/qcom/camss-8x16/camss-csid.c
@@ -69,6 +69,7 @@ struct csid_fmts {
 	u8 data_type;
 	u8 decode_format;
 	u8 bpp;
+	u8 spp; /* bus samples per pixel */
 };
 
 static const struct csid_fmts csid_input_fmts[] = {
@@ -76,97 +77,113 @@ static const struct csid_fmts csid_input_fmts[] = {
 		MEDIA_BUS_FMT_UYVY8_2X8,
 		DATA_TYPE_YUV422_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		16,
+		8,
+		2,
 	},
 	{
 		MEDIA_BUS_FMT_VYUY8_2X8,
 		DATA_TYPE_YUV422_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		16,
+		8,
+		2,
 	},
 	{
 		MEDIA_BUS_FMT_YUYV8_2X8,
 		DATA_TYPE_YUV422_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		16,
+		8,
+		2,
 	},
 	{
 		MEDIA_BUS_FMT_YVYU8_2X8,
 		DATA_TYPE_YUV422_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		16,
+		8,
+		2,
 	},
 	{
 		MEDIA_BUS_FMT_SBGGR8_1X8,
 		DATA_TYPE_RAW_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 		8,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SGBRG8_1X8,
 		DATA_TYPE_RAW_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 		8,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SGRBG8_1X8,
 		DATA_TYPE_RAW_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 		8,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SRGGB8_1X8,
 		DATA_TYPE_RAW_8BIT,
 		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
 		8,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SBGGR10_1X10,
 		DATA_TYPE_RAW_10BIT,
 		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 		10,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SGBRG10_1X10,
 		DATA_TYPE_RAW_10BIT,
 		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 		10,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SGRBG10_1X10,
 		DATA_TYPE_RAW_10BIT,
 		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 		10,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SRGGB10_1X10,
 		DATA_TYPE_RAW_10BIT,
 		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
 		10,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SBGGR12_1X12,
 		DATA_TYPE_RAW_12BIT,
 		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 		12,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SGBRG12_1X12,
 		DATA_TYPE_RAW_12BIT,
 		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 		12,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SGRBG12_1X12,
 		DATA_TYPE_RAW_12BIT,
 		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 		12,
+		1,
 	},
 	{
 		MEDIA_BUS_FMT_SRGGB12_1X12,
 		DATA_TYPE_RAW_12BIT,
 		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
 		12,
+		1,
 	}
 };
 
@@ -205,6 +222,67 @@ static irqreturn_t csid_isr(int irq, void *dev)
 }
 
 /*
+ * csid_set_clock_rates - Calculate and set clock rates on CSID module
+ * @csiphy: CSID device
+ */
+static int csid_set_clock_rates(struct csid_device *csid)
+{
+	struct device *dev = to_device_index(csid, csid->id);
+	u32 pixel_clock;
+	int i, j;
+	int ret;
+
+	ret = camss_get_pixel_clock(&csid->subdev.entity, &pixel_clock);
+	if (ret)
+		pixel_clock = 0;
+
+	for (i = 0; i < csid->nclocks; i++) {
+		struct camss_clock *clock = &csid->clock[i];
+
+		if (!strcmp(clock->name, "csi0") ||
+			!strcmp(clock->name, "csi1")) {
+			u8 bpp = csid_get_fmt_entry(
+				csid->fmt[MSM_CSIPHY_PAD_SINK].code)->bpp;
+			u8 num_lanes = csid->phy.lane_cnt;
+			u64 min_rate = pixel_clock * bpp / (2 * num_lanes * 4);
+			long rate;
+
+			camss_add_clock_margin(&min_rate);
+
+			for (j = 0; j < clock->nfreqs; j++)
+				if (min_rate < clock->freq[j])
+					break;
+
+			if (j == clock->nfreqs) {
+				dev_err(dev,
+					"Pixel clock is too high for CSID\n");
+				return -EINVAL;
+			}
+
+			/* if sensor pixel clock is not available */
+			/* set highest possible CSID clock rate */
+			if (min_rate == 0)
+				j = clock->nfreqs - 1;
+
+			rate = clk_round_rate(clock->clk, clock->freq[j]);
+			if (rate < 0) {
+				dev_err(dev, "clk round rate failed: %ld\n",
+					rate);
+				return -EINVAL;
+			}
+
+			ret = clk_set_rate(clock->clk, rate);
+			if (ret < 0) {
+				dev_err(dev, "clk set rate failed: %d\n", ret);
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*
  * csid_reset - Trigger reset on CSID module and wait to complete
  * @csid: CSID device
  *
@@ -249,6 +327,12 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 		if (ret < 0)
 			return ret;
 
+		ret = csid_set_clock_rates(csid);
+		if (ret < 0) {
+			regulator_disable(csid->vdda);
+			return ret;
+		}
+
 		ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
 		if (ret < 0) {
 			regulator_disable(csid->vdda);
@@ -316,7 +400,8 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 			struct v4l2_mbus_framefmt *f =
 					&csid->fmt[MSM_CSID_PAD_SRC];
 			u8 bpp = csid_get_fmt_entry(f->code)->bpp;
-			u32 num_bytes_per_line = f->width * bpp / 8;
+			u8 spp = csid_get_fmt_entry(f->code)->spp;
+			u32 num_bytes_per_line = f->width * bpp * spp / 8;
 			u32 num_lines = f->height;
 
 			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
@@ -719,7 +804,7 @@ int msm_csid_subdev_init(struct csid_device *csid,
 	struct device *dev = to_device_index(csid, id);
 	struct platform_device *pdev = to_platform_device(dev);
 	struct resource *r;
-	int i;
+	int i, j;
 	int ret;
 
 	csid->id = id;
@@ -766,26 +851,30 @@ int msm_csid_subdev_init(struct csid_device *csid,
 		return -ENOMEM;
 
 	for (i = 0; i < csid->nclocks; i++) {
-		csid->clock[i] = devm_clk_get(dev, res->clock[i]);
-		if (IS_ERR(csid->clock[i]))
-			return PTR_ERR(csid->clock[i]);
-
-		if (res->clock_rate[i]) {
-			long clk_rate = clk_round_rate(csid->clock[i],
-						       res->clock_rate[i]);
-			if (clk_rate < 0) {
-				dev_err(to_device_index(csid, csid->id),
-					"clk round rate failed: %ld\n",
-					clk_rate);
-				return -EINVAL;
-			}
-			ret = clk_set_rate(csid->clock[i], clk_rate);
-			if (ret < 0) {
-				dev_err(to_device_index(csid, csid->id),
-					"clk set rate failed: %d\n", ret);
-				return ret;
-			}
+		struct camss_clock *clock = &csid->clock[i];
+
+		clock->clk = devm_clk_get(dev, res->clock[i]);
+		if (IS_ERR(clock->clk))
+			return PTR_ERR(clock->clk);
+
+		clock->name = res->clock[i];
+
+		clock->nfreqs = 0;
+		while (res->clock_rate[i][clock->nfreqs])
+			clock->nfreqs++;
+
+		if (!clock->nfreqs) {
+			clock->freq = NULL;
+			continue;
 		}
+
+		clock->freq = devm_kzalloc(dev, clock->nfreqs *
+					   sizeof(*clock->freq), GFP_KERNEL);
+		if (!clock->freq)
+			return -ENOMEM;
+
+		for (j = 0; j < clock->nfreqs; j++)
+			clock->freq[j] = res->clock_rate[i][j];
 	}
 
 	/* Regulator */