msm: acpuclock-cortex: Detect bootloader cpu configuration
Detect which cpu rate is currently selected by the bootloaders during
probe. If the rate is invalid, then set the cpu rate to maximum.
Change-Id: I316d5de76133e76993a47b0d2917fd35bc5dd590
Signed-off-by: Patrick Daly <pdaly@codeaurora.org>
diff --git a/arch/arm/mach-msm/acpuclock-8226.c b/arch/arm/mach-msm/acpuclock-8226.c
index 733c7a8..0028d6e 100644
--- a/arch/arm/mach-msm/acpuclock-8226.c
+++ b/arch/arm/mach-msm/acpuclock-8226.c
@@ -87,7 +87,6 @@
static struct acpuclk_drv_data drv_data = {
.freq_tbl = acpu_freq_tbl_8226,
- .current_speed = &(struct clkctl_acpu_speed){ 0 },
.bus_scale = &bus_client_pdata,
.vdd_max_cpu = CPR_CORNER_TURBO,
.src_clocks = {
diff --git a/arch/arm/mach-msm/acpuclock-9625.c b/arch/arm/mach-msm/acpuclock-9625.c
index 42659f9..a4a5b2a 100644
--- a/arch/arm/mach-msm/acpuclock-9625.c
+++ b/arch/arm/mach-msm/acpuclock-9625.c
@@ -65,7 +65,6 @@
static struct acpuclk_drv_data drv_data = {
.freq_tbl = acpu_freq_tbl,
- .current_speed = &(struct clkctl_acpu_speed){ 0 },
.bus_scale = &bus_client_pdata,
.vdd_max_cpu = LVL_HIGH,
.vdd_max_mem = 1050000,
diff --git a/arch/arm/mach-msm/acpuclock-cortex.c b/arch/arm/mach-msm/acpuclock-cortex.c
index afa6909..2c3f97b 100644
--- a/arch/arm/mach-msm/acpuclock-cortex.c
+++ b/arch/arm/mach-msm/acpuclock-cortex.c
@@ -138,6 +138,49 @@
pr_warn("acpu rcg didn't update its configuration\n");
}
+static struct clkctl_acpu_speed *__init find_cur_cpu_level(void)
+{
+ struct clkctl_acpu_speed *f, *max = priv->freq_tbl;
+ void __iomem *apcs_rcg_config = priv->apcs_rcg_config;
+ struct acpuclk_reg_data *r = &priv->reg_data;
+ u32 regval, div, src;
+ unsigned long rate;
+ struct clk *parent;
+
+ regval = readl_relaxed(apcs_rcg_config);
+ src = regval & r->cfg_src_mask;
+ src >>= r->cfg_src_shift;
+
+ div = regval & r->cfg_div_mask;
+ div >>= r->cfg_div_shift;
+ /* No support for half-integer dividers */
+ div = div > 1 ? (div + 1) / 2 : 0;
+
+ for (f = priv->freq_tbl; f->khz; f++) {
+ if (f->use_for_scaling)
+ max = f;
+
+ if (f->src_sel != src || f->src_div != div)
+ continue;
+
+ parent = priv->src_clocks[f->src].clk;
+ rate = parent->rate / (div ? div : 1);
+ if (f->khz * 1000 == rate)
+ break;
+ }
+
+ if (f->khz)
+ return f;
+
+ pr_err("CPUs are running at an unknown rate. Defaulting to %u KHz.\n",
+ max->khz);
+
+ /* Change to a safe frequency */
+ select_clk_source_div(priv, priv->freq_tbl);
+ /* Default to largest frequency */
+ return max;
+}
+
static int set_speed_atomic(struct clkctl_acpu_speed *tgt_s)
{
struct clkctl_acpu_speed *strt_s = priv->current_speed;
@@ -231,7 +274,7 @@
strt_s = priv->current_speed;
/* Return early if rate didn't change */
- if (rate == strt_s->khz)
+ if (rate == strt_s->khz && reason != SETRATE_INIT)
goto out;
/* Find target frequency */
@@ -244,7 +287,7 @@
}
/* Increase VDD levels if needed */
- if ((reason == SETRATE_CPUFREQ || reason == SETRATE_INIT)
+ if ((reason == SETRATE_CPUFREQ)
&& (tgt_s->khz > strt_s->khz)) {
rc = increase_vdd(tgt_s->vdd_cpu, tgt_s->vdd_mem);
if (rc)
@@ -274,7 +317,7 @@
set_bus_bw(tgt_s->bw_level);
/* Drop VDD levels if we can. */
- if (tgt_s->khz < strt_s->khz)
+ if (tgt_s->khz < strt_s->khz || reason == SETRATE_INIT)
decrease_vdd(tgt_s->vdd_cpu, tgt_s->vdd_mem);
out:
@@ -328,8 +371,8 @@
int __init acpuclk_cortex_init(struct platform_device *pdev,
struct acpuclk_drv_data *data)
{
- unsigned long max_cpu_khz = 0;
- int i, rc;
+ int rc;
+ int parent;
priv = data;
mutex_init(&priv->lock);
@@ -343,46 +386,41 @@
BUG();
}
- /* Improve boot time by ramping up CPU immediately */
- for (i = 0; priv->freq_tbl[i].khz != 0; i++)
- if (priv->freq_tbl[i].use_for_scaling)
- max_cpu_khz = priv->freq_tbl[i].khz;
-
/* Initialize regulators */
rc = increase_vdd(priv->vdd_max_cpu, priv->vdd_max_mem);
if (rc)
- goto err_vdd;
+ return rc;
if (priv->vdd_mem) {
rc = regulator_enable(priv->vdd_mem);
if (rc) {
dev_err(&pdev->dev, "regulator_enable for mem failed\n");
- goto err_vdd;
+ return rc;
}
}
rc = regulator_enable(priv->vdd_cpu);
if (rc) {
dev_err(&pdev->dev, "regulator_enable for cpu failed\n");
- goto err_vdd_cpu;
+ return rc;
}
- /*
- * Select a state which is always a valid transition to align SW with
- * the HW configuration set by the bootloaders.
- */
- acpuclk_cortex_set_rate(0, acpuclk_cortex_data.power_collapse_khz,
- SETRATE_INIT);
- acpuclk_cortex_set_rate(0, max_cpu_khz, SETRATE_INIT);
+ priv->current_speed = find_cur_cpu_level();
+ parent = priv->current_speed->src;
+ rc = clk_prepare_enable(priv->src_clocks[parent].clk);
+ if (rc) {
+ dev_err(&pdev->dev, "handoff: prepare_enable failed\n");
+ return rc;
+ }
+
+ rc = acpuclk_cortex_set_rate(0, priv->current_speed->khz, SETRATE_INIT);
+ if (rc) {
+ dev_err(&pdev->dev, "handoff: set rate failed\n");
+ return rc;
+ }
acpuclk_register(&acpuclk_cortex_data);
cpufreq_table_init();
return 0;
-
-err_vdd_cpu:
- if (priv->vdd_mem)
- regulator_disable(priv->vdd_mem);
-err_vdd:
- return rc;
}
diff --git a/arch/arm/mach-msm/clock-8226.c b/arch/arm/mach-msm/clock-8226.c
index 1477541..f1df505 100644
--- a/arch/arm/mach-msm/clock-8226.c
+++ b/arch/arm/mach-msm/clock-8226.c
@@ -2846,11 +2846,6 @@
},
.num_fmax = VDD_SR2_PLL_NUM,
CLK_INIT(a7sspll.c),
- /*
- * Need to skip handoff of the acpu pll to avoid
- * turning off the pll when the cpu is using it
- */
- .flags = CLKFLAG_SKIP_HANDOFF,
},
};
diff --git a/arch/arm/mach-msm/clock-8610.c b/arch/arm/mach-msm/clock-8610.c
index e34b539..9a66b78 100644
--- a/arch/arm/mach-msm/clock-8610.c
+++ b/arch/arm/mach-msm/clock-8610.c
@@ -577,11 +577,6 @@
},
.num_fmax = VDD_SR2_PLL_NUM,
CLK_INIT(a7sspll.c),
- /*
- * Need to skip handoff of the acpu pll to avoid
- * turning off the pll when the cpu is using it
- */
- .flags = CLKFLAG_SKIP_HANDOFF,
},
};
diff --git a/arch/arm/mach-msm/clock-9625.c b/arch/arm/mach-msm/clock-9625.c
index 3277d75..adb1101 100644
--- a/arch/arm/mach-msm/clock-9625.c
+++ b/arch/arm/mach-msm/clock-9625.c
@@ -390,10 +390,6 @@
PLL_F_END
};
-/*
- * Need to skip handoff of the acpu pll to avoid handoff code
- * to turn off the pll when the acpu is running off this pll.
- */
static struct pll_clk apcspll_clk_src = {
.mode_reg = (void __iomem *)APCS_CPU_PLL_MODE_REG,
.l_reg = (void __iomem *)APCS_CPU_PLL_L_REG,
@@ -415,7 +411,6 @@
.dbg_name = "apcspll_clk_src",
.ops = &clk_ops_local_pll,
CLK_INIT(apcspll_clk_src.c),
- .flags = CLKFLAG_SKIP_HANDOFF,
},
};