forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2023-12-11 072de836f53be56a70cecf70b43ae43b7ce17376 
 kernel/arch/arm/mach-at91/pm.c
....@@ -1,13 +1,9 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
12 /*
23  * arch/arm/mach-at91/pm.c
34  * AT91 Power Management
45  *
56  * Copyright (C) 2005 David Brownell
6
- *
7
- * This program is free software; you can redistribute it and/or modify
8
- * it under the terms of the GNU General Public License as published by
9
- * the Free Software Foundation; either version 2 of the License, or
10
- * (at your option) any later version.
117  */
128 
139 #include <linux/genalloc.h>
....@@ -19,6 +15,7 @@
1915 #include <linux/suspend.h>
2016 
2117 #include <linux/clk/at91_pmc.h>
18
+#include <linux/platform_data/atmel.h>
2219 
2320 #include <asm/cacheflush.h>
2421 #include <asm/fncpy.h>
....@@ -39,24 +36,34 @@
3936 extern void at91_pinctrl_gpio_resume(void);
4037 #endif
4138 
39
+struct at91_soc_pm {
40
+	int (*config_shdwc_ws)(void __iomem *shdwc, u32 *mode, u32 *polarity);
41
+	int (*config_pmc_ws)(void __iomem *pmc, u32 mode, u32 polarity);
42
+	const struct of_device_id *ws_ids;
43
+	struct at91_pm_data data;
44
+};
45
+
46
+static struct at91_soc_pm soc_pm = {
47
+	.data = {
48
+		.standby_mode = AT91_PM_STANDBY,
49
+		.suspend_mode = AT91_PM_ULP0,
50
+	},
51
+};
52
+
4253 static const match_table_t pm_modes __initconst = {
43
-	{ AT91_PM_STANDBY, "standby" },
44
-	{ AT91_PM_ULP0, "ulp0" },
45
-	{ AT91_PM_ULP1, "ulp1" },
46
-	{ AT91_PM_BACKUP, "backup" },
54
+	{ AT91_PM_STANDBY,	"standby" },
55
+	{ AT91_PM_ULP0,		"ulp0" },
56
+	{ AT91_PM_ULP0_FAST,    "ulp0-fast" },
57
+	{ AT91_PM_ULP1,		"ulp1" },
58
+	{ AT91_PM_BACKUP,	"backup" },
4759 	{ -1, NULL },
4860 };
4961 
50
-static struct at91_pm_data pm_data = {
51
-	.standby_mode = AT91_PM_STANDBY,
52
-	.suspend_mode = AT91_PM_ULP0,
53
-};
54
-
5562 #define at91_ramc_read(id, field) \
56
-	__raw_readl(pm_data.ramc[id] + field)
63
+	__raw_readl(soc_pm.data.ramc[id] + field)
5764 
5865 #define at91_ramc_write(id, field, value) \
59
-	__raw_writel(value, pm_data.ramc[id] + field)
66
+	__raw_writel(value, soc_pm.data.ramc[id] + field)
6067 
6168 static int at91_pm_valid_state(suspend_state_t state)
6269 {
....@@ -91,17 +98,30 @@
9198 	{ .pmc_fsmr_bit = AT91_PMC_RTCAL,	.shdwc_mr_bit = BIT(17) },
9299 	{ .pmc_fsmr_bit = AT91_PMC_USBAL },
93100 	{ .pmc_fsmr_bit = AT91_PMC_SDMMC_CD },
101
+	{ .pmc_fsmr_bit = AT91_PMC_RTTAL },
102
+	{ .pmc_fsmr_bit = AT91_PMC_RXLP_MCE },
94103 };
95104 
96105 static const struct of_device_id sama5d2_ws_ids[] = {
97106 	{ .compatible = "atmel,sama5d2-gem",		.data = &ws_info[0] },
98
-	{ .compatible = "atmel,at91rm9200-rtc",		.data = &ws_info[1] },
107
+	{ .compatible = "atmel,sama5d2-rtc",		.data = &ws_info[1] },
99108 	{ .compatible = "atmel,sama5d3-udc",		.data = &ws_info[2] },
100109 	{ .compatible = "atmel,at91rm9200-ohci",	.data = &ws_info[2] },
101110 	{ .compatible = "usb-ohci",			.data = &ws_info[2] },
102111 	{ .compatible = "atmel,at91sam9g45-ehci",	.data = &ws_info[2] },
103112 	{ .compatible = "usb-ehci",			.data = &ws_info[2] },
104113 	{ .compatible = "atmel,sama5d2-sdhci",		.data = &ws_info[3] },
114
+	{ /* sentinel */ }
115
+};
116
+
117
+static const struct of_device_id sam9x60_ws_ids[] = {
118
+	{ .compatible = "microchip,sam9x60-rtc",	.data = &ws_info[1] },
119
+	{ .compatible = "atmel,at91rm9200-ohci",	.data = &ws_info[2] },
120
+	{ .compatible = "usb-ohci",			.data = &ws_info[2] },
121
+	{ .compatible = "atmel,at91sam9g45-ehci",	.data = &ws_info[2] },
122
+	{ .compatible = "usb-ehci",			.data = &ws_info[2] },
123
+	{ .compatible = "microchip,sam9x60-rtt",	.data = &ws_info[4] },
124
+	{ .compatible = "cdns,sam9x60-macb",		.data = &ws_info[5] },
105125 	{ /* sentinel */ }
106126 };
107127 
....@@ -116,24 +136,22 @@
116136 	if (pm_mode != AT91_PM_ULP1)
117137 		return 0;
118138 
119
-	if (!pm_data.pmc || !pm_data.shdwc)
139
+	if (!soc_pm.data.pmc || !soc_pm.data.shdwc || !soc_pm.ws_ids)
120140 		return -EPERM;
121141 
122142 	if (!set) {
123
-		writel(mode, pm_data.pmc + AT91_PMC_FSMR);
143
+		writel(mode, soc_pm.data.pmc + AT91_PMC_FSMR);
124144 		return 0;
125145 	}
126146 
127
-	/* SHDWC.WUIR */
128
-	val = readl(pm_data.shdwc + 0x0c);
129
-	mode |= (val & 0x3ff);
130
-	polarity |= ((val >> 16) & 0x3ff);
147
+	if (soc_pm.config_shdwc_ws)
148
+		soc_pm.config_shdwc_ws(soc_pm.data.shdwc, &mode, &polarity);
131149 
132150 	/* SHDWC.MR */
133
-	val = readl(pm_data.shdwc + 0x04);
151
+	val = readl(soc_pm.data.shdwc + 0x04);
134152 
135153 	/* Loop through defined wakeup sources. */
136
-	for_each_matching_node_and_match(np, sama5d2_ws_ids, &match) {
154
+	for_each_matching_node_and_match(np, soc_pm.ws_ids, &match) {
137155 		pdev = of_find_device_by_node(np);
138156 		if (!pdev)
139157 			continue;
....@@ -155,13 +173,41 @@
155173 	}
156174 
157175 	if (mode) {
158
-		writel(mode, pm_data.pmc + AT91_PMC_FSMR);
159
-		writel(polarity, pm_data.pmc + AT91_PMC_FSPR);
176
+		if (soc_pm.config_pmc_ws)
177
+			soc_pm.config_pmc_ws(soc_pm.data.pmc, mode, polarity);
160178 	} else {
161179 		pr_err("AT91: PM: no ULP1 wakeup sources found!");
162180 	}
163181 
164182 	return mode ? 0 : -EPERM;
183
+}
184
+
185
+static int at91_sama5d2_config_shdwc_ws(void __iomem *shdwc, u32 *mode,
186
+					u32 *polarity)
187
+{
188
+	u32 val;
189
+
190
+	/* SHDWC.WUIR */
191
+	val = readl(shdwc + 0x0c);
192
+	*mode |= (val & 0x3ff);
193
+	*polarity |= ((val >> 16) & 0x3ff);
194
+
195
+	return 0;
196
+}
197
+
198
+static int at91_sama5d2_config_pmc_ws(void __iomem *pmc, u32 mode, u32 polarity)
199
+{
200
+	writel(mode, pmc + AT91_PMC_FSMR);
201
+	writel(polarity, pmc + AT91_PMC_FSPR);
202
+
203
+	return 0;
204
+}
205
+
206
+static int at91_sam9x60_config_pmc_ws(void __iomem *pmc, u32 mode, u32 polarity)
207
+{
208
+	writel(mode, pmc + AT91_PMC_FSMR);
209
+
210
+	return 0;
165211 }
166212 
167213 /*
....@@ -171,18 +217,18 @@
171217 {
172218 	switch (state) {
173219 	case PM_SUSPEND_MEM:
174
-		pm_data.mode = pm_data.suspend_mode;
220
+		soc_pm.data.mode = soc_pm.data.suspend_mode;
175221 		break;
176222 
177223 	case PM_SUSPEND_STANDBY:
178
-		pm_data.mode = pm_data.standby_mode;
224
+		soc_pm.data.mode = soc_pm.data.standby_mode;
179225 		break;
180226 
181227 	default:
182
-		pm_data.mode = -1;
228
+		soc_pm.data.mode = -1;
183229 	}
184230 
185
-	return at91_pm_config_ws(pm_data.mode, true);
231
+	return at91_pm_config_ws(soc_pm.data.mode, true);
186232 }
187233 
188234 /*
....@@ -194,10 +240,10 @@
194240 	unsigned long scsr;
195241 	int i;
196242 
197
-	scsr = readl(pm_data.pmc + AT91_PMC_SCSR);
243
+	scsr = readl(soc_pm.data.pmc + AT91_PMC_SCSR);
198244 
199245 	/* USB must not be using PLLB */
200
-	if ((scsr & pm_data.uhp_udp_mask) != 0) {
246
+	if ((scsr & soc_pm.data.uhp_udp_mask) != 0) {
201247 		pr_err("AT91: PM - Suspend-to-RAM with USB still active\n");
202248 		return 0;
203249 	}
....@@ -208,7 +254,7 @@
208254 
209255 		if ((scsr & (AT91_PMC_PCK0 << i)) == 0)
210256 			continue;
211
-		css = readl(pm_data.pmc + AT91_PMC_PCKR(i)) & AT91_PMC_CSS;
257
+		css = readl(soc_pm.data.pmc + AT91_PMC_PCKR(i)) & AT91_PMC_CSS;
212258 		if (css != AT91_PMC_CSS_SLOW) {
213259 			pr_err("AT91: PM - Suspend-to-RAM with PCK%d src %d\n", i, css);
214260 			return 0;
....@@ -230,7 +276,7 @@
230276  */
231277 int at91_suspend_entering_slow_clock(void)
232278 {
233
-	return (pm_data.mode >= AT91_PM_ULP0);
279
+	return (soc_pm.data.mode >= AT91_PM_ULP0);
234280 }
235281 EXPORT_SYMBOL(at91_suspend_entering_slow_clock);
236282 
....@@ -243,14 +289,14 @@
243289 	flush_cache_all();
244290 	outer_disable();
245291 
246
-	at91_suspend_sram_fn(&pm_data);
292
+	at91_suspend_sram_fn(&soc_pm.data);
247293 
248294 	return 0;
249295 }
250296 
251297 static void at91_pm_suspend(suspend_state_t state)
252298 {
253
-	if (pm_data.mode == AT91_PM_BACKUP) {
299
+	if (soc_pm.data.mode == AT91_PM_BACKUP) {
254300 		pm_bu->suspended = 1;
255301 
256302 		cpu_suspend(0, at91_suspend_finish);
....@@ -289,7 +335,7 @@
289335 		/*
290336 		 * Ensure that clocks are in a valid state.
291337 		 */
292
-		if (pm_data.mode >= AT91_PM_ULP0 &&
338
+		if (soc_pm.data.mode >= AT91_PM_ULP0 &&
293339 		    !at91_pm_verify_clocks())
294340 			goto error;
295341 
....@@ -318,7 +364,7 @@
318364  */
319365 static void at91_pm_end(void)
320366 {
321
-	at91_pm_config_ws(pm_data.mode, false);
367
+	at91_pm_config_ws(soc_pm.data.mode, false);
322368 }
323369 
324370 
....@@ -351,7 +397,7 @@
351397 		"    str    %2, [%1, %3]\n\t"
352398 		"    mcr    p15, 0, %0, c7, c0, 4\n\t"
353399 		:
354
-		: "r" (0), "r" (pm_data.ramc[0]),
400
+		: "r" (0), "r" (soc_pm.data.ramc[0]),
355401 		  "r" (1), "r" (AT91_MC_SDRAMC_SRR));
356402 }
357403 
....@@ -374,7 +420,7 @@
374420 		at91_ramc_write(0, AT91_DDRSDRC_MDR, mdr);
375421 	}
376422 
377
-	if (pm_data.ramc[1]) {
423
+	if (soc_pm.data.ramc[1]) {
378424 		saved_lpr1 = at91_ramc_read(1, AT91_DDRSDRC_LPR);
379425 		lpr1 = saved_lpr1 & ~AT91_DDRSDRC_LPCB;
380426 		lpr1 |= AT91_DDRSDRC_LPCB_SELF_REFRESH;
....@@ -392,14 +438,14 @@
392438 
393439 	/* self-refresh mode now */
394440 	at91_ramc_write(0, AT91_DDRSDRC_LPR, lpr0);
395
-	if (pm_data.ramc[1])
441
+	if (soc_pm.data.ramc[1])
396442 		at91_ramc_write(1, AT91_DDRSDRC_LPR, lpr1);
397443 
398444 	cpu_do_idle();
399445 
400446 	at91_ramc_write(0, AT91_DDRSDRC_MDR, saved_mdr0);
401447 	at91_ramc_write(0, AT91_DDRSDRC_LPR, saved_lpr0);
402
-	if (pm_data.ramc[1]) {
448
+	if (soc_pm.data.ramc[1]) {
403449 		at91_ramc_write(0, AT91_DDRSDRC_MDR, saved_mdr1);
404450 		at91_ramc_write(1, AT91_DDRSDRC_LPR, saved_lpr1);
405451 	}
....@@ -429,7 +475,7 @@
429475 	u32 lpr0, lpr1 = 0;
430476 	u32 saved_lpr0, saved_lpr1 = 0;
431477 
432
-	if (pm_data.ramc[1]) {
478
+	if (soc_pm.data.ramc[1]) {
433479 		saved_lpr1 = at91_ramc_read(1, AT91_SDRAMC_LPR);
434480 		lpr1 = saved_lpr1 & ~AT91_SDRAMC_LPCB;
435481 		lpr1 |= AT91_SDRAMC_LPCB_SELF_REFRESH;
....@@ -441,13 +487,13 @@
441487 
442488 	/* self-refresh mode now */
443489 	at91_ramc_write(0, AT91_SDRAMC_LPR, lpr0);
444
-	if (pm_data.ramc[1])
490
+	if (soc_pm.data.ramc[1])
445491 		at91_ramc_write(1, AT91_SDRAMC_LPR, lpr1);
446492 
447493 	cpu_do_idle();
448494 
449495 	at91_ramc_write(0, AT91_SDRAMC_LPR, saved_lpr0);
450
-	if (pm_data.ramc[1])
496
+	if (soc_pm.data.ramc[1])
451497 		at91_ramc_write(1, AT91_SDRAMC_LPR, saved_lpr1);
452498 }
453499 
....@@ -471,36 +517,51 @@
471517 	{ /*sentinel*/ }
472518 };
473519 
474
-static __init void at91_dt_ramc(void)
520
+static __init int at91_dt_ramc(void)
475521 {
476522 	struct device_node *np;
477523 	const struct of_device_id *of_id;
478524 	int idx = 0;
479525 	void *standby = NULL;
480526 	const struct ramc_info *ramc;
527
+	int ret;
481528 
482529 	for_each_matching_node_and_match(np, ramc_ids, &of_id) {
483
-		pm_data.ramc[idx] = of_iomap(np, 0);
484
-		if (!pm_data.ramc[idx])
485
-			panic(pr_fmt("unable to map ramc[%d] cpu registers\n"), idx);
530
+		soc_pm.data.ramc[idx] = of_iomap(np, 0);
531
+		if (!soc_pm.data.ramc[idx]) {
532
+			pr_err("unable to map ramc[%d] cpu registers\n", idx);
533
+			ret = -ENOMEM;
534
+			goto unmap_ramc;
535
+		}
486536 
487537 		ramc = of_id->data;
488538 		if (!standby)
489539 			standby = ramc->idle;
490
-		pm_data.memctrl = ramc->memctrl;
540
+		soc_pm.data.memctrl = ramc->memctrl;
491541 
492542 		idx++;
493543 	}
494544 
495
-	if (!idx)
496
-		panic(pr_fmt("unable to find compatible ram controller node in dtb\n"));
545
+	if (!idx) {
546
+		pr_err("unable to find compatible ram controller node in dtb\n");
547
+		ret = -ENODEV;
548
+		goto unmap_ramc;
549
+	}
497550 
498551 	if (!standby) {
499552 		pr_warn("ramc no standby function available\n");
500
-		return;
553
+		return 0;
501554 	}
502555 
503556 	at91_cpuidle_device.dev.platform_data = standby;
557
+
558
+	return 0;
559
+
560
+unmap_ramc:
561
+	while (idx)
562
+		iounmap(soc_pm.data.ramc[--idx]);
563
+
564
+	return ret;
504565 }
505566 
506567 static void at91rm9200_idle(void)
....@@ -509,12 +570,12 @@
509570 	 * Disable the processor clock.  The processor will be automatically
510571 	 * re-enabled by an interrupt or by a reset.
511572 	 */
512
-	writel(AT91_PMC_PCK, pm_data.pmc + AT91_PMC_SCDR);
573
+	writel(AT91_PMC_PCK, soc_pm.data.pmc + AT91_PMC_SCDR);
513574 }
514575 
515576 static void at91sam9_idle(void)
516577 {
517
-	writel(AT91_PMC_PCK, pm_data.pmc + AT91_PMC_SCDR);
578
+	writel(AT91_PMC_PCK, soc_pm.data.pmc + AT91_PMC_SCDR);
518579 	cpu_do_idle();
519580 }
520581 
....@@ -571,8 +632,8 @@
571632 
572633 static bool __init at91_is_pm_mode_active(int pm_mode)
573634 {
574
-	return (pm_data.standby_mode == pm_mode ||
575
-		pm_data.suspend_mode == pm_mode);
635
+	return (soc_pm.data.standby_mode == pm_mode ||
636
+		soc_pm.data.suspend_mode == pm_mode);
576637 }
577638 
578639 static int __init at91_pm_backup_init(void)
....@@ -582,10 +643,11 @@
582643 	struct platform_device *pdev = NULL;
583644 	int ret = -ENODEV;
584645 
646
+	if (!IS_ENABLED(CONFIG_SOC_SAMA5D2))
647
+		return -EPERM;
648
+
585649 	if (!at91_is_pm_mode_active(AT91_PM_BACKUP))
586650 		return 0;
587
-
588
-	pm_bu = NULL;
589651 
590652 	np = of_find_compatible_node(NULL, NULL, "atmel,sama5d2-sfrbu");
591653 	if (!np) {
....@@ -593,9 +655,8 @@
593655 		return ret;
594656 	}
595657 
596
-	pm_data.sfrbu = of_iomap(np, 0);
658
+	soc_pm.data.sfrbu = of_iomap(np, 0);
597659 	of_node_put(np);
598
-	pm_bu = NULL;
599660 
600661 	np = of_find_compatible_node(NULL, NULL, "atmel,sama5d2-securam");
601662 	if (!np)
....@@ -630,8 +691,8 @@
630691 securam_fail:
631692 	put_device(&pdev->dev);
632693 securam_fail_no_ref_dev:
633
-	iounmap(pm_data.sfrbu);
634
-	pm_data.sfrbu = NULL;
694
+	iounmap(soc_pm.data.sfrbu);
695
+	soc_pm.data.sfrbu = NULL;
635696 	return ret;
636697 }
637698 
....@@ -640,11 +701,17 @@
640701 	if (pm_mode != AT91_PM_ULP1 && pm_mode != AT91_PM_BACKUP)
641702 		return;
642703 
643
-	if (pm_data.standby_mode == pm_mode)
644
-		pm_data.standby_mode = AT91_PM_ULP0;
645
-	if (pm_data.suspend_mode == pm_mode)
646
-		pm_data.suspend_mode = AT91_PM_ULP0;
704
+	if (soc_pm.data.standby_mode == pm_mode)
705
+		soc_pm.data.standby_mode = AT91_PM_ULP0;
706
+	if (soc_pm.data.suspend_mode == pm_mode)
707
+		soc_pm.data.suspend_mode = AT91_PM_ULP0;
647708 }
709
+
710
+static const struct of_device_id atmel_shdwc_ids[] = {
711
+	{ .compatible = "atmel,sama5d2-shdwc" },
712
+	{ .compatible = "microchip,sam9x60-shdwc" },
713
+	{ /* sentinel. */ }
714
+};
648715 
649716 static void __init at91_pm_modes_init(void)
650717 {
....@@ -655,13 +722,13 @@
655722 	    !at91_is_pm_mode_active(AT91_PM_ULP1))
656723 		return;
657724 
658
-	np = of_find_compatible_node(NULL, NULL, "atmel,sama5d2-shdwc");
725
+	np = of_find_matching_node(NULL, atmel_shdwc_ids);
659726 	if (!np) {
660727 		pr_warn("%s: failed to find shdwc!\n", __func__);
661728 		goto ulp1_default;
662729 	}
663730 
664
-	pm_data.shdwc = of_iomap(np, 0);
731
+	soc_pm.data.shdwc = of_iomap(np, 0);
665732 	of_node_put(np);
666733 
667734 	ret = at91_pm_backup_init();
....@@ -675,8 +742,8 @@
675742 	return;
676743 
677744 unmap:
678
-	iounmap(pm_data.shdwc);
679
-	pm_data.shdwc = NULL;
745
+	iounmap(soc_pm.data.shdwc);
746
+	soc_pm.data.shdwc = NULL;
680747 ulp1_default:
681748 	at91_pm_use_default_mode(AT91_PM_ULP1);
682749 backup_default:
....@@ -685,13 +752,36 @@
685752 
686753 struct pmc_info {
687754 	unsigned long uhp_udp_mask;
755
+	unsigned long mckr;
756
+	unsigned long version;
688757 };
689758 
690759 static const struct pmc_info pmc_infos[] __initconst = {
691
-	{ .uhp_udp_mask = AT91RM9200_PMC_UHP | AT91RM9200_PMC_UDP },
692
-	{ .uhp_udp_mask = AT91SAM926x_PMC_UHP | AT91SAM926x_PMC_UDP },
693
-	{ .uhp_udp_mask = AT91SAM926x_PMC_UHP },
694
-	{ .uhp_udp_mask = 0 },
760
+	{
761
+		.uhp_udp_mask = AT91RM9200_PMC_UHP | AT91RM9200_PMC_UDP,
762
+		.mckr = 0x30,
763
+		.version = AT91_PMC_V1,
764
+	},
765
+
766
+	{
767
+		.uhp_udp_mask = AT91SAM926x_PMC_UHP | AT91SAM926x_PMC_UDP,
768
+		.mckr = 0x30,
769
+		.version = AT91_PMC_V1,
770
+	},
771
+	{
772
+		.uhp_udp_mask = AT91SAM926x_PMC_UHP,
773
+		.mckr = 0x30,
774
+		.version = AT91_PMC_V1,
775
+	},
776
+	{	.uhp_udp_mask = 0,
777
+		.mckr = 0x30,
778
+		.version = AT91_PMC_V1,
779
+	},
780
+	{
781
+		.uhp_udp_mask = AT91SAM926x_PMC_UHP | AT91SAM926x_PMC_UDP,
782
+		.mckr = 0x28,
783
+		.version = AT91_PMC_V2,
784
+	},
695785 };
696786 
697787 static const struct of_device_id atmel_pmc_ids[] __initconst = {
....@@ -706,8 +796,54 @@
706796 	{ .compatible = "atmel,sama5d3-pmc", .data = &pmc_infos[1] },
707797 	{ .compatible = "atmel,sama5d4-pmc", .data = &pmc_infos[1] },
708798 	{ .compatible = "atmel,sama5d2-pmc", .data = &pmc_infos[1] },
799
+	{ .compatible = "microchip,sam9x60-pmc", .data = &pmc_infos[4] },
709800 	{ /* sentinel */ },
710801 };
802
+
803
+static void __init at91_pm_modes_validate(const int *modes, int len)
804
+{
805
+	u8 i, standby = 0, suspend = 0;
806
+	int mode;
807
+
808
+	for (i = 0; i < len; i++) {
809
+		if (standby && suspend)
810
+			break;
811
+
812
+		if (modes[i] == soc_pm.data.standby_mode && !standby) {
813
+			standby = 1;
814
+			continue;
815
+		}
816
+
817
+		if (modes[i] == soc_pm.data.suspend_mode && !suspend) {
818
+			suspend = 1;
819
+			continue;
820
+		}
821
+	}
822
+
823
+	if (!standby) {
824
+		if (soc_pm.data.suspend_mode == AT91_PM_STANDBY)
825
+			mode = AT91_PM_ULP0;
826
+		else
827
+			mode = AT91_PM_STANDBY;
828
+
829
+		pr_warn("AT91: PM: %s mode not supported! Using %s.\n",
830
+			pm_modes[soc_pm.data.standby_mode].pattern,
831
+			pm_modes[mode].pattern);
832
+		soc_pm.data.standby_mode = mode;
833
+	}
834
+
835
+	if (!suspend) {
836
+		if (soc_pm.data.standby_mode == AT91_PM_ULP0)
837
+			mode = AT91_PM_STANDBY;
838
+		else
839
+			mode = AT91_PM_ULP0;
840
+
841
+		pr_warn("AT91: PM: %s mode not supported! Using %s.\n",
842
+			pm_modes[soc_pm.data.suspend_mode].pattern,
843
+			pm_modes[mode].pattern);
844
+		soc_pm.data.suspend_mode = mode;
845
+	}
846
+}
711847 
712848 static void __init at91_pm_init(void (*pm_idle)(void))
713849 {
....@@ -719,14 +855,17 @@
719855 		platform_device_register(&at91_cpuidle_device);
720856 
721857 	pmc_np = of_find_matching_node_and_match(NULL, atmel_pmc_ids, &of_id);
722
-	pm_data.pmc = of_iomap(pmc_np, 0);
723
-	if (!pm_data.pmc) {
858
+	soc_pm.data.pmc = of_iomap(pmc_np, 0);
859
+	of_node_put(pmc_np);
860
+	if (!soc_pm.data.pmc) {
724861 		pr_err("AT91: PM not supported, PMC not found\n");
725862 		return;
726863 	}
727864 
728865 	pmc = of_id->data;
729
-	pm_data.uhp_udp_mask = pmc->uhp_udp_mask;
866
+	soc_pm.data.uhp_udp_mask = pmc->uhp_udp_mask;
867
+	soc_pm.data.pmc_mckr_offset = pmc->mckr;
868
+	soc_pm.data.pmc_version = pmc->version;
730869 
731870 	if (pm_idle)
732871 		arm_pm_idle = pm_idle;
....@@ -736,8 +875,8 @@
736875 	if (at91_suspend_sram_fn) {
737876 		suspend_set_ops(&at91_pm_ops);
738877 		pr_info("AT91: PM: standby: %s, suspend: %s\n",
739
-			pm_modes[pm_data.standby_mode].pattern,
740
-			pm_modes[pm_data.suspend_mode].pattern);
878
+			pm_modes[soc_pm.data.standby_mode].pattern,
879
+			pm_modes[soc_pm.data.suspend_mode].pattern);
741880 	} else {
742881 		pr_info("AT91: PM not supported, due to no SRAM allocated\n");
743882 	}
....@@ -745,10 +884,22 @@
745884 
746885 void __init at91rm9200_pm_init(void)
747886 {
887
+	int ret;
888
+
748889 	if (!IS_ENABLED(CONFIG_SOC_AT91RM9200))
749890 		return;
750891 
751
-	at91_dt_ramc();
892
+	/*
893
+	 * Force STANDBY and ULP0 mode to avoid calling
894
+	 * at91_pm_modes_validate() which may increase booting time.
895
+	 * Platform supports anyway only STANDBY and ULP0 modes.
896
+	 */
897
+	soc_pm.data.standby_mode = AT91_PM_STANDBY;
898
+	soc_pm.data.suspend_mode = AT91_PM_ULP0;
899
+
900
+	ret = at91_dt_ramc();
901
+	if (ret)
902
+		return;
752903 
753904 	/*
754905 	 * AT91RM9200 SDRAM low-power mode cannot be used with self-refresh.
....@@ -758,31 +909,90 @@
758909 	at91_pm_init(at91rm9200_idle);
759910 }
760911 
912
+void __init sam9x60_pm_init(void)
913
+{
914
+	static const int modes[] __initconst = {
915
+		AT91_PM_STANDBY, AT91_PM_ULP0, AT91_PM_ULP0_FAST, AT91_PM_ULP1,
916
+	};
917
+	int ret;
918
+
919
+	if (!IS_ENABLED(CONFIG_SOC_SAM9X60))
920
+		return;
921
+
922
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
923
+	at91_pm_modes_init();
924
+	ret = at91_dt_ramc();
925
+	if (ret)
926
+		return;
927
+
928
+	at91_pm_init(NULL);
929
+
930
+	soc_pm.ws_ids = sam9x60_ws_ids;
931
+	soc_pm.config_pmc_ws = at91_sam9x60_config_pmc_ws;
932
+}
933
+
761934 void __init at91sam9_pm_init(void)
762935 {
936
+	int ret;
937
+
763938 	if (!IS_ENABLED(CONFIG_SOC_AT91SAM9))
764939 		return;
765940 
766
-	at91_dt_ramc();
941
+	/*
942
+	 * Force STANDBY and ULP0 mode to avoid calling
943
+	 * at91_pm_modes_validate() which may increase booting time.
944
+	 * Platform supports anyway only STANDBY and ULP0 modes.
945
+	 */
946
+	soc_pm.data.standby_mode = AT91_PM_STANDBY;
947
+	soc_pm.data.suspend_mode = AT91_PM_ULP0;
948
+
949
+	ret = at91_dt_ramc();
950
+	if (ret)
951
+		return;
952
+
767953 	at91_pm_init(at91sam9_idle);
768954 }
769955 
770956 void __init sama5_pm_init(void)
771957 {
958
+	static const int modes[] __initconst = {
959
+		AT91_PM_STANDBY, AT91_PM_ULP0, AT91_PM_ULP0_FAST,
960
+	};
961
+	int ret;
962
+
772963 	if (!IS_ENABLED(CONFIG_SOC_SAMA5))
773964 		return;
774965 
775
-	at91_dt_ramc();
966
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
967
+	ret = at91_dt_ramc();
968
+	if (ret)
969
+		return;
970
+
776971 	at91_pm_init(NULL);
777972 }
778973 
779974 void __init sama5d2_pm_init(void)
780975 {
976
+	static const int modes[] __initconst = {
977
+		AT91_PM_STANDBY, AT91_PM_ULP0, AT91_PM_ULP0_FAST, AT91_PM_ULP1,
978
+		AT91_PM_BACKUP,
979
+	};
980
+	int ret;
981
+
781982 	if (!IS_ENABLED(CONFIG_SOC_SAMA5D2))
782983 		return;
783984 
985
+	at91_pm_modes_validate(modes, ARRAY_SIZE(modes));
784986 	at91_pm_modes_init();
785
-	sama5_pm_init();
987
+	ret = at91_dt_ramc();
988
+	if (ret)
989
+		return;
990
+
991
+	at91_pm_init(NULL);
992
+
993
+	soc_pm.ws_ids = sama5d2_ws_ids;
994
+	soc_pm.config_shdwc_ws = at91_sama5d2_config_shdwc_ws;
995
+	soc_pm.config_pmc_ws = at91_sama5d2_config_pmc_ws;
786996 }
787997 
788998 static int __init at91_pm_modes_select(char *str)
....@@ -803,8 +1013,8 @@
8031013 	if (suspend < 0)
8041014 		return 0;
8051015 
806
-	pm_data.standby_mode = standby;
807
-	pm_data.suspend_mode = suspend;
1016
+	soc_pm.data.standby_mode = standby;
1017
+	soc_pm.data.suspend_mode = suspend;
8081018 
8091019 	return 0;
8101020 }