video: rockchip: mpp: Add devfreq support for rkvdec2

Also return -EPROBE_DEFER for rkvenc when the regulator is not ready,
so that driver can probe again with devfreq until the regulator is ok.

Change-Id: I3bbf51052a8759260715d2aee40ecfe32290bc4e
Signed-off-by: Liang Chen <cl@rock-chips.com>

drivers/video/rockchip/mpp/mpp_rkvdec2.c

@@ -17,6 +17,15 @@
 
 #include "hack/mpp_rkvdec2_hack_rk3568.c"
 
+#include <linux/devfreq_cooling.h>
+#include <soc/rockchip/rockchip_ipa.h>
+#include <soc/rockchip/rockchip_opp_select.h>
+#include <soc/rockchip/rockchip_system_monitor.h>
+
+#ifdef CONFIG_PM_DEVFREQ
+#include "../drivers/devfreq/governor.h"
+#endif
+
 /*
  * hardware information
  */
@@ -676,6 +685,276 @@ static inline int rkvdec2_procfs_init(struct mpp_dev *mpp)
 }
 #endif
 
+#ifdef CONFIG_PM_DEVFREQ
+static int rkvdec2_devfreq_target(struct device *dev,
+				  unsigned long *freq, u32 flags)
+{
+	struct dev_pm_opp *opp;
+	unsigned long target_volt, target_freq;
+	int ret = 0;
+
+	struct rkvdec2_dev *dec = dev_get_drvdata(dev);
+	struct devfreq *devfreq = dec->devfreq;
+	struct devfreq_dev_status *stat = &devfreq->last_status;
+	unsigned long old_clk_rate = stat->current_frequency;
+
+	opp = devfreq_recommended_opp(dev, freq, flags);
+	if (IS_ERR(opp)) {
+		dev_err(dev, "Failed to find opp for %lu Hz\n", *freq);
+		return PTR_ERR(opp);
+	}
+	target_freq = dev_pm_opp_get_freq(opp);
+	target_volt = dev_pm_opp_get_voltage(opp);
+	dev_pm_opp_put(opp);
+
+	if (old_clk_rate == target_freq) {
+		dec->core_last_rate_hz = target_freq;
+		if (dec->volt == target_volt)
+			return ret;
+		ret = regulator_set_voltage(dec->vdd, target_volt, INT_MAX);
+		if (ret) {
+			dev_err(dev, "Cannot set voltage %lu uV\n",
+				target_volt);
+			return ret;
+		}
+		dec->volt = target_volt;
+		return 0;
+	}
+
+	if (old_clk_rate < target_freq) {
+		ret = regulator_set_voltage(dec->vdd, target_volt, INT_MAX);
+		if (ret) {
+			dev_err(dev, "set voltage %lu uV\n", target_volt);
+			return ret;
+		}
+	}
+
+	dev_dbg(dev, "%lu-->%lu\n", old_clk_rate, target_freq);
+	clk_set_rate(dec->core_clk_info.clk, target_freq);
+	stat->current_frequency = target_freq;
+	dec->core_last_rate_hz = target_freq;
+
+	if (old_clk_rate > target_freq) {
+		ret = regulator_set_voltage(dec->vdd, target_volt, INT_MAX);
+		if (ret) {
+			dev_err(dev, "set vol %lu uV\n", target_volt);
+			return ret;
+		}
+	}
+	dec->volt = target_volt;
+
+	return ret;
+}
+
+static int rkvdec2_devfreq_get_dev_status(struct device *dev,
+					  struct devfreq_dev_status *stat)
+{
+	return 0;
+}
+
+static int rkvdec2_devfreq_get_cur_freq(struct device *dev,
+					unsigned long *freq)
+{
+	struct rkvdec2_dev *dec = dev_get_drvdata(dev);
+
+	*freq = dec->core_last_rate_hz;
+
+	return 0;
+}
+
+static struct devfreq_dev_profile rkvdec2_devfreq_profile = {
+	.target	= rkvdec2_devfreq_target,
+	.get_dev_status	= rkvdec2_devfreq_get_dev_status,
+	.get_cur_freq = rkvdec2_devfreq_get_cur_freq,
+};
+
+static int devfreq_vdec2_ondemand_func(struct devfreq *df, unsigned long *freq)
+{
+	struct rkvdec2_dev *dec = df->data;
+
+	if (dec)
+		*freq = dec->core_rate_hz;
+	else
+		*freq = df->previous_freq;
+
+	return 0;
+}
+
+static int devfreq_vdec2_ondemand_handler(struct devfreq *devfreq,
+					  unsigned int event, void *data)
+{
+	return 0;
+}
+
+static struct devfreq_governor devfreq_vdec2_ondemand = {
+	.name = "vdec2_ondemand",
+	.get_target_freq = devfreq_vdec2_ondemand_func,
+	.event_handler = devfreq_vdec2_ondemand_handler,
+};
+
+static unsigned long rkvdec2_get_static_power(struct devfreq *devfreq,
+					      unsigned long voltage)
+{
+	struct rkvdec2_dev *dec = devfreq->data;
+
+	if (!dec->model_data)
+		return 0;
+	else
+		return rockchip_ipa_get_static_power(dec->model_data,
+						     voltage);
+}
+
+static struct devfreq_cooling_power vdec2_cooling_power_data = {
+	.get_static_power = rkvdec2_get_static_power,
+};
+
+static struct monitor_dev_profile vdec2_mdevp = {
+	.type = MONITOR_TPYE_DEV,
+	.low_temp_adjust = rockchip_monitor_dev_low_temp_adjust,
+	.high_temp_adjust = rockchip_monitor_dev_high_temp_adjust,
+};
+
+static int rkvdec2_devfreq_init(struct mpp_dev *mpp)
+{
+	struct rkvdec2_dev *dec = to_rkvdec2_dev(mpp);
+	struct clk *clk_core = dec->core_clk_info.clk;
+	struct devfreq_cooling_power *vdec2_dcp = &vdec2_cooling_power_data;
+	int ret = 0;
+
+	if (!clk_core)
+		return 0;
+
+	dec->vdd = devm_regulator_get_optional(mpp->dev, "vdec");
+	if (IS_ERR_OR_NULL(dec->vdd)) {
+		if (PTR_ERR(dec->vdd) == -EPROBE_DEFER) {
+			dev_warn(mpp->dev, "vdec regulator not ready, retry\n");
+
+			return -EPROBE_DEFER;
+		}
+		dev_info(mpp->dev, "no regulator, devfreq is disabled\n");
+
+		return 0;
+	}
+
+	ret = rockchip_init_opp_table(mpp->dev, NULL, "leakage", "vdec");
+	if (ret) {
+		dev_err(mpp->dev, "failed to init_opp_table\n");
+		return ret;
+	}
+
+	ret = devfreq_add_governor(&devfreq_vdec2_ondemand);
+	if (ret) {
+		dev_err(mpp->dev, "failed to add vdec2_ondemand governor\n");
+		goto governor_err;
+	}
+
+	rkvdec2_devfreq_profile.initial_freq = clk_get_rate(clk_core);
+
+	dec->devfreq = devm_devfreq_add_device(mpp->dev,
+					       &rkvdec2_devfreq_profile,
+					       "vdec2_ondemand", (void *)dec);
+	if (IS_ERR(dec->devfreq)) {
+		ret = PTR_ERR(dec->devfreq);
+		dec->devfreq = NULL;
+		goto devfreq_err;
+	}
+	dec->devfreq->last_status.total_time = 1;
+	dec->devfreq->last_status.busy_time = 1;
+
+	devfreq_register_opp_notifier(mpp->dev, dec->devfreq);
+
+	of_property_read_u32(mpp->dev->of_node, "dynamic-power-coefficient",
+			     (u32 *)&vdec2_dcp->dyn_power_coeff);
+	dec->model_data = rockchip_ipa_power_model_init(mpp->dev,
+							"vdec_leakage");
+	if (IS_ERR_OR_NULL(dec->model_data)) {
+		dec->model_data = NULL;
+		dev_err(mpp->dev, "failed to initialize power model\n");
+	} else if (dec->model_data->dynamic_coefficient) {
+		vdec2_dcp->dyn_power_coeff =
+			dec->model_data->dynamic_coefficient;
+	}
+	if (!vdec2_dcp->dyn_power_coeff) {
+		dev_err(mpp->dev, "failed to get dynamic-coefficient\n");
+		goto out;
+	}
+
+	dec->devfreq_cooling =
+		of_devfreq_cooling_register_power(mpp->dev->of_node,
+						  dec->devfreq, vdec2_dcp);
+	if (IS_ERR_OR_NULL(dec->devfreq_cooling))
+		dev_err(mpp->dev, "failed to register cooling device\n");
+
+	vdec2_mdevp.data = dec->devfreq;
+	dec->mdev_info = rockchip_system_monitor_register(mpp->dev, &vdec2_mdevp);
+	if (IS_ERR(dec->mdev_info)) {
+		dev_dbg(mpp->dev, "without system monitor\n");
+		dec->mdev_info = NULL;
+	}
+
+out:
+	return 0;
+
+devfreq_err:
+	devfreq_remove_governor(&devfreq_vdec2_ondemand);
+governor_err:
+	dev_pm_opp_of_remove_table(mpp->dev);
+
+	return ret;
+}
+
+static int rkvdec2_devfreq_remove(struct mpp_dev *mpp)
+{
+	struct rkvdec2_dev *dec = to_rkvdec2_dev(mpp);
+
+	if (dec->mdev_info)
+		rockchip_system_monitor_unregister(dec->mdev_info);
+	if (dec->devfreq) {
+		devfreq_unregister_opp_notifier(mpp->dev, dec->devfreq);
+		dev_pm_opp_of_remove_table(mpp->dev);
+		devfreq_remove_governor(&devfreq_vdec2_ondemand);
+	}
+
+	return 0;
+}
+
+void mpp_devfreq_set_core_rate(struct mpp_dev *mpp, enum MPP_CLOCK_MODE mode)
+{
+	struct rkvdec2_dev *dec = to_rkvdec2_dev(mpp);
+
+	if (dec->devfreq) {
+		unsigned long core_rate_hz;
+
+		mutex_lock(&dec->devfreq->lock);
+		core_rate_hz = mpp_get_clk_info_rate_hz(&dec->core_clk_info, mode);
+		if (dec->core_rate_hz != core_rate_hz) {
+			dec->core_rate_hz = core_rate_hz;
+			update_devfreq(dec->devfreq);
+		}
+		mutex_unlock(&dec->devfreq->lock);
+	}
+
+	mpp_clk_set_rate(&dec->core_clk_info, mode);
+}
+#else
+static inline int rkvdec2_devfreq_init(struct mpp_dev *mpp)
+{
+	return 0;
+}
+
+static inline int rkvdec2_devfreq_remove(struct mpp_dev *mpp)
+{
+	return 0;
+}
+
+void mpp_devfreq_set_core_rate(struct mpp_dev *mpp, enum MPP_CLOCK_MODE mode)
+{
+	struct rkvdec2_dev *dec = to_rkvdec2_dev(mpp);
+
+	mpp_clk_set_rate(&dec->core_clk_info, mode);
+}
+#endif
+
 static int rkvdec2_init(struct mpp_dev *mpp)
 {
 	int ret;
@@ -732,7 +1011,11 @@ static int rkvdec2_init(struct mpp_dev *mpp)
 	if (!dec->rst_hevc_cabac)
 		mpp_err("No hevc cabac reset resource define\n");
 
-	return 0;
+	ret = rkvdec2_devfreq_init(mpp);
+	if (ret)
+		mpp_err("failed to add vdec devfreq\n");
+
+	return ret;
 }
 
 static int rkvdec2_rk3568_init(struct mpp_dev *mpp)
