From: "Hector.Yuan" <hector.yuan@xxxxxxxxxxxx>
Add cpufreq HW support.
Signed-off-by: Hector.Yuan <hector.yuan@xxxxxxxxxxxx>
+
+static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
+{
+ struct cpufreq_mtk *c;
+ struct device *cpu_dev;
+ struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);
+ struct pm_qos_request *qos_request;
+ int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS;
+
+ qos_request = kzalloc(sizeof(*qos_request), GFP_KERNEL);
+ if (!qos_request)
+ return -ENOMEM;
+
+ cpu_dev = get_cpu_device(policy->cpu);
+ if (!cpu_dev) {
+ pr_err("failed to get cpu%d device\n", policy->cpu);
+ return -ENODEV;
+ }
+
+ c = mtk_freq_domain_map[policy->cpu];
+ if (!c) {
+ pr_err("No scaling support for CPU%d\n", policy->cpu);
+ return -ENODEV;
+ }
+
+ cpumask_copy(policy->cpus, &c->related_cpus);
+
+ policy->freq_table = c->table;
+ policy->driver_data = c;
+
+ /* Let CPUs leave idle-off state for SVS CPU initializing */
+ cpu_latency_qos_add_request(qos_request, 0);
+
+ /* HW should be in enabled state to proceed now */
+ writel_relaxed(0x1, c->reg_bases[REG_FREQ_ENABLE]);
+
+ if (readl_poll_timeout(c->reg_bases[REG_FREQ_HW_STATE], sig,
+ (sig & pwr_hw) == pwr_hw, POLL_USEC,
+ TIMEOUT_USEC)) {
+ if (!(sig & CPUFREQ_HW_STATUS)) {
+ pr_info("cpufreq hardware of CPU%d is not enabled\n",
+ policy->cpu);
+ return -ENODEV;
+ }
+
+ pr_info("SVS of CPU%d is not enabled\n", policy->cpu);
+ }
+
+ em_dev_register_perf_domain(cpu_dev, c->nr_opp, &em_cb, policy->cpus);