From d1b2562c706250d1435d80e237a0ab58807ce8db Mon Sep 17 00:00:00 2001
From: Tomeu Vizoso <tomeu.vizoso@collabora.com>
Date: Tue, 26 Mar 2019 16:11:40 +0100
Subject: [PATCH] drm/panfrost: Implement DVFS

Signed-off-by: Tomeu Vizoso <tomeu.vizoso@collabora.com>
---
 drivers/gpu/drm/panfrost/Makefile           |   1 +
 drivers/gpu/drm/panfrost/TODO               |   2 +-
 drivers/gpu/drm/panfrost/panfrost_devfreq.c | 191 ++++++++++++++++++++
 drivers/gpu/drm/panfrost/panfrost_devfreq.h |  14 ++
 drivers/gpu/drm/panfrost/panfrost_device.c  |   4 +
 drivers/gpu/drm/panfrost/panfrost_device.h  |  10 +
 drivers/gpu/drm/panfrost/panfrost_drv.c     |   7 +
 drivers/gpu/drm/panfrost/panfrost_job.c     |   5 +
 8 files changed, 233 insertions(+), 1 deletion(-)
 create mode 100644 drivers/gpu/drm/panfrost/panfrost_devfreq.c
 create mode 100644 drivers/gpu/drm/panfrost/panfrost_devfreq.h

diff --git a/drivers/gpu/drm/panfrost/Makefile b/drivers/gpu/drm/panfrost/Makefile
index d07e0971b687a..6de72d13c58f8 100644
--- a/drivers/gpu/drm/panfrost/Makefile
+++ b/drivers/gpu/drm/panfrost/Makefile
@@ -3,6 +3,7 @@
 panfrost-y := \
 	panfrost_drv.o \
 	panfrost_device.o \
+	panfrost_devfreq.o \
 	panfrost_gem.o \
 	panfrost_gpu.o \
 	panfrost_job.o \
diff --git a/drivers/gpu/drm/panfrost/TODO b/drivers/gpu/drm/panfrost/TODO
index 1588d295914fd..c2e44add37d89 100644
--- a/drivers/gpu/drm/panfrost/TODO
+++ b/drivers/gpu/drm/panfrost/TODO
@@ -1,4 +1,4 @@
-- Frequency scaling and thermal support. (Tomeu)
+- Thermal support.
 
 - Bifrost support:
   - DT bindings (Neil, WIP)
