From cf8b55ab71c780cefff2c152949303e82559701c Mon Sep 17 00:00:00 2001
From: Dom Cobley <popcornmix@gmail.com>
Date: Mon, 26 Jul 2021 18:03:53 +0100
Subject: [PATCH] drm/vc4: Increase the core clock based on HVS load

Depending on a given HVS output (HVS to PixelValves) and input (planes
attached to a channel) load, the HVS needs for the core clock to be
raised above its boot time default.

Failing to do so will result in a vblank timeout and a stalled display
pipeline.

Signed-off-by: Maxime Ripard <maxime@cerno.tech>
---
 drivers/gpu/drm/vc4/vc4_crtc.c |  15 +++++
 drivers/gpu/drm/vc4/vc4_drv.h  |   3 +
 drivers/gpu/drm/vc4/vc4_kms.c  | 116 ++++++++++++++++++++++++++++++++-
 3 files changed, 131 insertions(+), 3 deletions(-)

--- a/drivers/gpu/drm/vc4/vc4_crtc.c
+++ b/drivers/gpu/drm/vc4/vc4_crtc.c
@@ -644,12 +644,27 @@ static int vc4_crtc_atomic_check(struct
 	struct vc4_crtc_state *vc4_state = to_vc4_crtc_state(crtc_state);
 	struct drm_connector *conn;
 	struct drm_connector_state *conn_state;
+	struct drm_encoder *encoder;
 	int ret, i;
 
 	ret = vc4_hvs_atomic_check(crtc, state);
 	if (ret)
 		return ret;
 
+	encoder = vc4_get_crtc_encoder(crtc, crtc_state);
+	if (encoder) {
+		const struct drm_display_mode *mode = &crtc_state->adjusted_mode;
+		struct vc4_encoder *vc4_encoder = to_vc4_encoder(encoder);
+
+		mode = &crtc_state->adjusted_mode;
+		if (vc4_encoder->type == VC4_ENCODER_TYPE_HDMI0) {
+			vc4_state->hvs_load = max(mode->clock * mode->hdisplay / mode->htotal + 1000,
+						  mode->clock * 9 / 10) * 1000;
+		} else {
+			vc4_state->hvs_load = mode->clock * 1000;
+		}
+	}
+
 	for_each_new_connector_in_state(state, conn, conn_state,
 					i) {
 		if (conn_state->crtc != crtc)
--- a/drivers/gpu/drm/vc4/vc4_drv.h
+++ b/drivers/gpu/drm/vc4/vc4_drv.h
@@ -326,6 +326,7 @@ struct vc4_hvs {
 	u32 __iomem *dlist;
 
 	struct clk *core_clk;
+	struct clk_request *core_req;
 
 	/* Memory manager for CRTCs to allocate space in the display
 	 * list.  Units are dwords.
@@ -537,6 +538,8 @@ struct vc4_crtc_state {
 		unsigned int bottom;
 	} margins;
 
+	unsigned long hvs_load;
+
 	/* Transitional state below, only valid during atomic commits */
 	bool update_muxing;
 };
--- a/drivers/gpu/drm/vc4/vc4_kms.c
+++ b/drivers/gpu/drm/vc4/vc4_kms.c
@@ -40,6 +40,9 @@ static struct vc4_ctm_state *to_vc4_ctm_
 struct vc4_hvs_state {
 	struct drm_private_state base;
 	unsigned int unassigned_channels;
+	unsigned int num_outputs;
+	unsigned long fifo_load;
+	unsigned long core_clock_rate;
 };
 
 static struct vc4_hvs_state *
@@ -186,6 +189,19 @@ vc4_ctm_commit(struct vc4_dev *vc4, stru
 }
 
 static struct vc4_hvs_state *
+vc4_hvs_get_new_global_state(struct drm_atomic_state *state)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(state->dev);
+	struct drm_private_state *priv_state;
+
+	priv_state = drm_atomic_get_new_private_obj_state(state, &vc4->hvs_channels);
+	if (IS_ERR(priv_state))
+		return ERR_CAST(priv_state);
+
+	return to_vc4_hvs_state(priv_state);
+}
+
+static struct vc4_hvs_state *
 vc4_hvs_get_global_state(struct drm_atomic_state *state)
 {
 	struct vc4_dev *vc4 = to_vc4_dev(state->dev);
@@ -312,10 +328,15 @@ vc4_atomic_complete_commit(struct drm_at
 	struct vc4_dev *vc4 = to_vc4_dev(dev);
 	struct vc4_hvs *hvs = vc4->hvs;
 	struct drm_crtc_state *new_crtc_state;
+	struct vc4_hvs_state *hvs_state;
 	struct drm_crtc *crtc;
 	struct clk_request *core_req;
 	int i;
 
+	hvs_state = vc4_hvs_get_new_global_state(state);
+	if (WARN_ON(!hvs_state))
+		return;
+
 	for_each_new_crtc_in_state(state, crtc, new_crtc_state, i) {
 		struct vc4_crtc_state *vc4_crtc_state;
 
@@ -326,9 +347,20 @@ vc4_atomic_complete_commit(struct drm_at
 		vc4_hvs_mask_underrun(dev, vc4_crtc_state->assigned_channel);
 	}
 
-	if (vc4->hvs && vc4->hvs->hvs5)
+	if (vc4->hvs && vc4->hvs->hvs5) {
+		/*
+		 * Do a temporary request on the core clock during the
+		 * modeset.
+		 */
 		core_req = clk_request_start(hvs->core_clk, 500000000);
 
+		/*
+		 * And remove the previous one based on the HVS
+		 * requirements if any.
+		 */
+		clk_request_done(hvs->core_req);
+	}
+
 	drm_atomic_helper_wait_for_fences(dev, state, false);
 
 	drm_atomic_helper_wait_for_dependencies(state);
@@ -358,8 +390,20 @@ vc4_atomic_complete_commit(struct drm_at
 
 	drm_atomic_helper_commit_cleanup_done(state);
 
-	if (vc4->hvs && vc4->hvs->hvs5)
+	if (vc4->hvs && vc4->hvs->hvs5) {
+		drm_dbg(dev, "Running the core clock at %lu Hz\n",
+			hvs_state->core_clock_rate);
+
+		/*
+		 * Request a clock rate based on the current HVS
+		 * requirements.
+		 */
+		hvs->core_req = clk_request_start(hvs->core_clk,
+						  hvs_state->core_clock_rate);
+
+		/* And drop the temporary request */
 		clk_request_done(core_req);
+	}
 
 	drm_atomic_state_put(state);
 
@@ -703,6 +747,9 @@ vc4_hvs_channels_duplicate_state(struct
 	__drm_atomic_helper_private_obj_duplicate_state(obj, &state->base);
 
 	state->unassigned_channels = old_state->unassigned_channels;
+	state->fifo_load = old_state->fifo_load;
+	state->num_outputs = old_state->num_outputs;
+	state->core_clock_rate = old_state->core_clock_rate;
 
 	return &state->base;
 }
@@ -849,6 +896,65 @@ static int vc4_pv_muxing_atomic_check(st
 }
 
 static int
+vc4_core_clock_atomic_check(struct drm_atomic_state *state)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(state->dev);
+	struct drm_private_state *priv_state;
+	struct vc4_hvs_state *hvs_new_state;
+	struct vc4_load_tracker_state *load_state;
+	struct drm_crtc_state *old_crtc_state, *new_crtc_state;
+	struct drm_crtc *crtc;
+	unsigned long pixel_rate;
+	unsigned long cob_rate;
+	unsigned int i;
+
+	priv_state = drm_atomic_get_private_obj_state(state,
+						      &vc4->load_tracker);
+	if (IS_ERR(priv_state))
+		return PTR_ERR(priv_state);
+
+	load_state = to_vc4_load_tracker_state(priv_state);
+
+	hvs_new_state = vc4_hvs_get_global_state(state);
+	if (!hvs_new_state)
+		return -EINVAL;
+
+	for_each_oldnew_crtc_in_state(state, crtc,
+				      old_crtc_state,
+				      new_crtc_state,
+				      i) {
+		if (old_crtc_state->active) {
+			struct vc4_crtc_state *old_vc4_state =
+				to_vc4_crtc_state(old_crtc_state);
+
+			hvs_new_state->num_outputs -= 1;
+			hvs_new_state->fifo_load -= old_vc4_state->hvs_load;
+		}
+
+		if (new_crtc_state->active) {
+			struct vc4_crtc_state *new_vc4_state =
+				to_vc4_crtc_state(new_crtc_state);
+
+			hvs_new_state->num_outputs += 1;
+			hvs_new_state->fifo_load += new_vc4_state->hvs_load;
+		}
+	}
+
+	cob_rate = hvs_new_state->fifo_load;
+	pixel_rate = load_state->hvs_load;
+	if (hvs_new_state->num_outputs > 1) {
+		pixel_rate = (pixel_rate * 40) / 100;
+	} else {
+		pixel_rate = (pixel_rate * 60) / 100;
+	}
+
+	hvs_new_state->core_clock_rate = max(cob_rate, pixel_rate);
+
+	return 0;
+}
+
+
+static int
 vc4_atomic_check(struct drm_device *dev, struct drm_atomic_state *state)
 {
 	int ret;
@@ -865,7 +971,11 @@ vc4_atomic_check(struct drm_device *dev,
 	if (ret)
 		return ret;
 
-	return vc4_load_tracker_atomic_check(state);
+	ret = vc4_load_tracker_atomic_check(state);
+	if (ret)
+		return ret;
+
+	return vc4_core_clock_atomic_check(state);
 }
 
 static const struct drm_mode_config_funcs vc4_mode_funcs = {
