| // SPDX-License-Identifier: BSD-2-Clause |
| /* |
| * Copyright 2018 NXP |
| * |
| * Peng Fan <peng.fan@nxp.com> |
| */ |
| |
| #include <arm.h> |
| #include <arm32.h> |
| #include <atomic.h> |
| #include <console.h> |
| #include <drivers/imx_uart.h> |
| #include <io.h> |
| #include <imx.h> |
| #include <imx_pm.h> |
| #include <kernel/cache_helpers.h> |
| #include <kernel/generic_boot.h> |
| #include <kernel/interrupt.h> |
| #include <kernel/misc.h> |
| #include <kernel/panic.h> |
| #include <kernel/pm_stubs.h> |
| #include <kernel/spinlock.h> |
| #include <mm/core_mmu.h> |
| #include <mm/core_memprot.h> |
| #include <platform_config.h> |
| #include <stdint.h> |
| #include <sm/optee_smc.h> |
| #include <sm/psci.h> |
| #include <sm/pm.h> |
| #include <sm/sm.h> |
| #include <string.h> |
| #include <tee/entry_std.h> |
| #include <tee/entry_fast.h> |
| #include <util.h> |
| |
| int imx7d_cpuidle_init(void) |
| { |
| uint32_t lpm_idle_ocram_base = |
| core_mmu_get_va(TRUSTZONE_OCRAM_START + |
| LOWPOWER_IDLE_OCRAM_OFFSET, |
| MEM_AREA_TEE_COHERENT); |
| struct imx7_pm_info *p = |
| (struct imx7_pm_info *)lpm_idle_ocram_base; |
| |
| pm_imx7_iram_tbl_init(); |
| |
| dcache_op_level1(DCACHE_OP_CLEAN_INV); |
| |
| p->va_base = lpm_idle_ocram_base; |
| p->pa_base = TRUSTZONE_OCRAM_START + LOWPOWER_IDLE_OCRAM_OFFSET; |
| p->tee_resume = (paddr_t)virt_to_phys((void *)(vaddr_t)v7_cpu_resume); |
| p->pm_info_size = sizeof(*p); |
| p->ddrc_va_base = core_mmu_get_va(DDRC_BASE, MEM_AREA_IO_SEC); |
| p->ddrc_pa_base = DDRC_BASE; |
| p->ccm_va_base = core_mmu_get_va(CCM_BASE, MEM_AREA_IO_SEC); |
| p->ccm_pa_base = CCM_BASE; |
| p->anatop_va_base = core_mmu_get_va(ANATOP_BASE, MEM_AREA_IO_SEC); |
| p->anatop_pa_base = ANATOP_BASE; |
| p->src_va_base = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC); |
| p->src_pa_base = SRC_BASE; |
| p->iomuxc_gpr_va_base = core_mmu_get_va(IOMUXC_GPR_BASE, |
| MEM_AREA_IO_SEC); |
| p->iomuxc_gpr_pa_base = IOMUXC_GPR_BASE; |
| p->gpc_va_base = core_mmu_get_va(GPC_BASE, MEM_AREA_IO_SEC); |
| p->gpc_pa_base = GPC_BASE; |
| p->gic_va_base = core_mmu_get_va(GIC_BASE, MEM_AREA_IO_SEC); |
| p->gic_pa_base = GIC_BASE; |
| |
| p->num_lpi_cpus = 0; |
| p->num_online_cpus = -1; |
| |
| memcpy((void *)(lpm_idle_ocram_base + sizeof(*p)), |
| (void *)(vaddr_t)imx7d_low_power_idle, |
| LOWPOWER_IDLE_OCRAM_SIZE - sizeof(*p)); |
| |
| dcache_clean_range((void *)lpm_idle_ocram_base, |
| LOWPOWER_IDLE_OCRAM_SIZE); |
| /* |
| * Note that IRAM IOSEC map, if changed to MEM map, |
| * need to flush cache |
| */ |
| icache_inv_all(); |
| |
| return 0; |
| } |
| |
| static int lowpoweridle_init; |
| |
| static void imx_pen_lock(uint32_t cpu) |
| { |
| uint32_t cpuidle_ocram_base; |
| struct imx7_pm_info *p; |
| |
| 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; |
| |
| if (cpu == 0) { |
| atomic_store_u32(&p->flag0, 1); |
| dsb(); |
| atomic_store_u32(&p->val, cpu); |
| do { |
| dsb(); |
| } while (atomic_load_u32(&p->flag1) == 1 |
| && atomic_load_u32(&p->val) == cpu) |
| ; |
| } else { |
| atomic_store_u32(&p->flag1, 1); |
| dsb(); |
| atomic_store_u32(&p->val, cpu); |
| do { |
| dsb(); |
| } while (atomic_load_u32(&p->flag0) == 1 |
| && atomic_load_u32(&p->val) == cpu) |
| ; |
| } |
| } |
| |
| static void imx_pen_unlock(int cpu) |
| { |
| uint32_t cpuidle_ocram_base; |
| struct imx7_pm_info *p; |
| |
| 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; |
| |
| dsb(); |
| if (cpu == 0) |
| atomic_store_u32(&p->flag0, 0); |
| else |
| atomic_store_u32(&p->flag1, 0); |
| } |
| |
| static uint32_t get_online_cpus(void) |
| { |
| vaddr_t src_a7rcr1 = core_mmu_get_va(SRC_BASE + SRC_A7RCR1, |
| MEM_AREA_IO_SEC); |
| uint32_t val = io_read32(src_a7rcr1); |
| |
| return (val & (1 << SRC_A7RCR1_A7_CORE1_ENABLE_OFFSET)) ? 2 : 1; |
| } |
| |
| 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) |
| */ |
| if (!get_core_pos()) |
| plat_primary_init_early(); |
| |
| 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; |
| } |