WIP: drm/tegra: Add devfreq support to media engines

TODO: Better limit frequency count

Change-Id: If86527dc4985d9afd9d510430d1f3c137ca7f220

drivers/gpu/drm/tegra/nvdec.c

@@ -5,6 +5,7 @@
 
 #include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/devfreq.h>
 #include <linux/dma-mapping.h>
 #include <linux/host1x.h>
 #include <linux/iommu.h>
@@ -43,6 +44,7 @@ struct nvdec {
 	struct clk_bulk_data clks[3];
 	unsigned int num_clks;
 	struct reset_control *reset;
+	struct devfreq *devfreq;
 
 	/* Platform configuration */
 	const struct nvdec_config *config;
@@ -63,6 +65,75 @@ static inline void nvdec_writel(struct nvdec *nvdec, u32 value,
 	writel(value, nvdec->regs + offset);
 }
 
+static int nvdec_devfreq_target(struct device *dev, unsigned long *freq,
+				u32 flags)
+{
+	struct nvdec *nvdec = dev_get_drvdata(dev);
+	int err;
+
+	err = clk_set_rate(nvdec->clks[0].clk, *freq);
+	if (err < 0)
+		return err;
+
+	*freq = clk_get_rate(nvdec->clks[0].clk);
+
+	return 0;
+}
+
+static int nvdec_devfreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+	struct nvdec *nvdec = dev_get_drvdata(dev);
+
+	*freq = clk_get_rate(nvdec->clks[0].clk);
+
+	return 0;
+}
+
+static struct devfreq_dev_profile nvdec_devfreq_profile = {
+	.polling_ms = 50,
+	.target = nvdec_devfreq_target,
+	.get_cur_freq = nvdec_devfreq_get_cur_freq,
+};
+
+static int nvdec_devfreq_init(struct nvdec *nvdec)
+{
+	struct clk *clk = nvdec->clks[0].clk;
+	unsigned long max_rate = clk_round_rate(clk, ULONG_MAX);
+	unsigned long min_rate = clk_round_rate(clk, 0);
+	unsigned long margin = clk_round_rate(clk, min_rate + 1) - min_rate;
+	unsigned long rate = min_rate;
+	unsigned int hw_rates = ((max_rate - min_rate) / margin) + 1;
+	struct devfreq *devfreq;
+
+	if (min_rate == max_rate) {
+		dev_info(nvdec->dev, "Only one supported clock rate, disabling devfreq\n");
+		return 0;
+	}
+
+	// Only allow up to 20 rates
+	if (hw_rates > 20)
+		margin *= DIV_ROUND_UP(hw_rates, 20);
+
+	while (rate < max_rate) {
+		dev_pm_opp_add(nvdec->dev, rate, 0);
+		rate += margin;
+	}
+
+	// The margin step may not fall on max rate exactly, so add max rate explicitly
+	dev_pm_opp_add(nvdec->dev, max_rate, 0);
+
+	devfreq = devm_devfreq_add_device(nvdec->dev,
+					  &nvdec_devfreq_profile,
+					  DEVFREQ_GOV_USERSPACE,
+					  NULL);
+	if (IS_ERR(devfreq))
+		return PTR_ERR(devfreq);
+
+	nvdec->devfreq = devfreq;
+
+	return 0;
+}
+
 static int nvdec_boot_falcon(struct nvdec *nvdec)
 {
 	u32 stream_id;
@@ -331,6 +402,12 @@ static __maybe_unused int nvdec_runtime_resume(struct device *dev)
 			goto disable;
 	}
 
+	if (nvdec->devfreq) {
+		err = devfreq_resume_device(nvdec->devfreq);
+		if (err < 0)
+			goto disable;
+	}
+
 	return 0;
 
 disable:
@@ -341,6 +418,13 @@ disable:
 static __maybe_unused int nvdec_runtime_suspend(struct device *dev)
 {
 	struct nvdec *nvdec = dev_get_drvdata(dev);
+	int err;
+
+	if (nvdec->devfreq) {
+		err = devfreq_suspend_device(nvdec->devfreq);
+		if (err < 0)
+			return err;
+	}
 
 	host1x_channel_stop(nvdec->channel);
 
@@ -532,12 +616,20 @@ static int nvdec_probe(struct platform_device *pdev)
 		goto exit_falcon;
 	}
 
+	err = nvdec_devfreq_init(nvdec);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to init devfreq: %d\n", err);
+		goto unregister_client;
+	}
+
 	pm_runtime_enable(dev);
 	pm_runtime_use_autosuspend(dev);
 	pm_runtime_set_autosuspend_delay(dev, 500);
 
 	return 0;
 
