From e0a9d95a749319f17f01cbbe71f710ef87bea5ad Mon Sep 17 00:00:00 2001 From: Finley Xiao Date: Tue, 22 Nov 2016 18:12:35 +0800 Subject: [PATCH] PM / AVS: rockchip-cpu-avs: support adjusting initial frequency and voltage Change-Id: I377b7fccb90ecf350a37e4609bdc8f51c4e15e7a Signed-off-by: Finley Xiao --- drivers/power/avs/rockchip-cpu-avs.c | 99 ++++++++++++++-------------- 1 file changed, 49 insertions(+), 50 deletions(-) diff --git a/drivers/power/avs/rockchip-cpu-avs.c b/drivers/power/avs/rockchip-cpu-avs.c index 5af0a1ab0f0b..65c44d57b1ba 100644 --- a/drivers/power/avs/rockchip-cpu-avs.c +++ b/drivers/power/avs/rockchip-cpu-avs.c @@ -58,9 +58,9 @@ struct rockchip_cpu_avs { #define notifier_to_avs(_n) container_of(_n, struct rockchip_cpu_avs, \ cpufreq_notify) -static void rockchip_adjust_opp_table(struct device *cpu_dev, - struct cpufreq_frequency_table *table, - struct cluster_info *cluster) +static void rockchip_leakage_adjust_table(struct device *cpu_dev, + struct cpufreq_frequency_table *table, + struct cluster_info *cluster) { struct cpufreq_frequency_table *pos; struct dev_pm_opp *opp; @@ -102,7 +102,9 @@ static int rockchip_cpu_avs_notifier(struct notifier_block *nb, struct cpufreq_policy *policy = data; struct cluster_info *cluster; struct device *cpu_dev; + unsigned long cur_freq; int i, id, cpu = policy->cpu; + int ret; if (event != CPUFREQ_START) goto out; @@ -113,27 +115,36 @@ static int rockchip_cpu_avs_notifier(struct notifier_block *nb, goto out; } + cpu_dev = get_cpu_device(cpu); + if (!cpu_dev) { + pr_err("cpu%d failed to get device\n", cpu); + goto out; + } + + if (avs->num_clusters == 0) + goto next; + for (i = 0; i < avs->num_clusters; i++) { if (avs->cluster[i].id == id) break; } if (i == avs->num_clusters) - goto out; + goto next; else cluster = &avs->cluster[i]; if (!policy->freq_table) { pr_err("cpu%d freq table not found\n", cpu); - goto out; + goto next; } - cpu_dev = get_cpu_device(cpu); - if (!cpu_dev) { - pr_err("cpu%d failed to get device\n", cpu); - goto out; - } + rockchip_leakage_adjust_table(cpu_dev, policy->freq_table, cluster); + +next: + ret = dev_pm_opp_check_initial_rate(cpu_dev, &cur_freq); + if (!ret) + policy->cur = cur_freq / 1000; - rockchip_adjust_opp_table(cpu_dev, policy->freq_table, cluster); out: return NOTIFY_OK; @@ -244,54 +255,48 @@ static int rockchip_of_parse_cpu_avs(struct device_node *np, ret = of_property_read_u32(np, "cluster-id", &cluster->id); if (ret < 0) { pr_err("prop cluster-id missing\n"); - ret = -EINVAL; - goto out; + return -EINVAL; } if (cluster->id >= MAX_CLUSTERS) { pr_err("prop cluster-id invalid\n"); - ret = -EINVAL; - goto out; + return -EINVAL; } ret = of_property_read_u32(np, "min-freq", &cluster->min_freq); if (ret < 0) { pr_err("prop min_freq missing\n"); - ret = -EINVAL; - goto out; + return -EINVAL; } ret = of_property_read_u32(np, "min-volt", &cluster->min_volt); if (ret < 0) { pr_err("prop min_volt missing\n"); - ret = -EINVAL; - goto out; + return -EINVAL; } ret = rockchip_get_leakage_volt_table(np, &cluster->table); if (ret) { pr_err("prop leakage-adjust-volt invalid\n"); - ret = -EINVAL; - goto out; + return -EINVAL; } ret = rockchip_get_leakage(np, &cluster->leakage); if (ret) { pr_err("get leakage invalid\n"); - goto out; + return -EINVAL; } ret = rockchip_get_offset_volt(cluster->leakage, cluster->table, &cluster->offset_volt); if (ret) { pr_err("get offset volt err\n"); - goto out; + return -EINVAL; } pr_info("cluster%d leakage=%d adjust_volt=%d\n", cluster->id, cluster->leakage, cluster->offset_volt); -out: - return ret; + return 0; } static int rockchip_cpu_avs_probe(struct platform_device *pdev) @@ -301,59 +306,53 @@ static int rockchip_cpu_avs_probe(struct platform_device *pdev) struct rockchip_cpu_avs *avs; int ret, num_clusters = 0, i = 0; + avs = devm_kzalloc(dev, sizeof(*avs), GFP_KERNEL); + if (!avs) + return -ENOMEM; + np = of_find_node_by_name(NULL, "cpu-avs"); if (!np) { - pr_info("unable to find cpu-avs\n"); - return 0; + pr_info("unable to find cpu-avs node\n"); + goto out; } if (!of_device_is_available(np)) { - pr_info("cpu-avs disabled\n"); - ret = 0; - goto err; + pr_info("cpu-avs node disabled\n"); + goto next; } for_each_available_child_of_node(np, node) num_clusters++; if (!num_clusters) { - pr_info("cpu-avs child disabled\n"); - ret = 0; - goto err; - } - - avs = devm_kzalloc(dev, sizeof(*avs), GFP_KERNEL); - if (!avs) { - ret = -ENOMEM; - goto err; + pr_info("cpu-avs child node disabled\n"); + goto next; } avs->cluster = devm_kzalloc(dev, sizeof(*avs->cluster) * num_clusters, GFP_KERNEL); - if (!avs->cluster) { - ret = -ENOMEM; - goto err; - } + if (!avs->cluster) + goto next; avs->num_clusters = num_clusters; for_each_available_child_of_node(np, node) { ret = rockchip_of_parse_cpu_avs(node, &avs->cluster[i++]); - if (ret) - goto err; + if (ret) { + devm_kfree(dev, avs->cluster); + avs->num_clusters = 0; + break; + } } +next: of_node_put(np); +out: avs->cpufreq_notify.notifier_call = rockchip_cpu_avs_notifier; return cpufreq_register_notifier(&avs->cpufreq_notify, CPUFREQ_POLICY_NOTIFIER); - -err: - of_node_put(np); - - return ret; } static struct platform_driver rockchip_cpu_avs_platdrv = { -- 2.34.1