soc: rockchip: opp_select: Implement APIs for wide-temperature control

Add a basic wide-temperature control model for device to adjust opp
table and max frequency.

Change-Id: I23f29ac1892093c527e730164eba086f02667de3
Signed-off-by: Finley Xiao <finley.xiao@rock-chips.com>

drivers/cpufreq/rockchip-cpufreq.c

@@ -42,6 +42,7 @@ struct cluster_info {
 	unsigned int reboot_freq;
 	unsigned int threshold_freq;
 	unsigned int scale_rate;
+	unsigned int temp_limit_rate;
 	int volt_sel;
 	int scale;
 	int process;
@@ -189,6 +190,56 @@ int rockchip_cpufreq_check_rate_volt(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(rockchip_cpufreq_check_rate_volt);
 
+int rockchip_cpufreq_set_temp_limit_rate(struct device *dev, unsigned long rate)
+{
+	struct cluster_info *cluster;
+
+	cluster = rockchip_cluster_lookup_by_dev(dev);
+	if (!cluster)
+		return -EINVAL;
+	cluster->temp_limit_rate = rate / 1000;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rockchip_cpufreq_set_temp_limit_rate);
+
+int rockchip_cpufreq_update_policy(struct device *dev)
+{
+	struct cluster_info *cluster;
+	unsigned int cpu;
+
+	cluster = rockchip_cluster_lookup_by_dev(dev);
+	if (!cluster)
+		return -EINVAL;
+	cpu = cpumask_any(&cluster->cpus);
+	cpufreq_update_policy(cpu);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rockchip_cpufreq_update_policy);
+
+int rockchip_cpufreq_update_cur_volt(struct device *dev)
+{
+	struct cluster_info *cluster;
+	struct cpufreq_policy *policy;
+	unsigned int cpu;
+
+	cluster = rockchip_cluster_lookup_by_dev(dev);
+	if (!cluster)
+		return -EINVAL;
+	cpu = cpumask_any(&cluster->cpus);
+
+	policy = cpufreq_cpu_get(cpu);
+	if (!policy)
+		return -ENODEV;
+	down_write(&policy->rwsem);
+	dev_pm_opp_check_rate_volt(dev, false);
+	up_write(&policy->rwsem);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rockchip_cpufreq_update_cur_volt);
+
 static int rockchip_cpufreq_cluster_init(int cpu, struct cluster_info *cluster)
 {
 	struct device_node *np;
@@ -314,6 +365,16 @@ static int rockchip_cpufreq_policy_notifier(struct notifier_block *nb,
 	if (!cluster)
 		return NOTIFY_DONE;
 
+	if (cluster->scale_rate) {
+		if (cluster->scale_rate < policy->max)
+			policy->max = cluster->scale_rate;
+	}
+
+	if (cluster->temp_limit_rate) {
+		if (cluster->temp_limit_rate < policy->max)
+			policy->max = cluster->temp_limit_rate;
+	}
+
 	if (cluster->rebooting) {
 		if (cluster->reboot_freq < policy->max)
 			policy->max = cluster->reboot_freq;
@@ -324,11 +385,6 @@ static int rockchip_cpufreq_policy_notifier(struct notifier_block *nb,
 		return NOTIFY_OK;
 	}
 
-	if (cluster->scale_rate) {
-		if (cluster->scale_rate < policy->max)
-			policy->max = cluster->scale_rate;
-	}
-
 	return NOTIFY_OK;
 }
 

drivers/soc/rockchip/rockchip_opp_select.c

@@ -5,6 +5,7 @@
  */
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
+#include <linux/devfreq.h>
 #include <linux/nvmem-consumer.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
@@ -16,11 +17,15 @@
 
 #include "../../clk/rockchip/clk.h"
 #include "../../base/power/opp/opp.h"
+#include "../../devfreq/governor.h"
 
 #define MAX_PROP_NAME_LEN	6
 #define SEL_TABLE_END		~1
 #define LEAKAGE_INVALID		0xff
 
+#define to_thermal_opp_info(nb) container_of(nb, struct thermal_opp_info, \
+					     thermal_nb)
+
 struct sel_table {
 	int min;
 	int max;
@@ -762,6 +767,349 @@ int rockchip_adjust_power_scale(struct device *dev, int scale)
 }
 EXPORT_SYMBOL(rockchip_adjust_power_scale);
 
+static int rockchip_adjust_low_temp_opp_volt(struct thermal_opp_info *info,
+					     bool is_low_temp)
+{
+	struct device *dev = info->dev;
+	struct dev_pm_opp *opp;
+	unsigned long rate;
+	int i, count, ret = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0)
+	rcu_read_lock();
+#endif
+	count = dev_pm_opp_get_opp_count(dev);
+	if (count <= 0) {
+		ret = count ? count : -ENODATA;
+		goto out;
+	}
+
+	for (i = 0, rate = 0; i < count; i++, rate++) {
+		/* find next rate */
+		opp = dev_pm_opp_find_freq_ceil(dev, &rate);
+		if (IS_ERR(opp)) {
+			ret = PTR_ERR(opp);
+			goto out;
+		}
+		if (is_low_temp) {
+			opp->u_volt = info->opp_table[i].low_temp_volt;
+			opp->u_volt_min = opp->u_volt;
+			if (opp->u_volt_max < opp->u_volt)
+				opp->u_volt_max = opp->u_volt;
+		} else {
+			opp->u_volt = info->opp_table[i].volt;
+			opp->u_volt_min = opp->u_volt;
+			if (opp->u_volt_max < opp->u_volt)
+				opp->u_volt_max = opp->u_volt;
+		}
+	}
+
+out:
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0)
+	rcu_read_unlock();
+#endif
+	return ret;
+}
+
+int rockchip_cpu_low_temp_adjust(struct thermal_opp_info *info,
+				 bool is_low)
+{
+	struct device *dev = info->dev;
+	int ret = 0;
+
+	if (!info->low_limit)
+		goto next;
+
+	if (is_low)
+		rockchip_cpufreq_set_temp_limit_rate(dev, info->low_limit);
+	else
+		rockchip_cpufreq_set_temp_limit_rate(dev, 0);
+	ret = rockchip_cpufreq_update_policy(dev);
+	if (ret)
+		return ret;
+
+next:
+	ret = rockchip_cpufreq_update_cur_volt(dev);
+
+	return ret;
+}
+EXPORT_SYMBOL(rockchip_cpu_low_temp_adjust);
+
+int rockchip_cpu_high_temp_adjust(struct thermal_opp_info *info,
+				  bool is_high)
+{
+	struct device *dev = info->dev;
+	int ret = 0;
+
+	if (!info->high_limit)
+		return ret;
+
+	if (is_high)
+		rockchip_cpufreq_set_temp_limit_rate(dev, info->high_limit);
+	else
+		rockchip_cpufreq_set_temp_limit_rate(dev, 0);
+	ret = rockchip_cpufreq_update_policy(dev);
+
+	return ret;
+}
+EXPORT_SYMBOL(rockchip_cpu_high_temp_adjust);
+
+int rockchip_dev_low_temp_adjust(struct thermal_opp_info *info,
+				 bool is_low)
+{
+	struct devfreq *df;
+
+	if (info->dev_data && info->dev_data->data) {
+		df = (struct devfreq *)info->dev_data->data;
+		mutex_lock(&df->lock);
+		update_devfreq(df);
+		mutex_unlock(&df->lock);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(rockchip_dev_low_temp_adjust);
+
+int rockchip_dev_high_temp_adjust(struct thermal_opp_info *info,
+				  bool is_high)
+{
+	return 0;
+}
+EXPORT_SYMBOL(rockchip_dev_high_temp_adjust);
+
+static void rockchip_low_temp_adjust(struct thermal_opp_info *info,
+				     bool is_low)
+{
+	struct thermal_opp_device_data *dev_data = info->dev_data;
+	int ret = 0;
+
+	dev_dbg(info->dev, "low_temp %d\n", is_low);
+	rockchip_adjust_low_temp_opp_volt(info, is_low);
+	if (dev_data->low_temp_adjust)
+		ret = dev_data->low_temp_adjust(info, is_low);
+	if (!ret)
+		info->is_low_temp = is_low;
+}
+
+static void rockchip_high_temp_adjust(struct thermal_opp_info *info,
+				      bool is_high)
+{
+	struct thermal_opp_device_data *dev_data = info->dev_data;
+	int ret = 0;
+
+	dev_dbg(info->dev, "high_temp %d\n", is_high);
+	if (dev_data->high_temp_adjust)
+		ret = dev_data->high_temp_adjust(info, is_high);
+	if (!ret)
+		info->is_high_temp = is_high;
+}
+
+static int rockchip_thermal_zone_notifier_call(struct notifier_block *nb,
+					       unsigned long value, void *data)
+{
+	struct thermal_opp_info *info = to_thermal_opp_info(nb);
+	int temperature = (int)value;
+
+	dev_dbg(info->dev, "temp=%d\n", temperature);
+
+	if (temperature < info->low_temp) {
+		if (info->is_high_temp)
+			rockchip_high_temp_adjust(info, false);
+		if (!info->is_low_temp)
+			rockchip_low_temp_adjust(info, true);
+	} else if (temperature > (info->low_temp + info->temp_hysteresis)) {
+		if (info->is_low_temp)
+			rockchip_low_temp_adjust(info, false);
+	}
+
+	if (temperature > info->high_temp) {
+		if (!info->is_high_temp)
+			rockchip_high_temp_adjust(info, true);
+	} else if (temperature < (info->high_temp - info->temp_hysteresis)) {
+		if (info->is_high_temp)
+			rockchip_high_temp_adjust(info, false);
+	}
+
+	return NOTIFY_OK;
+}
+
+static int rockchip_get_low_temp_volt(struct thermal_opp_info *info,
+				      unsigned long rate, int *delta_volt)
+{
+	int i, ret = -EINVAL;
+	int _rate = (int)(rate / 1000000);
+
+	if (!info->low_temp_table)
+		return ret;
+
+	for (i = 0; info->low_temp_table[i].sel != SEL_TABLE_END; i++) {
+		if (_rate >= info->low_temp_table[i].min &&
+		    _rate <= info->low_temp_table[i].max) {
+			*delta_volt = info->low_temp_table[i].sel;
+			ret = 0;
+		}
+	}
+
+	return ret;
+}
+
+static int rockchip_init_temp_opp_table(struct thermal_opp_info *info)
+{
+	struct device *dev = info->dev;
+	struct dev_pm_opp *opp;
+	int delta_volt = 0;
+	int i, max_opps, ret = 0;
+	unsigned long rate;
+	bool reach_max_volt = false;
+	bool reach_high_temp_max_volt = false;
+
+	max_opps = dev_pm_opp_get_opp_count(dev);
+	if (max_opps <= 0) {
+		ret = max_opps ? max_opps : -ENODATA;
+		goto out;
+	}
+	info->opp_table = kzalloc(sizeof(*info->opp_table) * max_opps,
+				  GFP_KERNEL);
+	if (!info->opp_table) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	info->num_opps = max_opps;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0)
+	rcu_read_lock();
+#endif
+	for (i = 0, rate = 0; i < max_opps; i++, rate++) {
+		/* find next rate */
+		opp = dev_pm_opp_find_freq_ceil(dev, &rate);
+		if (IS_ERR(opp)) {
+			ret = PTR_ERR(opp);
+			goto out;
+		}
+		info->opp_table[i].rate = opp->rate;
+		info->opp_table[i].volt = opp->u_volt;
+
+		if (opp->u_volt <= info->high_temp_max_volt) {
+			if (!reach_high_temp_max_volt)
+				info->high_limit = opp->rate;
+			if (opp->u_volt == info->high_temp_max_volt)
+				reach_high_temp_max_volt = true;
+		}
+
+		if (rockchip_get_low_temp_volt(info, opp->rate, &delta_volt))
+			delta_volt = 0;
+		if ((opp->u_volt + delta_volt) <= info->max_volt) {
+			info->opp_table[i].low_temp_volt =
+				opp->u_volt + delta_volt;
+			if (!reach_max_volt)
+				info->low_limit = opp->rate;
+			if (info->opp_table[i].low_temp_volt == info->max_volt)
+				reach_max_volt = true;
+		} else {
+			info->opp_table[i].low_temp_volt = info->max_volt;
+		}
+		dev_dbg(dev, "rate=%lu, volt=%lu, low_temp_volt=%lu\n",
+			info->opp_table[i].rate, info->opp_table[i].volt,
+			info->opp_table[i].low_temp_volt);
+	}
+	if (info->low_limit == opp->rate)
+		info->low_limit = 0;
+	if (info->high_limit == opp->rate)
+		info->high_limit = 0;
+out:
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0)
+	rcu_read_unlock();
+#endif
+
+	return ret;
+}
+
+struct thermal_opp_info *
+rockchip_register_thermal_notifier(struct device *dev,
+				   struct thermal_opp_device_data *data)
+{
+	struct device_node *np;
+	struct thermal_zone_device *tz;
+	struct thermal_opp_info *info = NULL;
+	const char *tz_name;
+
+	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
+	if (!np) {
+		dev_warn(dev, "OPP-v2 not supported\n");
+		goto err_out;
+	}
+	if (of_property_read_string(np, "rockchip,thermal-zone", &tz_name))
+		goto err_np;
+
+	tz = thermal_zone_get_zone_by_name(tz_name);
+	if (IS_ERR_OR_NULL(tz))
+		goto err_np;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		goto err_np;
+	info->tz = tz;
+	info->dev = dev;
+	info->thermal_nb.notifier_call = rockchip_thermal_zone_notifier_call;
+	info->dev_data = data;
+
+	if (of_property_read_u32(np, "rockchip,max-volt", &info->max_volt))
+		info->max_volt = INT_MAX;
+	of_property_read_u32(np, "rockchip,temp-hysteresis",
+			     &info->temp_hysteresis);
+	if (of_property_read_u32(np, "rockchip,low-temp", &info->low_temp))
+		info->low_temp = INT_MIN;
+	rockchip_get_sel_table(np, "rockchip,low-temp-adjust-volt",
+			       &info->low_temp_table);
+	if (of_property_read_u32(np, "rockchip,high-temp", &info->high_temp))
+		info->high_temp = INT_MAX;
+	if (of_property_read_u32(np, "rockchip,high-temp-max-volt",
+				 &info->high_temp_max_volt))
+		info->high_temp_max_volt = INT_MAX;
+	rockchip_init_temp_opp_table(info);
+	dev_info(dev, "l=%d h=%d hyst=%d l_limit=%lu h_limit=%lu\n",
+		 info->low_temp, info->high_temp, info->temp_hysteresis,
+		 info->low_limit, info->high_limit);
+
+	if ((info->low_temp + info->temp_hysteresis) > info->high_temp) {
+		dev_err(dev, "Invalid temperature, low=%d high=%d hyst=%d\n",
+			info->low_temp, info->high_temp,
+			info->temp_hysteresis);
+		goto info_free;
+	}
+
+	if (!info->low_temp_table && !info->low_limit && !info->high_limit)
+		goto info_free;
+
+	srcu_notifier_chain_register(&tz->thermal_notifier_list,
+				     &info->thermal_nb);
+	thermal_zone_device_update(tz);
+	of_node_put(np);
+
+	return info;
+
+info_free:
+	kfree(info);
+err_np:
+	of_node_put(np);
+err_out:
+	return ERR_PTR(-EINVAL);
+}
+EXPORT_SYMBOL(rockchip_register_thermal_notifier);
+
+void rockchip_unregister_thermal_notifier(struct thermal_opp_info *info)
+{
+	if (!info)
+		return;
+
+	srcu_notifier_chain_unregister(&info->tz->thermal_notifier_list,
+				       &info->thermal_nb);
+	kfree(info->low_temp_table);
+	kfree(info->opp_table);
+	kfree(info);
+}
+EXPORT_SYMBOL(rockchip_unregister_thermal_notifier);
+
 int rockchip_init_opp_table(struct device *dev,
 			    const struct of_device_id *matches,
 			    char *lkg_name, char *reg_name)

include/linux/cpufreq.h

@@ -699,7 +699,11 @@ unsigned long cpufreq_scale_max_freq_capacity(int cpu);
 unsigned int rockchip_cpufreq_adjust_target(int cpu, unsigned int freq);
 int rockchip_cpufreq_get_scale(int cpu);
 int rockchip_cpufreq_set_scale_rate(struct device *dev, unsigned long rate);
+int rockchip_cpufreq_set_temp_limit_rate(struct device *dev,
+					 unsigned long rate);
 int rockchip_cpufreq_check_rate_volt(struct device *dev);
+int rockchip_cpufreq_update_policy(struct device *dev);
+int rockchip_cpufreq_update_cur_volt(struct device *dev);
 #else
 static inline unsigned int rockchip_cpufreq_adjust_target(int cpu,
 							  unsigned int freq)
@@ -722,5 +726,22 @@ static inline int rockchip_cpufreq_check_rate_volt(struct device *dev)
 {
 	return -EINVAL;
 }
+
+static inline int rockchip_cpufreq_set_temp_limit_rate(struct device *dev,
+						       unsigned long rate)
+{
+	return -EINVAL;
+}
+
+static inline int rockchip_cpufreq_update_policy(struct device *dev)
+{
+	return -EINVAL;
+}
+
+static inline int rockchip_cpufreq_update_cur_volt(struct device *dev)
+{
+	return -EINVAL;
+}
+
 #endif
 #endif /* _LINUX_CPUFREQ_H */

include/soc/rockchip/rockchip_opp_select.h

@@ -6,6 +6,45 @@
 #ifndef __SOC_ROCKCHIP_OPP_SELECT_H
 #define __SOC_ROCKCHIP_OPP_SELECT_H
 
+struct thermal_opp_info;
+
+enum thermal_opp_type {
+	THERMAL_OPP_TPYE_CPU = 0,
+	THERMAL_OPP_TPYE_DEV,
+};
+
+struct thermal_opp_device_data {
+	enum thermal_opp_type type;
+	void *data;
+	int (*low_temp_adjust)(struct thermal_opp_info *info, bool is_low);
+	int (*high_temp_adjust)(struct thermal_opp_info *info, bool is_low);
+};
+
+struct thermal_opp_table {
+	unsigned long rate;
+	unsigned long volt;
+	unsigned long low_temp_volt;
+};
+
+struct thermal_opp_info {
+	struct device *dev;
+	struct thermal_zone_device *tz;
+	struct notifier_block thermal_nb;
+	struct sel_table *low_temp_table;
+	struct thermal_opp_table *opp_table;
+	struct thermal_opp_device_data *dev_data;
+	unsigned int num_opps;
+	unsigned long low_limit;
+	unsigned long high_limit;
+	int low_temp;
+	int high_temp;
+	int temp_hysteresis;
+	int max_volt;
+	int high_temp_max_volt;
+	bool is_low_temp;
+	bool is_high_temp;
+};
+
 void rockchip_of_get_lkg_sel(struct device *dev, struct device_node *np,
 			     char *lkg_name, int process,
 			     int *volt_sel, int *scale_sel);
@@ -27,5 +66,17 @@ int rockchip_adjust_power_scale(struct device *dev, int scale);
 int rockchip_init_opp_table(struct device *dev,
 			    const struct of_device_id *matches,
 			    char *lkg_name, char *reg_name);
+struct thermal_opp_info *
+rockchip_register_thermal_notifier(struct device *dev,
+				   struct thermal_opp_device_data *data);
+void rockchip_unregister_thermal_notifier(struct thermal_opp_info *info);
+int rockchip_cpu_low_temp_adjust(struct thermal_opp_info *info,
+				 bool is_low);
+int rockchip_cpu_high_temp_adjust(struct thermal_opp_info *info,
+				  bool is_high);
+int rockchip_dev_low_temp_adjust(struct thermal_opp_info *info,
+				 bool is_low);
+int rockchip_dev_high_temp_adjust(struct thermal_opp_info *info,
+				  bool is_high);
 
 #endif