static void init_primary_helper(uint32_t pageable_part, uint32_t nsec_entry)
{
/*
* Mask asynchronous exceptions before switch to the thread vector
* as the thread handler requires those to be masked while
* executing with the temporary stack. The thread subsystem also
* asserts that IRQ is blocked when using most if its functions.
*/
thread_set_exceptions(THREAD_EXCP_ALL);
init_vfp_sec();
init_runtime(pageable_part);
IMSG("Initializing (%s)\n", core_v_str);
thread_init_primary(generic_boot_get_handlers());
thread_init_per_cpu();
init_sec_mon(nsec_entry);
main_init_gic();
init_vfp_nsec();
if (init_teecore() != TEE_SUCCESS)
panic();
DMSG("Primary CPU switching to normal world boot\n");
}
static void main_init_helper(bool is_primary, size_t pos, uint32_t nsec_entry)
{
/*
* Mask external Abort, IRQ and FIQ before switch to the thread
* vector as the thread handler requires externl Abort, IRQ and FIQ
* to be masked while executing with the temporary stack. The
* thread subsystem also asserts that IRQ is blocked when using
* most if its functions.
*/
write_cpsr(read_cpsr() | CPSR_FIA);
if (is_primary) {
uintptr_t bss_start = (uintptr_t)&__bss_start;
uintptr_t bss_end = (uintptr_t)&__bss_end;
size_t n;
/* Initialize uart with physical address */
uart_init(CONSOLE_UART_BASE);
/*
* Zero BSS area. Note that globals that would normally
* would go into BSS which are used before this has to be
* put into .nozi.* to avoid getting overwritten.
*/
memset((void *)bss_start, 0, bss_end - bss_start);
DMSG("TEE initializing\n");
/* Initialize canaries around the stacks */
init_canaries();
/* Assign the thread stacks */
for (n = 0; n < NUM_THREADS; n++) {
if (!thread_init_stack(n, GET_STACK(stack_thread[n])))
panic();
}
}
if (!thread_init_stack(THREAD_TMP_STACK, GET_STACK(stack_tmp[pos])))
panic();
if (!thread_init_stack(THREAD_ABT_STACK, GET_STACK(stack_abt[pos])))
panic();
thread_init_handlers(&handlers);
main_init_sec_mon(pos, nsec_entry);
if (is_primary) {
main_init_gic();
if (init_teecore() != TEE_SUCCESS)
panic();
DMSG("Primary CPU switching to normal world boot\n");
} else {
DMSG("Secondary CPU Switching to normal world boot\n");
}
}
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;
}