Merge pull request #1123 from robertovargas-arm/reset2

Integration of reset2 PSCI v1.1 functionality

8 files changed, 103 insertions, 41 deletions

diff --git a/include/lib/psci/psci.h b/include/lib/psci/psci.h
index 0b44ab2e..1fd4ec10 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 <stdint.h>
@@ -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 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);
+}
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index 93d51fe6..39c02aff 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.c
@@ -300,4 +300,7 @@ plat_psci_ops_t plat_arm_psci_pm_ops = {
 	.read_mem_protect	= arm_psci_read_mem_protect,
 	.write_mem_protect	= arm_nor_psci_write_mem_protect,
 #endif
+#if CSS_USE_SCMI_SDS_DRIVER
+	.system_reset2		= css_system_reset2,
+#endif
 };
diff --git a/plat/arm/css/drivers/scp/css_pm_scmi.c b/plat/arm/css/drivers/scp/css_pm_scmi.c
index c39bc4ba..e29cd867 100644
--- a/plat/arm/css/drivers/scp/css_pm_scmi.c
+++ b/plat/arm/css/drivers/scp/css_pm_scmi.c
@@ -259,10 +259,7 @@ int css_scp_get_power_state(u_register_t mpidr, unsigned int power_level)
 	return HW_OFF;
 }
 
-/*
- * Helper function to shutdown the system via SCMI.
- */
-void __dead2 css_scp_sys_shutdown(void)
+void __dead2 css_scp_system_off(int state)
 {
 	int ret;
 
@@ -273,52 +270,37 @@ void __dead2 css_scp_sys_shutdown(void)
 	plat_arm_gic_cpuif_disable();
 
 	/*
-	 * Issue SCMI command for SYSTEM_SHUTDOWN. First issue a graceful
+	 * Issue SCMI command. First issue a graceful
 	 * request and if that fails force the request.
 	 */
 	ret = scmi_sys_pwr_state_set(scmi_handle,
 			SCMI_SYS_PWR_FORCEFUL_REQ,
-			SCMI_SYS_PWR_SHUTDOWN);
+			state);
+
 	if (ret != SCMI_E_SUCCESS) {
-		ERROR("SCMI system power domain shutdown return 0x%x unexpected\n",
-				ret);
+		ERROR("SCMI system power state set 0x%x returns unexpected 0x%x\n",
+			state, ret);
 		panic();
 	}
-
 	wfi();
-	ERROR("CSS System Shutdown: operation not handled.\n");
+	ERROR("CSS set power state: operation not handled.\n");
 	panic();
 }
 
 /*
+ * Helper function to shutdown the system via SCMI.
+ */
+void __dead2 css_scp_sys_shutdown(void)
+{
+	css_scp_system_off(SCMI_SYS_PWR_SHUTDOWN);
+}
+
+/*
  * Helper function to reset the system via SCMI.
  */
 void __dead2 css_scp_sys_reboot(void)
 {
-	int ret;
-
-	/*
-	 * Disable GIC CPU interface to prevent pending interrupt from waking
-	 * up the AP from WFI.
-	 */
-	plat_arm_gic_cpuif_disable();
-
-	/*
-	 * Issue SCMI command for SYSTEM_REBOOT. First issue a graceful
-	 * request and if that fails force the request.
-	 */
-	ret = scmi_sys_pwr_state_set(scmi_handle,
-			SCMI_SYS_PWR_FORCEFUL_REQ,
-			SCMI_SYS_PWR_COLD_RESET);
-	if (ret != SCMI_E_SUCCESS) {
-		ERROR("SCMI system power domain reset return 0x%x unexpected\n",
-				ret);
-		panic();
-	}
-
-	wfi();
-	ERROR("CSS System Reset: operation not handled.\n");
-	panic();
+	css_scp_system_off(SCMI_SYS_PWR_COLD_RESET);
 }
 
 scmi_channel_plat_info_t plat_css_scmi_plat_info = {
@@ -376,13 +358,35 @@ const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
 		ops->system_off = NULL;
 		ops->system_reset = NULL;
 		ops->get_sys_suspend_power_state = NULL;
-	} else if (!(msg_attr & SCMI_SYS_PWR_SUSPEND_SUPPORTED)) {
-		/*
-		 * System power management protocol is available, but it does
-		 * not support SYSTEM SUSPEND.
-		 */
-		ops->get_sys_suspend_power_state = NULL;
+	} else {
+		if (!(msg_attr & SCMI_SYS_PWR_SUSPEND_SUPPORTED)) {
+			/*
+			 * System power management protocol is available, but
+			 * it does not support SYSTEM SUSPEND.
+			 */
+			ops->get_sys_suspend_power_state = NULL;
+		}
+		if (!(msg_attr & SCMI_SYS_PWR_WARM_RESET_SUPPORTED)) {
+			/*
+			 * WARM reset is not available.
+			 */
+			ops->system_reset2 = NULL;
+		}
 	}
 
 	return ops;
 }
+
+int css_system_reset2(int is_vendor, int reset_type, u_register_t cookie)
+{
+	if (is_vendor || (reset_type != PSCI_RESET2_SYSTEM_WARM_RESET))
+		return PSCI_E_INVALID_PARAMS;
+
+	css_scp_system_off(SCMI_SYS_PWR_WARM_RESET);
+	/*
+	 * css_scp_system_off cannot return (it is a __dead function),
+	 * but css_system_reset2 has to return some value, even in
+	 * this case.
+	 */
+	return 0;
+}
diff --git a/plat/arm/css/drivers/scp/css_scp.h b/plat/arm/css/drivers/scp/css_scp.h
index 223e372a..1f0cf8e2 100644
--- a/plat/arm/css/drivers/scp/css_scp.h
+++ b/plat/arm/css/drivers/scp/css_scp.h
@@ -15,12 +15,14 @@
 struct psci_power_state;
 
 /* API for power management by SCP */
+int css_system_reset2(int is_vendor, int reset_type, u_register_t cookie);
 void css_scp_suspend(const struct psci_power_state *target_state);
 void css_scp_off(const struct psci_power_state *target_state);
 void css_scp_on(u_register_t mpidr);
 int css_scp_get_power_state(u_register_t mpidr, unsigned int power_level);
 void __dead2 css_scp_sys_shutdown(void);
 void __dead2 css_scp_sys_reboot(void);
+void __dead2 css_scp_system_off(int state);
 
 /* API for SCP Boot Image transfer. Return 0 on success, -1 on error */
 int css_scp_boot_image_xfer(void *image, unsigned int image_size);