ARM: at91: pm: add support for sama5d2 secure suspend

When running with OP-TEE, the suspend control is handled securely.
Suspend can be entered using PSCI support. Since the sama5d2 supports
multiple suspend modes, add a new CONFIG_ATMEL_SECURE_PM which will
send a SMC call to select the suspend mode at init time.

"atmel.pm_modes" boot argument is still supported for compatibility
purposes but the standby value is actually ignored since PSCI suspend
is used and it only support one mode (suspend).

Signed-off-by: Clément Léger <clement.leger@bootlin.com>
Signed-off-by: Claudiu Beznea <claudiu.beznea@microchip.com>
Signed-off-by: Nicolas Ferre <nicolas.ferre@microchip.com>

arch/arm/mach-at91/Kconfig

@@ -218,7 +218,16 @@ config SOC_SAMA5
 	select SRAM if PM
 
 config ATMEL_PM
-	bool
+	bool "Atmel PM support"
+
+config ATMEL_SECURE_PM
+	bool "Atmel Secure PM support"
+	depends on SOC_SAMA5D2 && ATMEL_PM
+	select ARM_PSCI
+	help
+	  When running under a TEE, the suspend mode must be requested to be set
+	  at TEE level. When enable, this option will use secure monitor calls
+	  to set the suspend level. PSCI is then used to enter suspend.
 
 config SOC_SAMA7
 	bool

arch/arm/mach-at91/pm.c

@@ -28,6 +28,7 @@
 
 #include "generic.h"
 #include "pm.h"
+#include "sam_secure.h"
 
 #define BACKUP_DDR_PHY_CALIBRATION	(9)
 
@@ -1078,6 +1079,35 @@ securam_fail:
 	return ret;
 }
 
+static void at91_pm_secure_init(void)
+{
+	int suspend_mode;
+	struct arm_smccc_res res;
+
+	suspend_mode = soc_pm.data.suspend_mode;
+
+	res = sam_smccc_call(SAMA5_SMC_SIP_SET_SUSPEND_MODE,
+			     suspend_mode, 0);
+	if (res.a0 == 0) {
+		pr_info("AT91: Secure PM: suspend mode set to %s\n",
+			pm_modes[suspend_mode].pattern);
+		return;
+	}
+
+	pr_warn("AT91: Secure PM: %s mode not supported !\n",
+		pm_modes[suspend_mode].pattern);
+
+	res = sam_smccc_call(SAMA5_SMC_SIP_GET_SUSPEND_MODE, 0, 0);
+	if (res.a0 == 0) {
+		pr_warn("AT91: Secure PM: failed to get default mode\n");
+		return;
+	}
+
+	pr_info("AT91: Secure PM: using default suspend mode %s\n",
+		pm_modes[suspend_mode].pattern);
+
+	soc_pm.data.suspend_mode = res.a1;
+}
 static const struct of_device_id atmel_shdwc_ids[] = {
 	{ .compatible = "atmel,sama5d2-shdwc" },
 	{ .compatible = "microchip,sam9x60-shdwc" },
@@ -1526,6 +1556,12 @@ void __init sama5d2_pm_init(void)
 	if (!IS_ENABLED(CONFIG_SOC_SAMA5D2))
 		return;
 
+	if (IS_ENABLED(CONFIG_ATMEL_SECURE_PM)) {
+		pr_warn("AT91: Secure PM: ignoring standby mode\n");
+		at91_pm_secure_init();
+		return;
+	}
+
 	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
 	at91_pm_modes_init(iomaps, ARRAY_SIZE(iomaps));
 	ret = at91_dt_ramc(false);

arch/arm/mach-at91/sam_secure.h

@@ -8,6 +8,10 @@
 
 #include <linux/arm-smccc.h>
 
+/* Secure Monitor mode APIs */
+#define SAMA5_SMC_SIP_SET_SUSPEND_MODE	0x400
+#define SAMA5_SMC_SIP_GET_SUSPEND_MODE	0x401
+
 void __init sam_secure_init(void);
 struct arm_smccc_res sam_smccc_call(u32 fn, u32 arg0, u32 arg1);