[PATCH] media: stm32-dcmi: Set minimum cpufreq requirement

[Benjamin Gaignard <benjamin.gaignard@xxxxxx>]

Before start streaming set cpufreq minimum frequency requirement.
The cpufreq governor will adapt the frequencies and we will have
no latency for handling interrupts.

Signed-off-by: Benjamin Gaignard <benjamin.gaignard@xxxxxx>
---
 drivers/media/platform/stm32/stm32-dcmi.c | 29 ++++++++++++++++++++++++++++-
 1 file changed, 28 insertions(+), 1 deletion(-)

diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c
index b8931490b83b..97c342351569 100644
--- a/drivers/media/platform/stm32/stm32-dcmi.c
+++ b/drivers/media/platform/stm32/stm32-dcmi.c
@@ -13,6 +13,7 @@
 
 #include <linux/clk.h>
 #include <linux/completion.h>
+#include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/dmaengine.h>
 #include <linux/init.h>
@@ -99,6 +100,8 @@ enum state {
 
 #define OVERRUN_ERROR_THRESHOLD	3
 
+#define DCMI_MIN_FREQ	650000 /* in KHz */
+
 struct dcmi_graph_entity {
 	struct v4l2_async_subdev asd;
 
@@ -173,6 +176,10 @@ struct stm32_dcmi {
 	struct media_device		mdev;
 	struct media_pad		vid_cap_pad;
 	struct media_pipeline		pipeline;
+
+	/* CPU freq contraint */
+	struct cpufreq_policy		*policy;
+	struct freq_qos_request		qos_req;
 };
 
 static inline struct stm32_dcmi *notifier_to_dcmi(struct v4l2_async_notifier *n)
@@ -736,11 +743,20 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
 		goto err_release_buffers;
 	}
 
+	if (dcmi->policy) {
+		ret = freq_qos_add_request(&dcmi->policy->constraints,
+					   &dcmi->qos_req, FREQ_QOS_MIN,
+					   DCMI_MIN_FREQ);
+
+		if (ret < 0)
+			goto err_pm_put;
+	}
+
 	ret = media_pipeline_start(&dcmi->vdev->entity, &dcmi->pipeline);
 	if (ret < 0) {
 		dev_err(dcmi->dev, "%s: Failed to start streaming, media pipeline start error (%d)\n",
 			__func__, ret);
-		goto err_pm_put;
+		goto err_drop_qos;
 	}
 
 	ret = dcmi_pipeline_start(dcmi);
@@ -835,6 +851,9 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
 err_media_pipeline_stop:
 	media_pipeline_stop(&dcmi->vdev->entity);
 
+err_drop_qos:
+	if (dcmi->policy)
+		freq_qos_remove_request(&dcmi->qos_req);
 err_pm_put:
 	pm_runtime_put(dcmi->dev);
 
@@ -863,6 +882,9 @@ static void dcmi_stop_streaming(struct vb2_queue *vq)
 
 	media_pipeline_stop(&dcmi->vdev->entity);
 
+	if (dcmi->policy)
+		freq_qos_remove_request(&dcmi->qos_req);
+
 	spin_lock_irq(&dcmi->irqlock);
 
 	/* Disable interruptions */
@@ -2020,6 +2042,8 @@ static int dcmi_probe(struct platform_device *pdev)
 		goto err_cleanup;
 	}
 
+	dcmi->policy = cpufreq_cpu_get(0);
+
 	dev_info(&pdev->dev, "Probe done\n");
 
 	platform_set_drvdata(pdev, dcmi);
@@ -2049,6 +2073,9 @@ static int dcmi_remove(struct platform_device *pdev)
 
 	pm_runtime_disable(&pdev->dev);
 
+	if (dcmi->policy)
+		cpufreq_cpu_put(dcmi->policy);
+
 	v4l2_async_notifier_unregister(&dcmi->notifier);
 	v4l2_async_notifier_cleanup(&dcmi->notifier);
 	media_entity_cleanup(&dcmi->vdev->entity);
-- 
2.15.0