diff --git a/include/lib/psci/psci.h b/include/lib/psci/psci.h index 0b44ab2e0..1fd4ec109 100644 --- a/include/lib/psci/psci.h +++ b/include/lib/psci/psci.h @@ -65,6 +65,8 @@ #define PSCI_STAT_RESIDENCY_AARCH64 U(0xc4000010) #define PSCI_STAT_COUNT_AARCH32 U(0x84000011) #define PSCI_STAT_COUNT_AARCH64 U(0xc4000011) +#define PSCI_SYSTEM_RESET2_AARCH32 U(0x84000012) +#define PSCI_SYSTEM_RESET2_AARCH64 U(0xc4000012) #define PSCI_MEM_PROTECT U(0x84000013) #define PSCI_MEM_CHK_RANGE_AARCH32 U(0x84000014) #define PSCI_MEM_CHK_RANGE_AARCH64 U(0xc4000014) @@ -167,6 +169,14 @@ #define PSCI_INVALID_MPIDR ~((u_register_t)0) +/* + * SYSTEM_RESET2 macros + */ +#define PSCI_RESET2_TYPE_VENDOR_SHIFT 31 +#define PSCI_RESET2_TYPE_VENDOR (1U << PSCI_RESET2_TYPE_VENDOR_SHIFT) +#define PSCI_RESET2_TYPE_ARCH (0U << PSCI_RESET2_TYPE_VENDOR_SHIFT) +#define PSCI_RESET2_SYSTEM_WARM_RESET (PSCI_RESET2_TYPE_ARCH | 0) + #ifndef __ASSEMBLY__ #include @@ -294,6 +304,8 @@ typedef struct plat_psci_ops { int (*mem_protect_chk)(uintptr_t base, u_register_t length); int (*read_mem_protect)(int *val); int (*write_mem_protect)(int val); + int (*system_reset2)(int is_vendor, + int reset_type, u_register_t cookie); } plat_psci_ops_t; /******************************************************************************* diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c index a5d707e01..4105e63bd 100644 --- a/lib/psci/psci_main.c +++ b/lib/psci/psci_main.c @@ -414,6 +414,10 @@ u_register_t psci_smc_handler(uint32_t smc_fid, case PSCI_MEM_CHK_RANGE_AARCH32: return psci_mem_chk_range(x1, x2); + case PSCI_SYSTEM_RESET2_AARCH32: + /* We should never return from psci_system_reset2() */ + return psci_system_reset2(x1, x2); + default: break; } @@ -453,6 +457,9 @@ u_register_t psci_smc_handler(uint32_t smc_fid, case PSCI_MEM_CHK_RANGE_AARCH64: return psci_mem_chk_range(x1, x2); + case PSCI_SYSTEM_RESET2_AARCH64: + /* We should never return from psci_system_reset2() */ + return psci_system_reset2(x1, x2); default: break; diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h index facfacb03..f38421aa9 100644 --- a/lib/psci/psci_private.h +++ b/lib/psci/psci_private.h @@ -89,7 +89,8 @@ define_psci_cap(PSCI_NODE_HW_STATE_AARCH64) | \ define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64) | \ define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64) | \ - define_psci_cap(PSCI_STAT_COUNT_AARCH64)) + define_psci_cap(PSCI_STAT_COUNT_AARCH64) | \ + define_psci_cap(PSCI_SYSTEM_RESET2_AARCH64)) /* * Helper macros to get/set the fields of PSCI per-cpu data. @@ -258,6 +259,7 @@ void psci_do_pwrup_cache_maintenance(void); /* Private exported functions from psci_system_off.c */ void __dead2 psci_system_off(void); void __dead2 psci_system_reset(void); +int psci_system_reset2(uint32_t reset_type, u_register_t cookie); /* Private exported functions from psci_stat.c */ void psci_stats_update_pwr_down(unsigned int end_pwrlvl, diff --git a/lib/psci/psci_setup.c b/lib/psci/psci_setup.c index 5ef49acbe..a841ddab9 100644 --- a/lib/psci/psci_setup.c +++ b/lib/psci/psci_setup.c @@ -248,6 +248,8 @@ int psci_setup(const psci_lib_args_t *lib_args) psci_caps |= define_psci_cap(PSCI_MEM_PROTECT); if (psci_plat_pm_ops->mem_protect_chk) psci_caps |= define_psci_cap(PSCI_MEM_CHK_RANGE_AARCH64); + if (psci_plat_pm_ops->system_reset2) + psci_caps |= define_psci_cap(PSCI_SYSTEM_RESET2_AARCH64); #if ENABLE_PSCI_STAT psci_caps |= define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64); diff --git a/lib/psci/psci_system_off.c b/lib/psci/psci_system_off.c index ef5d3d1d6..13e9f4aae 100644 --- a/lib/psci/psci_system_off.c +++ b/lib/psci/psci_system_off.c @@ -49,3 +49,33 @@ void __dead2 psci_system_reset(void) /* This function does not return. We should never get here */ } + +int psci_system_reset2(uint32_t reset_type, u_register_t cookie) +{ + int is_vendor; + + psci_print_power_domain_map(); + + assert(psci_plat_pm_ops->system_reset2); + + is_vendor = (reset_type >> PSCI_RESET2_TYPE_VENDOR_SHIFT) & 1; + if (!is_vendor) { + /* + * Only WARM_RESET is allowed for architectural type resets. + */ + if (reset_type != PSCI_RESET2_SYSTEM_WARM_RESET) + return PSCI_E_INVALID_PARAMS; + if (psci_plat_pm_ops->write_mem_protect && + psci_plat_pm_ops->write_mem_protect(0) < 0) { + return PSCI_E_NOT_SUPPORTED; + } + } + + /* Notify the Secure Payload Dispatcher */ + if (psci_spd_pm && psci_spd_pm->svc_system_reset) { + psci_spd_pm->svc_system_reset(); + } + console_flush(); + + return psci_plat_pm_ops->system_reset2(is_vendor, reset_type, cookie); +}