3 files changed, 76 insertions, 3 deletions

diff --git a/arch/arm/mach-at91/include/mach/gpio.h b/arch/arm/mach-at91/include/mach/gpio.h
index eed465ab0dd7..5fc23771c154 100644
--- a/arch/arm/mach-at91/include/mach/gpio.h
+++ b/arch/arm/mach-at91/include/mach/gpio.h
@@ -209,6 +209,14 @@ extern int at91_get_gpio_value(unsigned pin);
 extern void at91_gpio_suspend(void);
 extern void at91_gpio_resume(void);
 
+#ifdef CONFIG_PINCTRL_AT91
+extern void at91_pinctrl_gpio_suspend(void);
+extern void at91_pinctrl_gpio_resume(void);
+#else
+static inline void at91_pinctrl_gpio_suspend(void) {}
+static inline void at91_pinctrl_gpio_resume(void) {}
+#endif
+
 #endif  /* __ASSEMBLY__ */
 
 #endif
diff --git a/arch/arm/mach-at91/pm.c b/arch/arm/mach-at91/pm.c
index adb6db888a1f..73f1f250403a 100644
--- a/arch/arm/mach-at91/pm.c
+++ b/arch/arm/mach-at91/pm.c
@@ -201,7 +201,10 @@ extern u32 at91_slow_clock_sz;
 
 static int at91_pm_enter(suspend_state_t state)
 {
-        at91_gpio_suspend();
+        if (of_have_populated_dt())
+                at91_pinctrl_gpio_suspend();
+        else
+                at91_gpio_suspend();
         at91_irq_suspend();
 
         pr_debug("AT91: PM - wake mask %08x, pm state %d\n",
@@ -286,7 +289,10 @@ static int at91_pm_enter(suspend_state_t state)
 error:
         target_state = PM_SUSPEND_ON;
         at91_irq_resume();
-        at91_gpio_resume();
+        if (of_have_populated_dt())
+                at91_pinctrl_gpio_resume();
+        else
+                at91_gpio_resume();
         return 0;
 }
 
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 75933a6aa828..efb7f10e902a 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -1277,21 +1277,80 @@ static int alt_gpio_irq_type(struct irq_data *d, unsigned type)
 }
 
 #ifdef CONFIG_PM
+
+static u32 wakeups[MAX_GPIO_BANKS];
+static u32 backups[MAX_GPIO_BANKS];
+
 static int gpio_irq_set_wake(struct irq_data *d, unsigned state)
 {
         struct at91_gpio_chip *at91_gpio = irq_data_get_irq_chip_data(d);
         unsigned        bank = at91_gpio->pioc_idx;
+        unsigned mask = 1 << d->hwirq;
 
         if (unlikely(bank >= MAX_GPIO_BANKS))
                 return -EINVAL;
 
+        if (state)
+                wakeups[bank] |= mask;
+        else
+                wakeups[bank] &= ~mask;
+
         irq_set_irq_wake(at91_gpio->pioc_virq, state);
 
         return 0;
 }
+
+void at91_pinctrl_gpio_suspend(void)
+{
+        int i;
+
+        for (i = 0; i < gpio_banks; i++) {
+                void __iomem  *pio;
+
+                if (!gpio_chips[i])
+                        continue;
+
+                pio = gpio_chips[i]->regbase;
+
+                backups[i] = __raw_readl(pio + PIO_IMR);
+                __raw_writel(backups[i], pio + PIO_IDR);
+                __raw_writel(wakeups[i], pio + PIO_IER);
+
+                if (!wakeups[i]) {
+                        clk_unprepare(gpio_chips[i]->clock);
+                        clk_disable(gpio_chips[i]->clock);
+                } else {
+                        printk(KERN_DEBUG "GPIO-%c may wake for %08x\n",
+                               'A'+i, wakeups[i]);
+                }
+        }
+}
+
+void at91_pinctrl_gpio_resume(void)
+{
+        int i;
+
+        for (i = 0; i < gpio_banks; i++) {
+                void __iomem  *pio;
+
+                if (!gpio_chips[i])
+                        continue;
+
+                pio = gpio_chips[i]->regbase;
+
+                if (!wakeups[i]) {
+                        if (clk_prepare(gpio_chips[i]->clock) == 0)
+                                clk_enable(gpio_chips[i]->clock);
+                }
+
+                __raw_writel(wakeups[i], pio + PIO_IDR);
+                __raw_writel(backups[i], pio + PIO_IER);
+        }
+}
+
 #else
 #define gpio_irq_set_wake       NULL
-#endif
+#endif /* CONFIG_PM */
 
 static struct irq_chip gpio_irqchip = {
         .name           = "GPIO",