[PATCH v2 2/3] ARM: at91: pm: add per soc validation of pm modes

[Claudiu Beznea]

Not all SoCs supports all the PM mode. User may end up settings,
e.g. backup mode, on a non SAMA5D2 device, but the mode to not be valid.
If backup mode is used on a devices not supporting it there will be no
way of resuming other than rebooting.

Signed-off-by: Claudiu Beznea <claudiu.beznea at microchip.com>
---
 arch/arm/mach-at91/pm.c | 83 +++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 81 insertions(+), 2 deletions(-)

diff --git a/arch/arm/mach-at91/pm.c b/arch/arm/mach-at91/pm.c
index 04fdcbd57100..51902c4c9bb4 100644
--- a/arch/arm/mach-at91/pm.c
+++ b/arch/arm/mach-at91/pm.c
@@ -785,6 +785,51 @@ static const struct of_device_id atmel_pmc_ids[] __initconst = {
 	{ /* sentinel */ },
 };
 
+static void __init at91_pm_modes_validate(const int *modes, int len)
+{
+	u8 i, standby = 0, suspend = 0;
+	int mode;
+
+	for (i = 0; i < len; i++) {
+		if (standby && suspend)
+			break;
+
+		if (modes[i] == soc_pm.data.standby_mode && !standby) {
+			standby = 1;
+			continue;
+		}
+
+		if (modes[i] == soc_pm.data.suspend_mode && !suspend) {
+			suspend = 1;
+			continue;
+		}
+	}
+
+	if (!standby) {
+		if (soc_pm.data.suspend_mode == AT91_PM_STANDBY)
+			mode = AT91_PM_ULP0;
+		else
+			mode = AT91_PM_STANDBY;
+
+		pr_warn("AT91: PM: %s mode not supported! Using %s.\n",
+			pm_modes[soc_pm.data.standby_mode].pattern,
+			pm_modes[mode].pattern);
+		soc_pm.data.standby_mode = mode;
+	}
+
+	if (!suspend) {
+		if (soc_pm.data.standby_mode == AT91_PM_ULP0)
+			mode = AT91_PM_STANDBY;
+		else
+			mode = AT91_PM_ULP0;
+
+		pr_warn("AT91: PM: %s mode not supported! Using %s.\n",
+			pm_modes[soc_pm.data.suspend_mode].pattern,
+			pm_modes[mode].pattern);
+		soc_pm.data.suspend_mode = mode;
+	}
+}
+
 static void __init at91_pm_init(void (*pm_idle)(void))
 {
 	struct device_node *pmc_np;
@@ -823,9 +868,14 @@ static void __init at91_pm_init(void (*pm_idle)(void))
 
 void __init at91rm9200_pm_init(void)
 {
+	static const int modes[] __initconst = {
+		AT91_PM_STANDBY, AT91_PM_ULP0
+	};
+
 	if (!IS_ENABLED(CONFIG_SOC_AT91RM9200))
 		return;
 
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
 	at91_dt_ramc();
 
 	/*
@@ -838,9 +888,14 @@ void __init at91rm9200_pm_init(void)
 
 void __init sam9x60_pm_init(void)
 {
+	static const int modes[] __initconst = {
+		AT91_PM_STANDBY, AT91_PM_ULP0, AT91_PM_ULP0_FAST, AT91_PM_ULP1,
+	};
+
 	if (!IS_ENABLED(CONFIG_SOC_SAM9X60))
 		return;
 
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
 	at91_pm_modes_init();
 	at91_dt_ramc();
 	at91_pm_init(at91sam9x60_idle);
@@ -851,14 +906,19 @@ void __init sam9x60_pm_init(void)
 
 void __init at91sam9_pm_init(void)
 {
+	static const int modes[] __initconst = {
+		AT91_PM_STANDBY, AT91_PM_ULP0,
+	};
+
 	if (!IS_ENABLED(CONFIG_SOC_AT91SAM9))
 		return;
 
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
 	at91_dt_ramc();
 	at91_pm_init(at91sam9_idle);
 }
 
-void __init sama5_pm_init(void)
+static void __init sama5_pm(void)
 {
 	if (!IS_ENABLED(CONFIG_SOC_SAMA5))
 		return;
@@ -867,13 +927,32 @@ void __init sama5_pm_init(void)
 	at91_pm_init(NULL);
 }
 
+void __init sama5_pm_init(void)
+{
+	static const int modes[] __initconst = {
+		AT91_PM_STANDBY, AT91_PM_ULP0, AT91_PM_ULP0_FAST,
+	};
+
+	if (!IS_ENABLED(CONFIG_SOC_SAMA5))
+		return;
+
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
+	sama5_pm();
+}
+
 void __init sama5d2_pm_init(void)
 {
+	static const int modes[] __initconst = {
+		AT91_PM_STANDBY, AT91_PM_ULP0, AT91_PM_ULP0_FAST, AT91_PM_ULP1,
+		AT91_PM_BACKUP,
+	};
+
 	if (!IS_ENABLED(CONFIG_SOC_SAMA5D2))
 		return;
 
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
 	at91_pm_modes_init();
-	sama5_pm_init();
+	sama5_pm();
 
 	soc_pm.ws_ids = sama5d2_ws_ids;
 	soc_pm.config_shdwc_ws = at91_sama5d2_config_shdwc_ws;
-- 
2.7.4