Input: atkbd - harden ACK/NAK and command processing logic

Signed-off-by: Dmitry Torokhov <dtor@mail.ru>

drivers/input/keyboard/atkbd.c

@@ -173,8 +173,7 @@ static unsigned char atkbd_scroll_keys[5][2] = {
 #define ATKBD_FLAG_ACK		0	/* Waiting for ACK/NAK */
 #define ATKBD_FLAG_CMD		1	/* Waiting for command to finish */
 #define ATKBD_FLAG_CMD1		2	/* First byte of command response */
-#define ATKBD_FLAG_ID		3	/* First byte is not keyboard ID */
-#define ATKBD_FLAG_ENABLED	4	/* Waining for init to finish */
+#define ATKBD_FLAG_ENABLED	3	/* Waining for init to finish */
 
 /*
  * The atkbd control structure
@@ -254,34 +253,30 @@ static irqreturn_t atkbd_interrupt(struct serio *serio, unsigned char data,
 		atkbd->resend = 0;
 #endif
 
-	if (test_bit(ATKBD_FLAG_ACK, &atkbd->flags))
+	if (test_bit(ATKBD_FLAG_ACK, &atkbd->flags)) {
 		switch (code) {
 			case ATKBD_RET_ACK:
 				atkbd->nak = 0;
 				if (atkbd->cmdcnt) {
 					set_bit(ATKBD_FLAG_CMD, &atkbd->flags);
 					set_bit(ATKBD_FLAG_CMD1, &atkbd->flags);
-					set_bit(ATKBD_FLAG_ID, &atkbd->flags);
 				}
 				clear_bit(ATKBD_FLAG_ACK, &atkbd->flags);
-				goto out;
+				break;
 			case ATKBD_RET_NAK:
 				atkbd->nak = 1;
 				clear_bit(ATKBD_FLAG_ACK, &atkbd->flags);
-				goto out;
+				break;
 		}
+		goto out;
+	}
 
 	if (test_bit(ATKBD_FLAG_CMD, &atkbd->flags)) {
 
-		atkbd->cmdcnt--;
-		atkbd->cmdbuf[atkbd->cmdcnt] = code;
-
-		if (atkbd->cmdcnt == 1) {
-		    	if (code != 0xab && code != 0xac)
-				clear_bit(ATKBD_FLAG_ID, &atkbd->flags);
-			clear_bit(ATKBD_FLAG_CMD1, &atkbd->flags);
-		}
+		if (atkbd->cmdcnt)
+			atkbd->cmdbuf[--atkbd->cmdcnt] = code;
 
+		clear_bit(ATKBD_FLAG_CMD1, &atkbd->flags);
 		if (!atkbd->cmdcnt)
 			clear_bit(ATKBD_FLAG_CMD, &atkbd->flags);
 
@@ -426,14 +421,14 @@ static int atkbd_sendbyte(struct atkbd *atkbd, unsigned char byte)
 #ifdef ATKBD_DEBUG
 	printk(KERN_DEBUG "atkbd.c: Sent: %02x\n", byte);
 #endif
-
+	atkbd->nak = 1;
 	set_bit(ATKBD_FLAG_ACK, &atkbd->flags);
-	clear_bit(ATKBD_FLAG_CMD, &atkbd->flags);
-	if (serio_write(atkbd->serio, byte))
-		return -1;
-	while (test_bit(ATKBD_FLAG_ACK, &atkbd->flags) && timeout--) udelay(1);
-	clear_bit(ATKBD_FLAG_ACK, &atkbd->flags);
 
+	if (serio_write(atkbd->serio, byte) == 0)
+		while (test_bit(ATKBD_FLAG_ACK, &atkbd->flags) && timeout--)
+			udelay(1);
+
+	clear_bit(ATKBD_FLAG_ACK, &atkbd->flags);
 	return -atkbd->nak;
 }
 
@@ -447,33 +442,42 @@ static int atkbd_command(struct atkbd *atkbd, unsigned char *param, int command)
 	int timeout = 500000; /* 500 msec */
 	int send = (command >> 12) & 0xf;
 	int receive = (command >> 8) & 0xf;
+	int rc = -1;
 	int i;
 
-	atkbd->cmdcnt = receive;
-
 	if (command == ATKBD_CMD_RESET_BAT)
 		timeout = 4000000; /* 4 sec */
 
+	clear_bit(ATKBD_FLAG_CMD, &atkbd->flags);
+
 	if (receive && param)
 		for (i = 0; i < receive; i++)
 			atkbd->cmdbuf[(receive - 1) - i] = param[i];
 
+	atkbd->cmdcnt = receive;
+
 	if (command & 0xff)
 		if (atkbd_sendbyte(atkbd, command & 0xff))
-			return -1;
+			goto out;
 
 	for (i = 0; i < send; i++)
 		if (atkbd_sendbyte(atkbd, param[i]))
-			return -1;
+			goto out;
 
 	while (test_bit(ATKBD_FLAG_CMD, &atkbd->flags) && timeout--) {
 
 		if (!test_bit(ATKBD_FLAG_CMD1, &atkbd->flags)) {
-		    
+
 			if (command == ATKBD_CMD_RESET_BAT && timeout > 100000)
 				timeout = 100000;
 
-			if (command == ATKBD_CMD_GETID && !test_bit(ATKBD_FLAG_ID, &atkbd->flags)) {
+			if (command == ATKBD_CMD_GETID &&
+			    atkbd->cmdbuf[receive - 1] != 0xab && atkbd->cmdbuf[receive - 1] != 0xac) {
+				/*
+				 * Device behind the port is not a keyboard
+				 * so we don't need to wait for the 2nd byte
+				 * of ID response.
+				 */
 				clear_bit(ATKBD_FLAG_CMD, &atkbd->flags);
 				atkbd->cmdcnt = 0;
 				break;
@@ -483,19 +487,20 @@ static int atkbd_command(struct atkbd *atkbd, unsigned char *param, int command)
 		udelay(1);
 	}
 
-	clear_bit(ATKBD_FLAG_CMD, &atkbd->flags);
-
 	if (param)
 		for (i = 0; i < receive; i++)
 			param[i] = atkbd->cmdbuf[(receive - 1) - i];
 
-	if (command == ATKBD_CMD_RESET_BAT && atkbd->cmdcnt == 1)
-		return 0;
+	if (atkbd->cmdcnt && (command != ATKBD_CMD_RESET_BAT || atkbd->cmdcnt != 1))
+		goto out;
 
-	if (atkbd->cmdcnt)
-		return -1;
+	rc = 0;
 
-	return 0;
+out:
+	clear_bit(ATKBD_FLAG_CMD, &atkbd->flags);
+	clear_bit(ATKBD_FLAG_CMD1, &atkbd->flags);
+
+	return rc;
 }
 
 /*