+unregister_client:
+	host1x_client_unregister(&nvdec->client.base);
 exit_falcon:
 	falcon_exit(&nvdec->falcon);
 

drivers/gpu/drm/tegra/nvenc.c

@@ -2,6 +2,7 @@
 
 #include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/devfreq.h>
 #include <linux/dma-mapping.h>
 #include <linux/host1x.h>
 #include <linux/iommu.h>
@@ -30,6 +31,7 @@ struct nvenc {
 	struct host1x_channel *channel;
 	struct device *dev;
 	struct clk *clk;
+	struct devfreq *devfreq;
 
 	/* Platform configuration */
 	const struct nvenc_config *config;
@@ -46,6 +48,74 @@ static inline void nvenc_writel(struct nvenc *nvenc, u32 value,
 	writel(value, nvenc->regs + offset);
 }
 
+static int nvenc_devfreq_target(struct device *dev, unsigned long *freq,
+				u32 flags)
+{
+	struct nvenc *nvenc = dev_get_drvdata(dev);
+	int err;
+
+	err = clk_set_rate(nvenc->clk, *freq);
+	if (err < 0)
+		return err;
+
+	*freq = clk_get_rate(nvenc->clk);
+
+	return 0;
+}
+
+static int nvenc_devfreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+	struct nvenc *nvenc = dev_get_drvdata(dev);
+
+	*freq = clk_get_rate(nvenc->clk);
+
+	return 0;
+}
+
+static struct devfreq_dev_profile nvenc_devfreq_profile = {
+	.polling_ms = 50,
+	.target = nvenc_devfreq_target,
+	.get_cur_freq = nvenc_devfreq_get_cur_freq,
+};
+
+static int nvenc_devfreq_init(struct nvenc *nvenc)
+{
+	unsigned long max_rate = clk_round_rate(nvenc->clk, ULONG_MAX);
+	unsigned long min_rate = clk_round_rate(nvenc->clk, 0);
+	unsigned long margin = clk_round_rate(nvenc->clk, min_rate + 1) - min_rate;
+	unsigned long rate = min_rate;
+	unsigned int hw_rates = ((max_rate - min_rate) / margin) + 1;
+	struct devfreq *devfreq;
+
+	if (min_rate == max_rate) {
+		dev_info(nvenc->dev, "Only one supported clock rate, disabling devfreq\n");
+		return 0;
+	}
+
+	// Only allow up to 20 rates
+	if (hw_rates > 20)
+		margin *= DIV_ROUND_UP(hw_rates, 20);
+
+	while (rate < max_rate) {
+		dev_pm_opp_add(nvenc->dev, rate, 0);
+		rate += margin;
+	}
+
+	// The margin step may not fall on max rate exactly, so add max rate explicitly
+	dev_pm_opp_add(nvenc->dev, max_rate, 0);
+
+	devfreq = devm_devfreq_add_device(nvenc->dev,
+					  &nvenc_devfreq_profile,
+					  DEVFREQ_GOV_USERSPACE,
+					  NULL);
+	if (IS_ERR(devfreq))
+		return PTR_ERR(devfreq);
+
+	nvenc->devfreq = devfreq;
+
+	return 0;
+}
+
 static int nvenc_init(struct host1x_client *client)
 {
 	struct tegra_drm_client *drm = host1x_to_drm_client(client);
@@ -248,6 +318,12 @@ static __maybe_unused int nvenc_runtime_resume(struct device *dev)
 	if (err < 0)
 		goto disable_clk;
 
+	if (nvenc->devfreq) {
+		err = devfreq_resume_device(nvenc->devfreq);
+		if (err < 0)
+			goto disable_clk;
+	}
+
 	return 0;
 
 disable_clk:
@@ -258,6 +334,13 @@ disable_clk:
 static __maybe_unused int nvenc_runtime_suspend(struct device *dev)
 {
 	struct nvenc *nvenc = dev_get_drvdata(dev);
+	int err;
+
+	if (nvenc->devfreq) {
+		err = devfreq_suspend_device(nvenc->devfreq);
+		if (err < 0)
+			return err;
+	}
 
 	host1x_channel_stop(nvenc->channel);
 
@@ -411,12 +494,20 @@ static int nvenc_probe(struct platform_device *pdev)
 		goto exit_falcon;
 	}
 
+	err = nvenc_devfreq_init(nvenc);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to init devfreq: %d\n", err);
+		goto unregister_client;
+	}
+
 	pm_runtime_use_autosuspend(dev);
 	pm_runtime_set_autosuspend_delay(dev, 500);
 	devm_pm_runtime_enable(dev);
 
 	return 0;
 
