Merge pull request #1123 from robertovargas-arm/reset2

Integration of reset2 PSCI v1.1 functionality

4 files changed, 42 insertions, 1 deletions

diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c
index a5d707e0..4105e63b 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 facfacb0..f38421aa 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 5ef49acb..a841ddab 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 ef5d3d1d..13e9f4aa 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);
+}