static void __init imx7d_init_late(void)
{
if (IS_ENABLED(CONFIG_ARM_IMX7D_CPUFREQ)) {
platform_device_register_simple("imx7d-cpufreq", -1, NULL, 0);
}
imx7d_cpuidle_init();
}
int imx7d_lowpower_idle(uint32_t power_state __unused,
uintptr_t entry __unused,
uint32_t context_id __unused,
struct sm_nsec_ctx *nsec)
{
struct imx7_pm_info *p;
uint32_t cpuidle_ocram_base;
static uint32_t gic_inited;
int ret;
uint32_t cpu_id __maybe_unused = get_core_pos();
uint32_t type = (power_state & PSCI_POWER_STATE_TYPE_MASK) >>
PSCI_POWER_STATE_TYPE_SHIFT;
uint32_t cpu = get_core_pos();
cpuidle_ocram_base = core_mmu_get_va(TRUSTZONE_OCRAM_START +
LOWPOWER_IDLE_OCRAM_OFFSET,
MEM_AREA_TEE_COHERENT);
p = (struct imx7_pm_info *)cpuidle_ocram_base;
imx_pen_lock(cpu);
if (!lowpoweridle_init) {
imx7d_cpuidle_init();
lowpoweridle_init = 1;
}
if (type != PSCI_POWER_STATE_TYPE_POWER_DOWN)
panic();
p->num_online_cpus = get_online_cpus();
p->num_lpi_cpus++;
sm_save_unbanked_regs(&nsec->ub_regs);
ret = sm_pm_cpu_suspend((uint32_t)p, (int (*)(uint32_t))
(cpuidle_ocram_base + sizeof(*p)));
/*
* Sometimes cpu_suspend may not really suspended, we need to check
* it's return value to restore reg or not
*/
if (ret < 0) {
p->num_lpi_cpus--;
imx_pen_unlock(cpu);
DMSG("=== Not suspended, GPC IRQ Pending === %d\n", cpu_id);
return 0;
}
/*
* Restore register of different mode in secure world
* When cpu powers up, after ROM init, cpu in secure SVC
* mode, we first need to restore monitor regs.
*/
sm_restore_unbanked_regs(&nsec->ub_regs);
p->num_lpi_cpus--;
/* Back to Linux */
nsec->mon_lr = (uint32_t)entry;
if (gic_inited == 0) {
/*
* TODO: Call the Wakeup Late function to restore some
* HW configuration (e.g. TZASC)
*/
plat_cpu_reset_late();
main_init_gic();
gic_inited = 1;
DMSG("=== Back from Suspended ===\n");
} else {
main_secondary_init_gic();
gic_inited = 0;
}
imx_pen_unlock(cpu);
return 0;
}
