@@ -7,20 +7,27 @@
#include <linux/cpufreq.h>
#include <linux/energy_model.h>
#include <linux/init.h>
+#include <linux/iopoll.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/of_address.h>
#include <linux/of_platform.h>
+#include <linux/pm_qos.h>
#include <linux/slab.h>
#define LUT_MAX_ENTRIES 32U
#define LUT_FREQ GENMASK(11, 0)
#define LUT_ROW_SIZE 0x4
+#define CPUFREQ_HW_STATUS BIT(0)
+#define SVS_HW_STATUS BIT(1)
+#define POLL_USEC 1000
+#define TIMEOUT_USEC 300000
enum {
REG_FREQ_LUT_TABLE,
REG_FREQ_ENABLE,
REG_FREQ_PERF_STATE,
+ REG_FREQ_HW_STATE,
REG_EM_POWER_TBL,
REG_ARRAY_SIZE,
@@ -37,6 +44,7 @@ struct cpufreq_mtk {
[REG_FREQ_LUT_TABLE] = 0x0,
[REG_FREQ_ENABLE] = 0x84,
[REG_FREQ_PERF_STATE] = 0x88,
+ [REG_FREQ_HW_STATE] = 0x8c,
[REG_EM_POWER_TBL] = 0x3D0,
};
@@ -89,6 +97,12 @@ 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) {
@@ -107,11 +121,29 @@ static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
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);
+ cpu_latency_qos_remove_request(qos_request);
+ kfree(qos_request);
+
return 0;
}