diff --git a/plat/nvidia/tegra/soc/t186/plat_psci_handlers.c b/plat/nvidia/tegra/soc/t186/plat_psci_handlers.c index 0123854831f207fd397ff379f0320cd47f9d76c3..35b7ee91565bf6a75c5702f72ac0f5831ae89f40 100644 --- a/plat/nvidia/tegra/soc/t186/plat_psci_handlers.c +++ b/plat/nvidia/tegra/soc/t186/plat_psci_handlers.c @@ -28,6 +28,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include <arch.h> +#include <arch_helpers.h> +#include <debug.h> +#include <mce.h> #include <psci.h> #include <tegra_private.h> @@ -45,3 +49,47 @@ int32_t tegra_soc_validate_power_state(unsigned int power_state) return PSCI_E_SUCCESS; } + +int tegra_soc_prepare_cpu_on(unsigned long mpidr) +{ + int target_cpu = mpidr & MPIDR_CPU_MASK; + int target_cluster = (mpidr & MPIDR_CLUSTER_MASK) >> + MPIDR_AFFINITY_BITS; + + if (target_cluster > MPIDR_AFFLVL1) { + ERROR("%s: unsupported CPU (0x%lx)\n", __func__, mpidr); + return PSCI_E_NOT_PRESENT; + } + + /* construct the target CPU # */ + target_cpu |= (target_cluster << 2); + + mce_command_handler(MCE_CMD_ONLINE_CORE, target_cpu, 0, 0); + + return PSCI_E_SUCCESS; +} + +int tegra_soc_prepare_cpu_on_finish(unsigned long mpidr) +{ + /* + * Check if we are exiting from SOC_POWERDN. + */ + if (tegra_system_suspended()) { + + /* + * System resume complete. + */ + tegra_pm_system_suspend_exit(); + } + + return PSCI_E_SUCCESS; +} + +int tegra_soc_prepare_cpu_off(unsigned long mpidr) +{ + /* Turn off wake_mask */ + mce_command_handler(MCE_CMD_UPDATE_CSTATE_INFO, 0, 0, 1); + + /* Turn off CPU */ + return mce_command_handler(MCE_CMD_ENTER_CSTATE, ~0, 0, 0); +} diff --git a/plat/nvidia/tegra/soc/t186/plat_secondary.c b/plat/nvidia/tegra/soc/t186/plat_secondary.c index 85cc32c842118d8cd38ec8ba1747804489daf204..df802891a9be45bfb371922364a5860a4a21fd59 100644 --- a/plat/nvidia/tegra/soc/t186/plat_secondary.c +++ b/plat/nvidia/tegra/soc/t186/plat_secondary.c @@ -28,10 +28,45 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include <debug.h> +#include <mce.h> +#include <mmio.h> +#include <tegra_def.h> + +#define MISCREG_CPU_RESET_VECTOR 0x2000 +#define MISCREG_AA64_RST_LOW 0x2004 +#define MISCREG_AA64_RST_HIGH 0x2008 + +#define SCRATCH_SECURE_RSV1_SCRATCH_0 0x658 +#define SCRATCH_SECURE_RSV1_SCRATCH_1 0x65C + +#define CPU_RESET_MODE_AA64 1 + +extern void tegra_secure_entrypoint(void); + /******************************************************************************* * Setup secondary CPU vectors ******************************************************************************/ void plat_secondary_setup(void) { - ; /* do nothing */ + uint32_t addr_low, addr_high; + uint64_t reset_addr = (uint64_t)tegra_secure_entrypoint; + + INFO("Setting up secondary CPU boot\n"); + + addr_low = (uint32_t)reset_addr | CPU_RESET_MODE_AA64; + addr_high = (uint32_t)((reset_addr >> 32) & 0x7ff); + + /* write lower 32 bits first, then the upper 11 bits */ + mmio_write_32(TEGRA_MISC_BASE + MISCREG_AA64_RST_LOW, addr_low); + mmio_write_32(TEGRA_MISC_BASE + MISCREG_AA64_RST_HIGH, addr_high); + + /* save reset vector to be used during SYSTEM_SUSPEND exit */ + mmio_write_32(TEGRA_SCRATCH_BASE + SCRATCH_SECURE_RSV1_SCRATCH_0, + addr_low); + mmio_write_32(TEGRA_SCRATCH_BASE + SCRATCH_SECURE_RSV1_SCRATCH_1, + addr_high); + + /* update reset vector address to the CCPLEX */ + mce_update_reset_vector(addr_low, addr_high); }