|
| 1 | +#include <linux/cpufreq.h> |
| 2 | +#include <linux/kernel.h> |
| 3 | +#include <linux/module.h> |
| 4 | +#include <linux/init.h> |
| 5 | +#include <linux/cpumask.h> |
| 6 | +#include <linux/clk/clk-conf.h> |
| 7 | +#include <linux/pm_qos.h> |
| 8 | +#include <linux/notifier.h> |
| 9 | +#include "../opp/opp.h" |
| 10 | + |
| 11 | +#if 0 |
| 12 | +#define CCI_CLK_DEFAULT_RATE 245760 |
| 13 | + |
| 14 | +struct per_dev_qos { |
| 15 | + struct clk *clk; |
| 16 | + struct notifier_block notifier; |
| 17 | + struct dev_pm_qos_request req; |
| 18 | +}; |
| 19 | + |
| 20 | +static struct per_dev_qos *clk_dev_qos; |
| 21 | + |
| 22 | +static int spacemit_handle_clk_max_notifier_call(struct notifier_block *nb, unsigned long action, void *data) |
| 23 | +{ |
| 24 | + struct per_dev_qos *per_qos = container_of(nb, struct per_dev_qos, notifier); |
| 25 | + |
| 26 | + clk_set_rate(per_qos->clk, action * 1000); |
| 27 | + |
| 28 | + return 0; |
| 29 | +} |
| 30 | +#endif |
| 31 | + |
| 32 | +static int spacemit_policy_notifier(struct notifier_block *nb, |
| 33 | + unsigned long event, void *data) |
| 34 | +{ |
| 35 | + int cpu; |
| 36 | + u64 rates; |
| 37 | + static int cci_init; |
| 38 | + struct clk *cci_clk; |
| 39 | + struct device *cpu_dev; |
| 40 | + struct cpufreq_policy *policy = data; |
| 41 | + struct opp_table *opp_table; |
| 42 | + |
| 43 | + cpu = cpumask_first(policy->related_cpus); |
| 44 | + cpu_dev = get_cpu_device(cpu); |
| 45 | + opp_table = _find_opp_table(cpu_dev); |
| 46 | + |
| 47 | + if (cci_init == 0) { |
| 48 | + cci_clk = of_clk_get_by_name(opp_table->np, "cci"); |
| 49 | + of_property_read_u64_array(opp_table->np, "cci-hz", &rates, 1); |
| 50 | + clk_set_rate(cci_clk, rates); |
| 51 | + clk_put(cci_clk); |
| 52 | + cci_init = 1; |
| 53 | + } |
| 54 | + |
| 55 | + return 0; |
| 56 | +} |
| 57 | + |
| 58 | +static int spacemit_processor_notifier(struct notifier_block *nb, |
| 59 | + unsigned long event, void *data) |
| 60 | +{ |
| 61 | + int cpu; |
| 62 | + struct device *cpu_dev; |
| 63 | + struct cpufreq_freqs *freqs = (struct cpufreq_freqs *)data; |
| 64 | + struct cpufreq_policy *policy = ( struct cpufreq_policy *)freqs->policy; |
| 65 | + struct opp_table *opp_table; |
| 66 | + struct device_node *np; |
| 67 | + struct clk *tcm_clk, *ace_clk; |
| 68 | + u64 rates; |
| 69 | + |
| 70 | + cpu = cpumask_first(policy->related_cpus); |
| 71 | + cpu_dev = get_cpu_device(cpu); |
| 72 | + opp_table = _find_opp_table(cpu_dev); |
| 73 | + |
| 74 | +#if 0 |
| 75 | + if (clk_dev_qos == NULL) { |
| 76 | + clk_dev_qos = (struct per_dev_qos *)devm_kzalloc(cpu_dev, sizeof(struct per_dev_qos), GFP_KERNEL); |
| 77 | + if (!clk_dev_qos) { |
| 78 | + pr_err(" allocate per device qos error\n"); |
| 79 | + return -ENOMEM; |
| 80 | + } |
| 81 | + |
| 82 | + clk_dev_qos->clk = of_clk_get_by_name(opp_table->np, "cci"); |
| 83 | + clk_dev_qos->notifier.notifier_call = spacemit_handle_clk_max_notifier_call; |
| 84 | + dev_pm_qos_add_notifier(get_cpu_device(0), &clk_dev_qos->notifier, DEV_PM_QOS_MAX_FREQUENCY); |
| 85 | + dev_pm_qos_add_request(get_cpu_device(0), &clk_dev_qos->req, DEV_PM_QOS_MAX_FREQUENCY, CCI_CLK_DEFAULT_RATE); |
| 86 | + } |
| 87 | +#endif |
| 88 | + for_each_available_child_of_node(opp_table->np, np) { |
| 89 | + of_property_read_u64_array(np, "opp-hz", &rates, 1); |
| 90 | + if (rates == freqs->new * 1000) |
| 91 | + break; |
| 92 | + } |
| 93 | +#if 0 |
| 94 | + if (event == CPUFREQ_PRECHANGE) { |
| 95 | + /* decrease the freqs */ |
| 96 | + if (freqs->old > freqs->new) { |
| 97 | + /* decrease the cci first */ |
| 98 | + of_property_read_u64_array(np, "cci-hz", &rates, 1); |
| 99 | + dev_pm_qos_update_request(&clk_dev_qos->req, rates / 1000); |
| 100 | + } |
| 101 | + } |
| 102 | +#endif |
| 103 | + |
| 104 | + /* get the tcm/ace clk handler */ |
| 105 | + tcm_clk = of_clk_get_by_name(opp_table->np, "tcm"); |
| 106 | + ace_clk = of_clk_get_by_name(opp_table->np, "ace"); |
| 107 | + |
| 108 | + if (event == CPUFREQ_PRECHANGE) { |
| 109 | + /** |
| 110 | + * change the tcm/ace's frequency first. |
| 111 | + * binary division is safe |
| 112 | + */ |
| 113 | + if (!IS_ERR(ace_clk)) { |
| 114 | + clk_set_rate(ace_clk, clk_get_rate(clk_get_parent(ace_clk)) / 2); |
| 115 | + clk_put(ace_clk); |
| 116 | + } |
| 117 | + |
| 118 | + if (!IS_ERR(tcm_clk)) { |
| 119 | + clk_set_rate(tcm_clk, clk_get_rate(clk_get_parent(tcm_clk)) / 2); |
| 120 | + clk_put(tcm_clk); |
| 121 | + } |
| 122 | + } |
| 123 | + |
| 124 | + if (event == CPUFREQ_POSTCHANGE) { |
| 125 | + |
| 126 | + if (!IS_ERR(tcm_clk)) { |
| 127 | + clk_get_rate(clk_get_parent(tcm_clk)); |
| 128 | + /* get the tcm-hz */ |
| 129 | + of_property_read_u64_array(np, "tcm-hz", &rates, 1); |
| 130 | + /* then set rate */ |
| 131 | + clk_set_rate(tcm_clk, rates); |
| 132 | + clk_put(tcm_clk); |
| 133 | + } |
| 134 | + |
| 135 | + if (!IS_ERR(ace_clk)) { |
| 136 | + clk_get_rate(clk_get_parent(ace_clk)); |
| 137 | + /* get the ace-hz */ |
| 138 | + of_property_read_u64_array(np, "ace-hz", &rates, 1); |
| 139 | + /* then set rate */ |
| 140 | + clk_set_rate(ace_clk, rates); |
| 141 | + clk_put(ace_clk); |
| 142 | + } |
| 143 | +#if 0 |
| 144 | + /* increase the freqs */ |
| 145 | + if (freqs->old < freqs->new) { |
| 146 | + /* increase the cci after */ |
| 147 | + of_property_read_u64_array(np, "cci-hz", &rates, 1); |
| 148 | + dev_pm_qos_update_request(&clk_dev_qos->req, rates / 1000); |
| 149 | + } |
| 150 | +#endif |
| 151 | + } |
| 152 | + |
| 153 | + dev_pm_opp_put_opp_table(opp_table); |
| 154 | + |
| 155 | + return 0; |
| 156 | +} |
| 157 | + |
| 158 | +static struct notifier_block spacemit_processor_notifier_block = { |
| 159 | + .notifier_call = spacemit_processor_notifier, |
| 160 | +}; |
| 161 | + |
| 162 | +static struct notifier_block spacemit_policy_notifier_block = { |
| 163 | + .notifier_call = spacemit_policy_notifier, |
| 164 | +}; |
| 165 | + |
| 166 | +static int __init spacemit_processor_driver_init(void) |
| 167 | +{ |
| 168 | + int ret; |
| 169 | + |
| 170 | + ret = cpufreq_register_notifier(&spacemit_processor_notifier_block, CPUFREQ_TRANSITION_NOTIFIER); |
| 171 | + if (ret) { |
| 172 | + pr_err("register cpufreq notifier failed\n"); |
| 173 | + return -EINVAL; |
| 174 | + } |
| 175 | + |
| 176 | + ret = cpufreq_register_notifier(&spacemit_policy_notifier_block, CPUFREQ_POLICY_NOTIFIER); |
| 177 | + if (ret) { |
| 178 | + pr_err("register cpufreq notifier failed\n"); |
| 179 | + return -EINVAL; |
| 180 | + } |
| 181 | + |
| 182 | + return 0; |
| 183 | +} |
| 184 | + |
| 185 | +arch_initcall(spacemit_processor_driver_init); |
0 commit comments