2 files changed, 17 insertions, 49 deletions

diff --git a/payloads/libpayload/drivers/i8042/i8042.h b/payloads/libpayload/drivers/i8042/i8042.h
index 6d15d1e0c5..bcb42fd13d 100644
--- a/payloads/libpayload/drivers/i8042/i8042.h
+++ b/payloads/libpayload/drivers/i8042/i8042.h
@@ -56,9 +56,6 @@
 #define   I8042_MODE_SCROLL_LOCK_ON	(1 << 0)
 #define   I8042_MODE_SCROLL_LOCK_OFF	(0 << 0)
 #define I8042_KBCMD_SET_SCANCODE	0xf0
-#define   I8042_SCANCODE_SET_1		(1)
-#define   I8042_SCANCODE_SET_2		(2)
-#define   I8042_SCANCODE_SET_3		(3)
 #define I8042_KBCMD_SET_TYPEMATIC	0xf3
 #define I8042_KBCMD_EN			0xf4
 #define I8042_KBCMD_DEFAULT_DIS		0xf5
diff --git a/payloads/libpayload/drivers/i8042/keyboard.c b/payloads/libpayload/drivers/i8042/keyboard.c
index 2dec3a38f2..22747ff79d 100644
--- a/payloads/libpayload/drivers/i8042/keyboard.c
+++ b/payloads/libpayload/drivers/i8042/keyboard.c
@@ -313,55 +313,22 @@ static struct console_input_driver cons = {
 	.input_type = CONSOLE_INPUT_TYPE_EC,
 };
 
-/* Enable keyboard translated */
-static bool enable_translated(void)
+static bool set_scancode_set(const unsigned char set)
 {
-	if (!i8042_cmd(I8042_CMD_RD_CMD_BYTE)) {
-		int cmd = i8042_read_data_ps2();
-		cmd |= I8042_CMD_BYTE_XLATE;
-		if (!i8042_cmd(I8042_CMD_WR_CMD_BYTE)) {
-			i8042_write_data(cmd);
-		} else {
-			printf("ERROR: i8042_cmd WR_CMD failed!\n");
-			return false;
-		}
-	} else {
-		printf("ERROR: i8042_cmd RD_CMD failed!\n");
+	bool ret;
+
+	if (set < 1 || set > 3)
 		return false;
-	}
-	return true;
-}
 
-/* Set scancode set 1 */
-static bool set_scancode_set(void)
-{
-	bool ret;
 	ret = keyboard_cmd(I8042_KBCMD_SET_SCANCODE);
 	if (!ret) {
 		printf("ERROR: Keyboard set scancode failed!\n");
 		return ret;
 	}
 
-	ret = keyboard_cmd(I8042_SCANCODE_SET_1);
-	if (!ret) {
-		printf("ERROR: Keyboard scancode set#1 failed!\n");
-		return ret;
-	}
-
-	/*
-	 * Set default parameters.
-	 * Fix for broken QEMU PS/2 make scancodes.
-	 */
-	ret = keyboard_cmd(I8042_KBCMD_SET_DEFAULT);
+	ret = keyboard_cmd(set);
 	if (!ret) {
-		printf("ERROR: Keyboard set default params failed!\n");
-		return ret;
-	}
-
-	/* Enable scanning */
-	ret = keyboard_cmd(I8042_KBCMD_EN);
-	if (!ret) {
-		printf("ERROR: Keyboard enable scanning failed!\n");
+		printf("ERROR: Keyboard scancode set#%u failed!\n", set);
 		return ret;
 	}
 
@@ -383,13 +350,17 @@ void keyboard_init(void)
 	/* Enable first PS/2 port */
 	i8042_cmd(I8042_CMD_EN_KB);
 
-	if (CONFIG(LP_PC_KEYBOARD_AT_TRANSLATED)) {
-		if (!enable_translated())
-			return;
-	} else {
-		if (!set_scancode_set())
-			return;
-	}
+	i8042_set_kbd_translation(false);
+
+	if (set_scancode_set(2))
+		i8042_set_kbd_translation(true);
+	else if (!set_scancode_set(1))
+		return; /* give up, leave keyboard input disabled */
+
+	/* Enable scanning */
+	const bool ret = keyboard_cmd(I8042_KBCMD_EN);
+	if (!ret)
+		printf("ERROR: Keyboard enable scanning failed!\n");
 
 	console_add_input_driver(&cons);
 }