/*******************************************************************************
* FVP handler called when an affinity instance is about to be turned on. The
* level and mpidr determine the affinity instance.
******************************************************************************/
int plat_affinst_on(unsigned long mpidr,
unsigned long sec_entrypoint,
unsigned long ns_entrypoint,
unsigned int afflvl,
unsigned int state)
{
int rc = PSCI_E_SUCCESS;
unsigned long linear_id;
/*
* It's possible to turn on only affinity level 0 i.e. a cpu
* on the FVP. Ignore any other affinity level.
*/
if (afflvl != MPIDR_AFFLVL0)
return rc;
/*
* Ensure that we do not cancel an inflight power off request
* for the target cpu. That would leave it in a zombie wfi.
* Wait for it to power off, program the jump address for the
* target cpu and then program the power controller to turn
* that cpu on
*/
// do {
// psysr = plat_pwrc_read_psysr(mpidr);
// } while (psysr & PSYSR_AFF_L0);
plat_program_mailbox(mpidr, sec_entrypoint);
// plat_pwrc_write_pponr(mpidr);
linear_id = platform_get_core_pos(mpidr);
extern void bl31_on_entrypoint(void);
if (linear_id >= 4) {
mmio_write_32(MP1_MISC_CONFIG_BOOT_ADDR(linear_id-4), (unsigned long)bl31_on_entrypoint);
printf("mt_on_1, entry %x\n", mmio_read_32(MP1_MISC_CONFIG_BOOT_ADDR(linear_id-4)));
} else {
/* set secondary CPUs to AArch64 */
mmio_write_32(MP0_MISC_CONFIG3, mmio_read_32(MP0_MISC_CONFIG3) | 0x0000E000);
mmio_write_32(MP0_MISC_CONFIG_BOOT_ADDR(linear_id), (unsigned long)bl31_on_entrypoint);
printf("mt_on_0, entry %x\n", mmio_read_32(MP1_MISC_CONFIG_BOOT_ADDR(linear_id)));
}
return rc;
}
/*******************************************************************************
* MTK_platform handler called when an affinity instance is about to be turned on. The
* level and mpidr determine the affinity instance.
******************************************************************************/
int mt_affinst_on(unsigned long mpidr,
unsigned long sec_entrypoint,
unsigned long ns_entrypoint,
unsigned int afflvl,
unsigned int state)
{
int rc = PSCI_E_SUCCESS;
unsigned long linear_id;
mailbox_t *mt_mboxes;
/*
* It's possible to turn on only affinity level 0 i.e. a cpu
* on the MTK_platform. Ignore any other affinity level.
*/
if (afflvl != MPIDR_AFFLVL0)
goto exit;
linear_id = platform_get_core_pos(mpidr);
mt_mboxes = (mailbox_t *) (MBOX_OFF);
mt_mboxes[linear_id].value = sec_entrypoint;
flush_dcache_range((unsigned long) &mt_mboxes[linear_id],
sizeof(unsigned long));
extern void bl31_on_entrypoint(void);
if (linear_id >= 4) {
mmio_write_32(MP1_MISC_CONFIG_BOOT_ADDR(linear_id-4), (unsigned long)bl31_on_entrypoint);
printf("mt_on, entry %x\n", mmio_read_32(MP1_MISC_CONFIG_BOOT_ADDR(linear_id-4)));
} else {
/* set secondary CPUs to AArch64 */
mmio_write_32(MP0_MISC_CONFIG3, mmio_read_32(MP0_MISC_CONFIG3) | 0x0000E000);
mmio_write_32(MP0_MISC_CONFIG_BOOT_ADDR(linear_id), (unsigned long)bl31_on_entrypoint);
printf("mt_on, entry %x\n", mmio_read_32(MP0_MISC_CONFIG_BOOT_ADDR(linear_id)));
}
exit:
return rc;
}
