Merge tag 'dlm-3.5' of git://git.kernel.org/pub/scm/linux/kernel/git/teigland/linux-dlm

[firefly-linux-kernel-4.4.55.git] / arch / arm / mach-pxa / mfp-pxa2xx.c

diff --git a/arch/arm/mach-pxa/mfp-pxa2xx.c b/arch/arm/mach-pxa/mfp-pxa2xx.c
index b0a842887780f9acbe32e4223b4fe769f49b88f1..ef0426a159d4d4e04c681f7097fb8732d7096156 100644 (file)
--- a/arch/arm/mach-pxa/mfp-pxa2xx.c
+++ b/arch/arm/mach-pxa/mfp-pxa2xx.c
@@ -33,6 +33,8 @@
 #define BANK_OFF(n)    (((n) < 3) ? (n) << 2 : 0x100 + (((n) - 3) << 2))
 #define GPLR(x)                __REG2(0x40E00000, BANK_OFF((x) >> 5))
 #define GPDR(x)                __REG2(0x40E00000, BANK_OFF((x) >> 5) + 0x0c)
+#define GPSR(x)                __REG2(0x40E00000, BANK_OFF((x) >> 5) + 0x18)
+#define GPCR(x)                __REG2(0x40E00000, BANK_OFF((x) >> 5) + 0x24)
 
 #define PWER_WE35      (1 << 24)
 
@@ -348,6 +350,7 @@ static inline void pxa27x_mfp_init(void) {}
 #ifdef CONFIG_PM
 static unsigned long saved_gafr[2][4];
 static unsigned long saved_gpdr[4];
+static unsigned long saved_gplr[4];
 static unsigned long saved_pgsr[4];
 
 static int pxa2xx_mfp_suspend(void)
@@ -366,14 +369,26 @@ static int pxa2xx_mfp_suspend(void)
        }
 
        for (i = 0; i <= gpio_to_bank(pxa_last_gpio); i++) {
-
                saved_gafr[0][i] = GAFR_L(i);
                saved_gafr[1][i] = GAFR_U(i);
                saved_gpdr[i] = GPDR(i * 32);
+               saved_gplr[i] = GPLR(i * 32);
                saved_pgsr[i] = PGSR(i);
 
-               GPDR(i * 32) = gpdr_lpm[i];
+               GPSR(i * 32) = PGSR(i);
+               GPCR(i * 32) = ~PGSR(i);
+       }
+
+       /* set GPDR bits taking into account MFP_LPM_KEEP_OUTPUT */
+       for (i = 0; i < pxa_last_gpio; i++) {
+               if ((gpdr_lpm[gpio_to_bank(i)] & GPIO_bit(i)) ||
+                   ((gpio_desc[i].config & MFP_LPM_KEEP_OUTPUT) &&
+                    (saved_gpdr[gpio_to_bank(i)] & GPIO_bit(i))))
+                       GPDR(i) |= GPIO_bit(i);
+               else
+                       GPDR(i) &= ~GPIO_bit(i);
        }
+
        return 0;
 }
 
@@ -384,6 +399,8 @@ static void pxa2xx_mfp_resume(void)
        for (i = 0; i <= gpio_to_bank(pxa_last_gpio); i++) {
                GAFR_L(i) = saved_gafr[0][i];
                GAFR_U(i) = saved_gafr[1][i];
+               GPSR(i * 32) = saved_gplr[i];
+               GPCR(i * 32) = ~saved_gplr[i];
                GPDR(i * 32) = saved_gpdr[i];
                PGSR(i) = saved_pgsr[i];
        }