1 files changed, 62 insertions, 28 deletions

diff --git a/drivers/input/serio/libps2.c b/drivers/input/serio/libps2.c
index 92b92ee0379..d4c990f7c85 100644
--- a/drivers/input/serio/libps2.c
+++ b/drivers/input/serio/libps2.c
@@ -98,6 +98,66 @@ void ps2_drain(struct ps2dev *ps2dev, int maxbytes, int timeout)
 }
 
 /*
+ * ps2_is_keyboard_id() checks received ID byte against the list of
+ * known keyboard IDs.
+ */
+
+static inline int ps2_is_keyboard_id(char id_byte)
+{
+	static char keyboard_ids[] = {
+		0xab,	/* Regular keyboards		*/
+		0xac,	/* NCD Sun keyboard		*/
+		0x2b,	/* Trust keyboard, translated	*/
+		0x5d,	/* Trust keyboard		*/
+		0x60,	/* NMB SGI keyboard, translated */
+		0x47,	/* NMB SGI keyboard		*/
+	};
+
+	return memchr(keyboard_ids, id_byte, sizeof(keyboard_ids)) != NULL;
+}
+
+/*
+ * ps2_adjust_timeout() is called after receiving 1st byte of command
+ * response and tries to reduce remaining timeout to speed up command
+ * completion.
+ */
+
+static int ps2_adjust_timeout(struct ps2dev *ps2dev, int command, int timeout)
+{
+	switch (command) {
+		case PS2_CMD_RESET_BAT:
+			/*
+			 * Device has sent the first response byte after
+			 * reset command, reset is thus done, so we can
+			 * shorten the timeout.
+			 * The next byte will come soon (keyboard) or not
+			 * at all (mouse).
+			 */
+			if (timeout > msecs_to_jiffies(100))
+				timeout = msecs_to_jiffies(100);
+			break;
+
+		case PS2_CMD_GETID:
+			/*
+			 * If device behind the port is not a keyboard there
+			 * won't be 2nd byte of ID response.
+			 */
+			if (!ps2_is_keyboard_id(ps2dev->cmdbuf[1])) {
+				serio_pause_rx(ps2dev->serio);
+				ps2dev->flags = ps2dev->cmdcnt = 0;
+				serio_continue_rx(ps2dev->serio);
+				timeout = 0;
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	return timeout;
+}
+
+/*
  * ps2_command() sends a command and its parameters to the mouse,
  * then waits for the response and puts it in the param array.
  *
@@ -150,33 +210,7 @@ int ps2_command(struct ps2dev *ps2dev, unsigned char *param, int command)
 
 	if (ps2dev->cmdcnt && timeout > 0) {
 
-		if (command == PS2_CMD_RESET_BAT && timeout > msecs_to_jiffies(100)) {
-			/*
-			 * Device has sent the first response byte
-			 * after a reset command, reset is thus done,
-			 * shorten the timeout. The next byte will come
-			 * soon (keyboard) or not at all (mouse).
-			 */
-			timeout = msecs_to_jiffies(100);
-		}
-
-		if (command == PS2_CMD_GETID &&
-		    ps2dev->cmdbuf[receive - 1] != 0xab && /* Regular keyboards */
-		    ps2dev->cmdbuf[receive - 1] != 0xac && /* NCD Sun keyboard */
-		    ps2dev->cmdbuf[receive - 1] != 0x2b && /* Trust keyboard, translated */
-		    ps2dev->cmdbuf[receive - 1] != 0x5d && /* Trust keyboard */
-		    ps2dev->cmdbuf[receive - 1] != 0x60 && /* NMB SGI keyboard, translated */
-		    ps2dev->cmdbuf[receive - 1] != 0x47) { /* NMB SGI keyboard */
-			/*
-			 * Device behind the port is not a keyboard
-			 * so we don't need to wait for the 2nd byte
-			 * of ID response.
-			 */
-			serio_pause_rx(ps2dev->serio);
-			ps2dev->flags = ps2dev->cmdcnt = 0;
-			serio_continue_rx(ps2dev->serio);
-		}
-
+		timeout = ps2_adjust_timeout(ps2dev, command, timeout);
 		wait_event_timeout(ps2dev->wait,
 				   !(ps2dev->flags & PS2_FLAG_CMD), timeout);
 	}
@@ -190,7 +224,7 @@ int ps2_command(struct ps2dev *ps2dev, unsigned char *param, int command)
 
 	rc = 0;
 
-out:
+ out:
 	serio_pause_rx(ps2dev->serio);
 	ps2dev->flags = 0;
 	serio_continue_rx(ps2dev->serio);