+unregister_client:
+	host1x_client_unregister(&nvenc->client.base);
 exit_falcon:
 	falcon_exit(&nvenc->falcon);
 

drivers/gpu/drm/tegra/nvjpg.c

@@ -2,6 +2,7 @@
 
 #include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/devfreq.h>
 #include <linux/dma-mapping.h>
 #include <linux/host1x.h>
 #include <linux/iommu.h>
@@ -30,6 +31,7 @@ struct nvjpg {
 	struct host1x_channel *channel;
 	struct device *dev;
 	struct clk *clk;
+	struct devfreq *devfreq;
 
 	/* Platform configuration */
 	const struct nvjpg_config *config;
@@ -46,6 +48,74 @@ static inline void nvjpg_writel(struct nvjpg *nvjpg, u32 value,
 	writel(value, nvjpg->regs + offset);
 }
 
+static int nvjpg_devfreq_target(struct device *dev, unsigned long *freq,
+				u32 flags)
+{
+	struct nvjpg *nvjpg = dev_get_drvdata(dev);
+	int err;
+
+	err = clk_set_rate(nvjpg->clk, *freq);
+	if (err < 0)
+		return err;
+
+	*freq = clk_get_rate(nvjpg->clk);
+
+	return 0;
+}
+
+static int nvjpg_devfreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+	struct nvjpg *nvjpg = dev_get_drvdata(dev);
+
+	*freq = clk_get_rate(nvjpg->clk);
+
+	return 0;
+}
+
+static struct devfreq_dev_profile nvjpg_devfreq_profile = {
+	.polling_ms = 50,
+	.target = nvjpg_devfreq_target,
+	.get_cur_freq = nvjpg_devfreq_get_cur_freq,
+};
+
+static int nvjpg_devfreq_init(struct nvjpg *nvjpg)
+{
+	unsigned long max_rate = clk_round_rate(nvjpg->clk, ULONG_MAX);
+	unsigned long min_rate = clk_round_rate(nvjpg->clk, 0);
+	unsigned long margin = clk_round_rate(nvjpg->clk, min_rate + 1) - min_rate;
+	unsigned long rate = min_rate;
+	unsigned int hw_rates = ((max_rate - min_rate) / margin) + 1;
+	struct devfreq *devfreq;
+
+	if (min_rate == max_rate) {
+		dev_info(nvjpg->dev, "Only one supported clock rate, disabling devfreq\n");
+		return 0;
+	}
+
+	// Only allow up to 20 rates
+	if (hw_rates > 20)
+		margin *= DIV_ROUND_UP(hw_rates, 20);
+
+	while (rate < max_rate) {
+		dev_pm_opp_add(nvjpg->dev, rate, 0);
+		rate += margin;
+	}
+
+	// The margin step may not fall on max rate exactly, so add max rate explicitly
+	dev_pm_opp_add(nvjpg->dev, max_rate, 0);
+
+	devfreq = devm_devfreq_add_device(nvjpg->dev,
+					  &nvjpg_devfreq_profile,
+					  DEVFREQ_GOV_USERSPACE,
+					  NULL);
+	if (IS_ERR(devfreq))
+		return PTR_ERR(devfreq);
+
+	nvjpg->devfreq = devfreq;
+
+	return 0;
+}
+
 static int nvjpg_init(struct host1x_client *client)
 {
 	struct tegra_drm_client *drm = host1x_to_drm_client(client);
@@ -248,6 +318,12 @@ static __maybe_unused int nvjpg_runtime_resume(struct device *dev)
 	if (err < 0)
 		goto disable_clk;
 
+	if (nvjpg->devfreq) {
+		err = devfreq_resume_device(nvjpg->devfreq);
+		if (err < 0)
+			goto disable_clk;
+	}
+
 	return 0;
 
 disable_clk:
@@ -258,6 +334,13 @@ disable_clk:
 static __maybe_unused int nvjpg_runtime_suspend(struct device *dev)
 {
 	struct nvjpg *nvjpg = dev_get_drvdata(dev);
+	int err;
+
+	if (nvjpg->devfreq) {
+		err = devfreq_suspend_device(nvjpg->devfreq);
+		if (err < 0)
+			return err;
+	}
 
 	host1x_channel_stop(nvjpg->channel);
 
@@ -411,12 +494,20 @@ static int nvjpg_probe(struct platform_device *pdev)
 		goto exit_falcon;
 	}
 
+	err = nvjpg_devfreq_init(nvjpg);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to init devfreq: %d\n", err);
+		goto unregister_client;
+	}
+
 	pm_runtime_use_autosuspend(dev);
 	pm_runtime_set_autosuspend_delay(dev, 500);
 	devm_pm_runtime_enable(dev);
 
 	return 0;
 
