1 files changed, 34 insertions, 9 deletions

diff --git a/arch/arm/mach-pxa/mfp-pxa2xx.c b/arch/arm/mach-pxa/mfp-pxa2xx.c
index cf6b720c055..1d1419b7345 100644
--- a/arch/arm/mach-pxa/mfp-pxa2xx.c
+++ b/arch/arm/mach-pxa/mfp-pxa2xx.c
@@ -81,6 +81,7 @@ static int __mfp_config_gpio(unsigned gpio, unsigned long c)
 		PGSR(bank) &= ~mask;
 		is_out = 1;
 		break;
+	case MFP_LPM_INPUT:
 	case MFP_LPM_DEFAULT:
 		break;
 	default:
@@ -178,8 +179,17 @@ int gpio_set_wake(unsigned int gpio, unsigned int on)
 	if (!d->valid)
 		return -EINVAL;
 
-	if (d->keypad_gpio)
-		return -EINVAL;
+	/* Allow keypad GPIOs to wakeup system when
+	 * configured as generic GPIOs.
+	 */
+	if (d->keypad_gpio && (MFP_AF(d->config) == 0) &&
+	    (d->config & MFP_LPM_CAN_WAKEUP)) {
+		if (on)
+			PKWR |= d->mask;
+		else
+			PKWR &= ~d->mask;
+		return 0;
+	}
 
 	mux_taken = (PWER & d->mux_mask) & (~d->mask);
 	if (on && mux_taken)
@@ -239,21 +249,25 @@ static int pxa27x_pkwr_gpio[] = {
 int keypad_set_wake(unsigned int on)
 {
 	unsigned int i, gpio, mask = 0;
-
-	if (!on) {
-		PKWR = 0;
-		return 0;
-	}
+	struct gpio_desc *d;
 
 	for (i = 0; i < ARRAY_SIZE(pxa27x_pkwr_gpio); i++) {
 
 		gpio = pxa27x_pkwr_gpio[i];
+		d = &gpio_desc[gpio];
 
-		if (gpio_desc[gpio].config & MFP_LPM_CAN_WAKEUP)
+		/* skip if configured as generic GPIO */
+		if (MFP_AF(d->config) == 0)
+			continue;
+
+		if (d->config & MFP_LPM_CAN_WAKEUP)
 			mask |= gpio_desc[gpio].mask;
 	}
 
-	PKWR = mask;
+	if (on)
+		PKWR |= mask;
+	else
+		PKWR &= ~mask;
 	return 0;
 }
 
@@ -328,6 +342,17 @@ static int pxa2xx_mfp_suspend(struct sys_device *d, pm_message_t state)
 {
 	int i;
 
+	/* set corresponding PGSR bit of those marked MFP_LPM_KEEP_OUTPUT */
+	for (i = 0; i < pxa_last_gpio; i++) {
+		if ((gpio_desc[i].config & MFP_LPM_KEEP_OUTPUT) &&
+		    (GPDR(i) & GPIO_bit(i))) {
+			if (GPLR(i) & GPIO_bit(i))
+				PGSR(i) |= GPIO_bit(i);
+			else
+				PGSR(i) &= ~GPIO_bit(i);
+		}
+	}
+
 	for (i = 0; i <= gpio_to_bank(pxa_last_gpio); i++) {
 
 		saved_gafr[0][i] = GAFR_L(i);