From 3c422305869a1485bb0b6aec0665062ea033b323 Mon Sep 17 00:00:00 2001
From: Naveen Kumar Gaddipati <naveen.gaddipati@stericsson.com>
Date: Mon, 25 Jun 2012 00:25:41 -0700
Subject: [PATCH] --- yaml --- r: 318277 b: refs/heads/master c:
 af77c88b8971232f23b4cba7a46fc7a43914bf22 h: refs/heads/master i:   318275:
 5e99d1f4f38149970717a81fc206eff28e8eb9e7 v: v3

---
 [refs]                                        |  2 +-
 .../input/keyboard/nomadik-ske-keypad.c       | 65 ++++++++++++-------
 2 files changed, 41 insertions(+), 26 deletions(-)

diff --git a/[refs] b/[refs]
index 544678aa483f..fb421a053b3b 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: e7ec014a47e4d68fc01561d0541a50650646317c
+refs/heads/master: af77c88b8971232f23b4cba7a46fc7a43914bf22
diff --git a/trunk/drivers/input/keyboard/nomadik-ske-keypad.c b/trunk/drivers/input/keyboard/nomadik-ske-keypad.c
index 4ea4341a68c5..6857454bb13c 100644
--- a/trunk/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/trunk/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -135,12 +135,37 @@ static int __init ske_keypad_chip_init(struct ske_keypad *keypad)
 	return 0;
 }
 
-static void ske_keypad_read_data(struct ske_keypad *keypad)
+static void ske_keypad_report(struct ske_keypad *keypad, u8 status, int col)
 {
+	int row = 0, code, pos;
 	struct input_dev *input = keypad->input;
-	u16 status;
-	int col = 0, row = 0, code;
-	int ske_asr, ske_ris, key_pressed, i;
+	u32 ske_ris;
+	int key_pressed;
+	int num_of_rows;
+
+	/* find out the row */
+	num_of_rows = hweight8(status);
+	do {
+		pos = __ffs(status);
+		row = pos;
+		status &= ~(1 << pos);
+
+		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
+		ske_ris = readl(keypad->reg_base + SKE_RIS);
+		key_pressed = ske_ris & SKE_KPRISA;
+
+		input_event(input, EV_MSC, MSC_SCAN, code);
+		input_report_key(input, keypad->keymap[code], key_pressed);
+		input_sync(input);
+		num_of_rows--;
+	} while (num_of_rows);
+}
+
+static void ske_keypad_read_data(struct ske_keypad *keypad)
+{
+	u8 status;
+	int col = 0;
+	int ske_asr, i;
 
 	/*
 	 * Read the auto scan registers
@@ -154,25 +179,17 @@ static void ske_keypad_read_data(struct ske_keypad *keypad)
 		if (!ske_asr)
 			continue;
 
-		/* now that ASRx is zero, find out the column x and row y*/
-		if (ske_asr & 0xff) {
+		/* now that ASRx is zero, find out the coloumn x and row y */
+		status = ske_asr & 0xff;
+		if (status) {
 			col = i * 2;
-			status = ske_asr & 0xff;
-		} else {
+			ske_keypad_report(keypad, status, col);
+		}
+		status = (ske_asr & 0xff00) >> 8;
+		if (status) {
 			col = (i * 2) + 1;
-			status = (ske_asr & 0xff00) >> 8;
+			ske_keypad_report(keypad, status, col);
 		}
-
-		/* find out the row */
-		row = __ffs(status);
-
-		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
-		ske_ris = readl(keypad->reg_base + SKE_RIS);
-		key_pressed = ske_ris & SKE_KPRISA;
-
-		input_event(input, EV_MSC, MSC_SCAN, code);
-		input_report_key(input, keypad->keymap[code], key_pressed);
-		input_sync(input);
 	}
 }
 
@@ -186,12 +203,10 @@ static irqreturn_t ske_keypad_irq(int irq, void *dev_id)
 	ske_keypad_set_bits(keypad, SKE_ICR, 0x0, SKE_KPICA);
 
 	while ((readl(keypad->reg_base + SKE_CR) & SKE_KPASON) && --retries)
-		msleep(5);
+		cpu_relax();
 
-	if (retries) {
-		/* SKEx registers are stable and can be read */
-		ske_keypad_read_data(keypad);
-	}
+	/* SKEx registers are stable and can be read */
+	ske_keypad_read_data(keypad);
 
 	/* enable auto scan interrupts */
 	ske_keypad_set_bits(keypad, SKE_IMSC, 0x0, SKE_KPIMA);