@@ -756,6 +1039,8 @@ static int rkvdec2_rk3568_exit(struct mpp_dev *mpp)
 {
 	struct rkvdec2_dev *dec = to_rkvdec2_dev(mpp);
 
+	rkvdec2_devfreq_remove(mpp);
+
 	if (dec->fix)
 		mpp_dma_free(dec->fix);
 
@@ -831,9 +1116,9 @@ static int rkvdec2_set_freq(struct mpp_dev *mpp,
 	struct rkvdec2_task *task =  to_rkvdec2_task(mpp_task);
 
 	mpp_clk_set_rate(&dec->aclk_info, task->clk_mode);
-	mpp_clk_set_rate(&dec->core_clk_info, task->clk_mode);
 	mpp_clk_set_rate(&dec->cabac_clk_info, task->clk_mode);
 	mpp_clk_set_rate(&dec->hevc_cabac_clk_info, task->clk_mode);
+	mpp_devfreq_set_core_rate(mpp, task->clk_mode);
 
 	return 0;
 }

drivers/video/rockchip/mpp/mpp_rkvdec2.h

@@ -185,6 +185,17 @@ struct rkvdec2_dev {
 	struct reset_control *rst_cabac;
 	struct reset_control *rst_hevc_cabac;
 
+#ifdef CONFIG_PM_DEVFREQ
+	struct regulator *vdd;
+	struct devfreq *devfreq;
+	unsigned long volt;
+	unsigned long core_rate_hz;
+	unsigned long core_last_rate_hz;
+	struct ipa_power_model_data *model_data;
+	struct thermal_cooling_device *devfreq_cooling;
+	struct monitor_dev_info *mdev_info;
+#endif
+
 	/* internal rcb-memory */
 	u32 sram_size;
 	u32 rcb_size;
@@ -216,4 +227,6 @@ int rkvdec2_result(struct mpp_dev *mpp, struct mpp_task *mpp_task,
 		   struct mpp_task_msgs *msgs);
 int rkvdec2_reset(struct mpp_dev *mpp);
 
+void mpp_devfreq_set_core_rate(struct mpp_dev *mpp, enum MPP_CLOCK_MODE mode);
+
 #endif

drivers/video/rockchip/mpp/mpp_rkvdec2_link.c

@@ -1056,6 +1056,11 @@ static int rkvdec2_link_power_on(struct mpp_dev *mpp)
 			enable_irq(mpp->irq);
 			link_dec->irq_enabled = 1;
 		}
+
+		mpp_clk_set_rate(&dec->aclk_info, CLK_MODE_ADVANCED);
+		mpp_clk_set_rate(&dec->cabac_clk_info, CLK_MODE_ADVANCED);
+		mpp_clk_set_rate(&dec->hevc_cabac_clk_info, CLK_MODE_ADVANCED);
+		mpp_devfreq_set_core_rate(mpp, CLK_MODE_ADVANCED);
 	}
 	return 0;
 }
@@ -1077,6 +1082,11 @@ static void rkvdec2_link_power_off(struct mpp_dev *mpp)
 
 		link_dec->task_decoded = 0;
 		link_dec->task_total = 0;
+
+		mpp_clk_set_rate(&dec->aclk_info, CLK_MODE_NORMAL);
+		mpp_clk_set_rate(&dec->cabac_clk_info, CLK_MODE_NORMAL);
+		mpp_clk_set_rate(&dec->hevc_cabac_clk_info, CLK_MODE_NORMAL);
+		mpp_devfreq_set_core_rate(mpp, CLK_MODE_NORMAL);
 	}
 }
 

drivers/video/rockchip/mpp/mpp_rkvenc.c

@@ -1221,7 +1221,7 @@ static int rkvenc_init(struct mpp_dev *mpp)
 
 	mpp->iommu_info->hdl = rkvenc_iommu_fault_handle;
 
-	return 0;
+	return ret;
 }
 
 static int rkvenc_exit(struct mpp_dev *mpp)