+unregister_client:
+	host1x_client_unregister(&nvjpg->client.base);
 exit_falcon:
 	falcon_exit(&nvjpg->falcon);
 

drivers/gpu/drm/tegra/vic.c

@@ -5,6 +5,7 @@
 
 #include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/devfreq.h>
 #include <linux/dma-mapping.h>
 #include <linux/host1x.h>
 #include <linux/iommu.h>
@@ -35,6 +36,7 @@ struct vic {
 	struct device *dev;
 	struct clk *clk;
 	struct reset_control *rst;
+	struct devfreq *devfreq;
 
 	bool can_use_context;
 
@@ -52,6 +54,74 @@ static void vic_writel(struct vic *vic, u32 value, unsigned int offset)
 	writel(value, vic->regs + offset);
 }
 
+static int vic_devfreq_target(struct device *dev, unsigned long *freq,
+				u32 flags)
+{
+	struct vic *vic = dev_get_drvdata(dev);
+	int err;
+
+	err = clk_set_rate(vic->clk, *freq);
+	if (err < 0)
+		return err;
+
+	*freq = clk_get_rate(vic->clk);
+
+	return 0;
+}
+
+static int vic_devfreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+	struct vic *vic = dev_get_drvdata(dev);
+
+	*freq = clk_get_rate(vic->clk);
+
+	return 0;
+}
+
+static struct devfreq_dev_profile vic_devfreq_profile = {
+	.polling_ms = 50,
+	.target = vic_devfreq_target,
+	.get_cur_freq = vic_devfreq_get_cur_freq,
+};
+
+static int vic_devfreq_init(struct vic *vic)
+{
+	unsigned long max_rate = clk_round_rate(vic->clk, ULONG_MAX);
+	unsigned long min_rate = clk_round_rate(vic->clk, 0);
+	unsigned long margin = clk_round_rate(vic->clk, min_rate + 1) - min_rate;
+	unsigned long rate = min_rate;
+	unsigned int hw_rates = ((max_rate - min_rate) / margin) + 1;
+	struct devfreq *devfreq;
+
+	if (min_rate == max_rate) {
+		dev_info(vic->dev, "Only one supported clock rate, disabling devfreq\n");
+		return 0;
+	}
+
+	// Only allow up to 20 rates
+	if (hw_rates > 20)
+		margin *= DIV_ROUND_UP(hw_rates, 20);
+
+	while (rate < max_rate) {
+		dev_pm_opp_add(vic->dev, rate, 0);
+		rate += margin;
+	}
+
+	// The margin step may not fall on max rate exactly, so add max rate explicitly
+	dev_pm_opp_add(vic->dev, max_rate, 0);
+
+	devfreq = devm_devfreq_add_device(vic->dev,
+					  &vic_devfreq_profile,
+					  DEVFREQ_GOV_USERSPACE,
+					  NULL);
+	if (IS_ERR(devfreq))
+		return PTR_ERR(devfreq);
+
+	vic->devfreq = devfreq;
+
+	return 0;
+}
+
 static int vic_boot(struct vic *vic)
 {
 	u32 fce_ucode_size, fce_bin_data_offset, stream_id;
@@ -328,6 +398,12 @@ static int __maybe_unused vic_runtime_resume(struct device *dev)
 	if (err < 0)
 		goto assert;
 
+	if (vic->devfreq) {
+		err = devfreq_resume_device(vic->devfreq);
+		if (err < 0)
+			goto assert;
+	}
+
 	return 0;
 
 assert:
@@ -342,6 +418,12 @@ static int __maybe_unused vic_runtime_suspend(struct device *dev)
 	struct vic *vic = dev_get_drvdata(dev);
 	int err;
 
+	if (vic->devfreq) {
+		err = devfreq_suspend_device(vic->devfreq);
+		if (err < 0)
+			return err;
+	}
+
 	host1x_channel_stop(vic->channel);
 
 	err = reset_control_assert(vic->rst);
@@ -520,12 +602,20 @@ static int vic_probe(struct platform_device *pdev)
 		goto exit_falcon;
 	}
 
+	err = vic_devfreq_init(vic);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to init devfreq: %d\n", err);
+		goto unregister_client;
+	}
+
 	pm_runtime_enable(dev);
 	pm_runtime_use_autosuspend(dev);
 	pm_runtime_set_autosuspend_delay(dev, 500);
 
 	return 0;
 
+unregister_client:
+	host1x_client_unregister(&vic->client.base);
 exit_falcon:
 	falcon_exit(&vic->falcon);