diff --git a/drivers/gpu/drm/panfrost/panfrost_devfreq.c b/drivers/gpu/drm/panfrost/panfrost_devfreq.c
new file mode 100644
index 0000000000000..b1c4453f99917
--- /dev/null
+++ b/drivers/gpu/drm/panfrost/panfrost_devfreq.c
@@ -0,0 +1,191 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright 2019 Collabora ltd. */
+#include <linux/devfreq.h>
+#include <linux/platform_device.h>
+#include <linux/pm_opp.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include "panfrost_device.h"
+#include "panfrost_features.h"
+#include "panfrost_issues.h"
+#include "panfrost_gpu.h"
+#include "panfrost_regs.h"
+
+static int panfrost_devfreq_target(struct device *dev, unsigned long *freq,
+				   u32 flags)
+{
+	struct panfrost_device *pfdev = platform_get_drvdata(to_platform_device(dev));
+	struct dev_pm_opp *opp;
+	unsigned long old_clk_rate = pfdev->devfreq.cur_freq;
+	unsigned long target_volt, target_rate;
+	int err;
+
+	opp = devfreq_recommended_opp(dev, freq, flags);
+	if (IS_ERR(opp))
+		return PTR_ERR(opp);
+
+	target_rate = dev_pm_opp_get_freq(opp);
+	target_volt = dev_pm_opp_get_voltage(opp);
+	dev_pm_opp_put(opp);
+
+	if (old_clk_rate == target_rate)
+		return 0;
+
+	/*
+	 * If frequency scaling from low to high, adjust voltage first.
+	 * If frequency scaling from high to low, adjust frequency first.
+	 */
+	if (old_clk_rate < target_rate) {
+		err = regulator_set_voltage(pfdev->regulator, target_volt,
+					    target_volt);
+		if (err) {
+			dev_err(dev, "Cannot set voltage %lu uV\n",
+				target_volt);
+			return err;
+		}
+	}
+
+	err = clk_set_rate(pfdev->clock, target_rate);
+	if (err) {
+		dev_err(dev, "Cannot set frequency %lu (%d)\n", target_rate,
+			err);
+		regulator_set_voltage(pfdev->regulator, pfdev->devfreq.cur_volt,
+				      pfdev->devfreq.cur_volt);
+		return err;
+	}
+
+	if (old_clk_rate > target_rate) {
+		err = regulator_set_voltage(pfdev->regulator, target_volt,
+					    target_volt);
+		if (err)
+			dev_err(dev, "Cannot set voltage %lu uV\n", target_volt);
+	}
+
+	pfdev->devfreq.cur_freq = target_rate;
+	pfdev->devfreq.cur_volt = target_volt;
+
+	return 0;
+}
+
+static void panfrost_devfreq_reset(struct panfrost_device *pfdev)
+{
+	ktime_t now = ktime_get();
+	int i;
+
+	for (i = 0; i < NUM_JOB_SLOTS; i++) {
+		pfdev->devfreq.busy_time[i] = 0;
+		pfdev->devfreq.idle_time[i] = 0;
+		pfdev->devfreq.time_last_update[i] = now;
+	}
+}
+
+static int panfrost_devfreq_get_dev_status(struct device *dev,
+					   struct devfreq_dev_status *status)
+{
+	struct panfrost_device *pfdev = platform_get_drvdata(to_platform_device(dev));
+	int i;
+
+	status->current_frequency = clk_get_rate(pfdev->clock);
+	status->total_time = ktime_to_ns(ktime_add(pfdev->devfreq.busy_time[0],
+						   pfdev->devfreq.idle_time[0]));
+
+	status->busy_time = 0;
+	for (i = 0; i < NUM_JOB_SLOTS; i++) {
+		status->busy_time += ktime_to_ns(pfdev->devfreq.busy_time[i]);
+	}
+
+	/* We're scheduling only to one core atm, so don't divide for now */
+	//status->busy_time /= NUM_JOB_SLOTS;
+
+	panfrost_devfreq_reset(pfdev);
+
+	dev_dbg(pfdev->dev, "busy %lu total %lu %lu %%\n", status->busy_time,
+		status->total_time,
+		status->busy_time * 100 / status->total_time);
+
+	return 0;
+}
+
+static int panfrost_devfreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+	struct panfrost_device *pfdev = platform_get_drvdata(to_platform_device(dev));
+
+	*freq = pfdev->devfreq.cur_freq;
+
+	return 0;
+}
+
+static struct devfreq_dev_profile panfrost_devfreq_profile = {
+	.polling_ms = 10,
+	.target = panfrost_devfreq_target,
+	.get_dev_status = panfrost_devfreq_get_dev_status,
+	.get_cur_freq = panfrost_devfreq_get_cur_freq,
+};
+
+int panfrost_devfreq_init(struct panfrost_device *pfdev)
+{
+	int ret;
+	struct dev_pm_opp *opp;
+
+	ret = dev_pm_opp_of_add_table(&pfdev->pdev->dev);
+	if (ret == -ENODEV) /* Optional, continue without devfreq */
+		return 0;
+
+	panfrost_devfreq_reset(pfdev);
+
+	pfdev->devfreq.cur_freq = clk_get_rate(pfdev->clock);
+
+	opp = devfreq_recommended_opp(&pfdev->pdev->dev, &pfdev->devfreq.cur_freq, 0);
+	if (IS_ERR(opp))
+		return PTR_ERR(opp);
+
+	panfrost_devfreq_profile.initial_freq = pfdev->devfreq.cur_freq;
+	dev_pm_opp_put(opp);
+
+	pfdev->devfreq.devfreq = devm_devfreq_add_device(&pfdev->pdev->dev,
+			&panfrost_devfreq_profile, "simple_ondemand", NULL);
+	if (IS_ERR(pfdev->devfreq.devfreq)) {
+		DRM_DEV_ERROR(&pfdev->pdev->dev, "Couldn't initialize GPU devfreq\n");
+		pfdev->devfreq.devfreq = NULL;
+		return PTR_ERR(pfdev->devfreq.devfreq);
+	}
+
+	return 0;
+}
+
+void panfrost_devfreq_resume(struct panfrost_device *pfdev)
+{
+	if (!pfdev->devfreq.devfreq)
+		return;
+
+	panfrost_devfreq_reset(pfdev);
+	devfreq_resume_device(pfdev->devfreq.devfreq);
+}
+
+void panfrost_devfreq_suspend(struct panfrost_device *pfdev)
+{
+	if (!pfdev->devfreq.devfreq)
+		return;
+
+	devfreq_suspend_device(pfdev->devfreq.devfreq);
+}
+
+void panfrost_devfreq_update_utilization(struct panfrost_device *pfdev, int slot, bool busy)
+{
+	ktime_t now;
+	ktime_t last;
+
+	if (!pfdev->devfreq.devfreq)
+		return;
+
+	now = ktime_get();
+	last = pfdev->devfreq.time_last_update[slot];
+
+	if (busy)
+		pfdev->devfreq.busy_time[slot] += ktime_sub(now, last);
+	else
+		pfdev->devfreq.idle_time[slot] += ktime_sub(now, last);
+
+	pfdev->devfreq.time_last_update[slot] = now;
+}
diff --git a/drivers/gpu/drm/panfrost/panfrost_devfreq.h b/drivers/gpu/drm/panfrost/panfrost_devfreq.h
new file mode 100644
index 0000000000000..1dd7db3b86307
--- /dev/null
+++ b/drivers/gpu/drm/panfrost/panfrost_devfreq.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright 2019 Collabora ltd. */
+
+#ifndef __PANFROST_DEVFREQ_H__
+#define __PANFROST_DEVFREQ_H__
+
+int panfrost_devfreq_init(struct panfrost_device *pfdev);
+
+void panfrost_devfreq_resume(struct panfrost_device *pfdev);
+void panfrost_devfreq_suspend(struct panfrost_device *pfdev);
+
+void panfrost_devfreq_update_utilization(struct panfrost_device *pfdev, int slot, bool busy);
+
+#endif /* __PANFROST_DEVFREQ_H__ */
diff --git a/drivers/gpu/drm/panfrost/panfrost_device.c b/drivers/gpu/drm/panfrost/panfrost_device.c
index 9112aaea3f76b..7705aa34a5277 100644
--- a/drivers/gpu/drm/panfrost/panfrost_device.c
+++ b/drivers/gpu/drm/panfrost/panfrost_device.c
@@ -8,6 +8,7 @@
 #include <linux/regulator/consumer.h>
 
 #include "panfrost_device.h"
