[PATCH v6 05/10] cpufreq: mediatek: Add opp notification support

[Viresh Kumar]

On 05-05-22, 19:52, Rex-BC Chen wrote:
> >From this opp notifier, cpufreq should listen to opp notification and do
> proper actions when receiving events of disable and voltage adjustment.
> 
> One of the user for this opp notifier is MediaTek SVS.
> The MediaTek Smart Voltage Scaling (SVS) is a hardware which calculates
> suitable SVS bank voltages to OPP voltage table.
> 
> Signed-off-by: Andrew-sh.Cheng <andrew-sh.cheng at mediatek.com>
> Signed-off-by: Jia-Wei Chang <jia-wei.chang at mediatek.com>
> Signed-off-by: Rex-BC Chen <rex-bc.chen at mediatek.com>
> Reviewed-by: AngeloGioacchino Del Regno <angelogioacchino.delregno at collabora.com>
> ---
>  drivers/cpufreq/mediatek-cpufreq.c | 91 +++++++++++++++++++++++++++---
>  1 file changed, 83 insertions(+), 8 deletions(-)
> 
> diff --git a/drivers/cpufreq/mediatek-cpufreq.c b/drivers/cpufreq/mediatek-cpufreq.c
> index fe205eca657d..06d80ee06bbf 100644
> --- a/drivers/cpufreq/mediatek-cpufreq.c
> +++ b/drivers/cpufreq/mediatek-cpufreq.c
> @@ -46,6 +46,11 @@ struct mtk_cpu_dvfs_info {
>  	int intermediate_voltage;
>  	bool need_voltage_tracking;
>  	int pre_vproc;
> +	/* Avoid race condition for regulators between notify and policy */
> +	struct mutex reg_lock;
> +	struct notifier_block opp_nb;
> +	unsigned int opp_cpu;
> +	unsigned long opp_freq;

The name opp_freq doesn't fit well, I renamed it to current_freq.

>  	const struct mtk_cpufreq_platform_data *soc_data;
>  	int vtrack_max;
>  };
> @@ -182,6 +187,8 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
>  
>  	pre_freq_hz = clk_get_rate(cpu_clk);
>  
> +	mutex_lock(&info->reg_lock);
> +
>  	if (unlikely(info->pre_vproc <= 0))
>  		pre_vproc = regulator_get_voltage(info->proc_reg);
>  	else
> @@ -214,7 +221,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
>  			dev_err(cpu_dev,
>  				"cpu%d: failed to scale up voltage!\n", policy->cpu);
>  			mtk_cpufreq_set_voltage(info, pre_vproc);
> -			return ret;
> +			goto out;
>  		}
>  	}
>  
> @@ -224,8 +231,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
>  		dev_err(cpu_dev,
>  			"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
>  		mtk_cpufreq_set_voltage(info, pre_vproc);
> -		WARN_ON(1);
> -		return ret;
> +		goto out;
>  	}
>  
>  	/* Set the original PLL to target rate. */
> @@ -235,7 +241,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
>  			"cpu%d: failed to scale cpu clock rate!\n", policy->cpu);
>  		clk_set_parent(cpu_clk, armpll);
>  		mtk_cpufreq_set_voltage(info, pre_vproc);
> -		return ret;
> +		goto out;
>  	}
>  
>  	/* Set parent of CPU clock back to the original PLL. */
> @@ -244,8 +250,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
>  		dev_err(cpu_dev,
>  			"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
>  		mtk_cpufreq_set_voltage(info, inter_vproc);
> -		WARN_ON(1);
> -		return ret;
> +		goto out;
>  	}
>  
>  	/*
> @@ -260,15 +265,72 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
>  			clk_set_parent(cpu_clk, info->inter_clk);
>  			clk_set_rate(armpll, pre_freq_hz);
>  			clk_set_parent(cpu_clk, armpll);
> -			return ret;
> +			goto out;
>  		}
>  	}
>  
> -	return 0;
> +	info->opp_freq = freq_hz;
> +
> +out:
> +	mutex_unlock(&info->reg_lock);
> +
> +	return ret;
>  }
>  
>  #define DYNAMIC_POWER "dynamic-power-coefficient"
>  
> +static int mtk_cpufreq_opp_notifier(struct notifier_block *nb,
> +				    unsigned long event, void *data)
> +{
> +	struct dev_pm_opp *opp = data;
> +	struct dev_pm_opp *new_opp;
> +	struct mtk_cpu_dvfs_info *info;
> +	unsigned long freq, volt;
> +	struct cpufreq_policy *policy;
> +	int ret = 0;
> +
> +	info = container_of(nb, struct mtk_cpu_dvfs_info, opp_nb);
> +
> +	if (event == OPP_EVENT_ADJUST_VOLTAGE) {
> +		freq = dev_pm_opp_get_freq(opp);
> +
> +		mutex_lock(&info->reg_lock);
> +		if (info->opp_freq == freq) {
> +			volt = dev_pm_opp_get_voltage(opp);
> +			ret = mtk_cpufreq_set_voltage(info, volt);
> +			if (ret)
> +				dev_err(info->cpu_dev,
> +					"failed to scale voltage: %d\n", ret);
> +		}
> +		mutex_unlock(&info->reg_lock);
> +	} else if (event == OPP_EVENT_DISABLE) {
> +		freq = dev_pm_opp_get_freq(opp);
> +
> +		/* case of current opp item is disabled */
> +		if (info->opp_freq == freq) {
> +			freq = 1;
> +			new_opp = dev_pm_opp_find_freq_ceil(info->cpu_dev,
> +							    &freq);
> +			if (IS_ERR(new_opp)) {
> +				dev_err(info->cpu_dev,
> +					"all opp items are disabled\n");
> +				ret = PTR_ERR(new_opp);
> +				return notifier_from_errno(ret);
> +			}
> +
> +			dev_pm_opp_put(new_opp);
> +			policy = cpufreq_cpu_get(info->opp_cpu);
> +			if (policy) {
> +				cpufreq_driver_target(policy, freq / 1000,
> +						      CPUFREQ_RELATION_L);
> +				cpufreq_cpu_put(policy);
> +			}
> +		}
> +	}
> +
> +	return notifier_from_errno(ret);
> +}
> +
>  static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
>  {
>  	struct device *cpu_dev;
> @@ -357,6 +419,18 @@ static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
>  	info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
>  	dev_pm_opp_put(opp);
>  
> +	mutex_init(&info->reg_lock);
> +
> +	info->opp_cpu = cpu;
> +	info->opp_nb.notifier_call = mtk_cpufreq_opp_notifier;
> +	ret = dev_pm_opp_register_notifier(cpu_dev, &info->opp_nb);
> +	if (ret) {
> +		dev_err(cpu_dev, "cpu%d: failed to register opp notifier\n", cpu);
> +		goto out_disable_inter_clock;
> +	}
> +
> +	info->opp_freq = clk_get_rate(info->cpu_clk);

This is a resource as well, which should have been initialized before
registering notifier.

Applied with above changes. Thanks.

-- 
viresh