+#include "panfrost_devfreq.h"
 #include "panfrost_features.h"
 #include "panfrost_gpu.h"
 #include "panfrost_job.h"
@@ -204,6 +205,7 @@ int panfrost_device_resume(struct device *dev)
 	panfrost_gpu_power_on(pfdev);
 	panfrost_mmu_enable(pfdev, 0);
 	panfrost_job_enable_interrupts(pfdev);
+	panfrost_devfreq_resume(pfdev);
 
 	return 0;
 }
@@ -213,6 +215,8 @@ int panfrost_device_suspend(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct panfrost_device *pfdev = platform_get_drvdata(pdev);
 
+	panfrost_devfreq_suspend(pfdev);
+
 	if (!panfrost_job_is_idle(pfdev))
 		return -EBUSY;
 
diff --git a/drivers/gpu/drm/panfrost/panfrost_device.h b/drivers/gpu/drm/panfrost/panfrost_device.h
index cc2f9589f66c7..f92011d05cc09 100644
--- a/drivers/gpu/drm/panfrost/panfrost_device.h
+++ b/drivers/gpu/drm/panfrost/panfrost_device.h
@@ -70,6 +70,16 @@ struct panfrost_device {
 	struct list_head scheduled_jobs;
 
 	struct mutex sched_lock;
+
+	struct {
+		struct devfreq *devfreq;
+		struct thermal_cooling_device *cooling;
+		unsigned long cur_freq;
+		unsigned long cur_volt;
+		ktime_t busy_time[NUM_JOB_SLOTS];
+		ktime_t idle_time[NUM_JOB_SLOTS];
+		ktime_t time_last_update[NUM_JOB_SLOTS];
+	} devfreq;
 };
 
 struct panfrost_file_priv {
diff --git a/drivers/gpu/drm/panfrost/panfrost_drv.c b/drivers/gpu/drm/panfrost/panfrost_drv.c
index 8cffb70a35486..57a99032bcc61 100644
--- a/drivers/gpu/drm/panfrost/panfrost_drv.c
+++ b/drivers/gpu/drm/panfrost/panfrost_drv.c
@@ -15,6 +15,7 @@
 #include <drm/drm_utils.h>
 
 #include "panfrost_device.h"
+#include "panfrost_devfreq.h"
 #include "panfrost_gem.h"
 #include "panfrost_mmu.h"
 #include "panfrost_job.h"
@@ -411,6 +412,12 @@ static int panfrost_probe(struct platform_device *pdev)
 		goto err_out0;
 	}
 
+	err = panfrost_devfreq_init(pfdev);
+	if (err) {
+		dev_err(&pdev->dev, "Fatal error during devfreq init\n");
+		goto err_out1;
+	}
+
 	/*
 	 * Register the DRM device with the core and the connectors with
 	 * sysfs
diff --git a/drivers/gpu/drm/panfrost/panfrost_job.c b/drivers/gpu/drm/panfrost/panfrost_job.c
index 8d570c3f15d0d..482e86866b2c4 100644
--- a/drivers/gpu/drm/panfrost/panfrost_job.c
+++ b/drivers/gpu/drm/panfrost/panfrost_job.c
@@ -11,6 +11,7 @@
 #include <drm/panfrost_drm.h>
 
 #include "panfrost_device.h"
+#include "panfrost_devfreq.h"
 #include "panfrost_job.h"
 #include "panfrost_features.h"
 #include "panfrost_issues.h"
@@ -145,6 +146,8 @@ static void panfrost_job_hw_submit(struct panfrost_job *job, int js)
 	u64 jc_head = job->jc;
 	int ret;
 
+	panfrost_devfreq_update_utilization(pfdev, js, false);
+
 	ret = pm_runtime_get_sync(pfdev->dev);
 	if (ret < 0)
 		return;
@@ -386,6 +389,7 @@ static void panfrost_job_timedout(struct drm_sched_job *sched_job)
 
 	//panfrost_core_dump(pfdev);
 
+	panfrost_devfreq_update_utilization(pfdev, js, true);
 	panfrost_gpu_soft_reset(pfdev);
 
 	/* TODO: Re-enable all other address spaces */
@@ -441,6 +445,7 @@ static irqreturn_t panfrost_job_irq_handler(int irq, void *data)
 		}
 
 		if (status & JOB_INT_MASK_DONE(j)) {
+			panfrost_devfreq_update_utilization(pfdev, j, true);
 			dma_fence_signal(pfdev->jobs[j]->done_fence);
 		}
 
-- 
GitLab