2 files changed, 66 insertions, 2 deletions

diff --git a/payloads/libpayload/drivers/i8042/i8042.h b/payloads/libpayload/drivers/i8042/i8042.h
index bcb42fd13d..8c18670cb6 100644
--- a/payloads/libpayload/drivers/i8042/i8042.h
+++ b/payloads/libpayload/drivers/i8042/i8042.h
@@ -55,6 +55,7 @@
 #define   I8042_MODE_NUM_LOCK_OFF	(0 << 1)
 #define   I8042_MODE_SCROLL_LOCK_ON	(1 << 0)
 #define   I8042_MODE_SCROLL_LOCK_OFF	(0 << 0)
+#define I8042_KBCMD_ECHO		0xee
 #define I8042_KBCMD_SET_SCANCODE	0xf0
 #define I8042_KBCMD_SET_TYPEMATIC	0xf3
 #define I8042_KBCMD_EN			0xf4
diff --git a/payloads/libpayload/drivers/i8042/keyboard.c b/payloads/libpayload/drivers/i8042/keyboard.c
index 91a51bb0ee..6d0cdd1c7b 100644
--- a/payloads/libpayload/drivers/i8042/keyboard.c
+++ b/payloads/libpayload/drivers/i8042/keyboard.c
@@ -237,9 +237,17 @@ static bool set_scancode_set(const unsigned char set)
 	return ret;
 }
 
+static bool keyboard_peek_echo_result(void)
+{
+	const uint8_t ch = i8042_peek_data_ps2();
+	return ch == 0xee || ch == 0xfe;
+}
+
 static enum keyboard_state {
 	STATE_INIT = 0,
 	STATE_SIMPLIFIED_INIT,
+	STATE_HOTPLUG,
+	STATE_HOTPLUG_ECHO,
 	STATE_DISABLE_SCAN,
 	STATE_DRAIN_INPUT,
 	STATE_DISABLE_TRANSLATION,
@@ -250,6 +258,7 @@ static enum keyboard_state {
 	STATE_ENABLE_TRANSLATION,
 	STATE_ENABLE_SCAN,
 	STATE_RUNNING,
+	STATE_RUNNING_ECHO,
 	STATE_IGNORE,
 } keyboard_state;
 
@@ -257,6 +266,8 @@ static enum keyboard_state {
 static const char *const state_names[] = {
 	STATE_NAMES_ENTRY(INIT),
 	STATE_NAMES_ENTRY(SIMPLIFIED_INIT),
+	STATE_NAMES_ENTRY(HOTPLUG),
+	STATE_NAMES_ENTRY(HOTPLUG_ECHO),
 	STATE_NAMES_ENTRY(DISABLE_SCAN),
 	STATE_NAMES_ENTRY(DRAIN_INPUT),
 	STATE_NAMES_ENTRY(DISABLE_TRANSLATION),
@@ -267,6 +278,7 @@ static const char *const state_names[] = {
 	STATE_NAMES_ENTRY(ENABLE_TRANSLATION),
 	STATE_NAMES_ENTRY(ENABLE_SCAN),
 	STATE_NAMES_ENTRY(RUNNING),
+	STATE_NAMES_ENTRY(RUNNING_ECHO),
 	STATE_NAMES_ENTRY(IGNORE),
 };
 
@@ -301,6 +313,28 @@ static void keyboard_poll(void)
 		next_state = STATE_CONFIGURE;
 		break;
 
+	case STATE_HOTPLUG:
+		if (timer_us(state_time) > 1*1000*1000) {
+			i8042_write_data(I8042_KBCMD_ECHO);
+			next_state = STATE_HOTPLUG_ECHO;
+		}
+		break;
+
+	case STATE_HOTPLUG_ECHO:
+		if (!i8042_data_ready_ps2()) {
+			if (timer_us(state_time) > 200*1000)
+				next_state = STATE_HOTPLUG;
+			break;
+		}
+
+		if (keyboard_peek_echo_result()) {
+			next_state = STATE_DISABLE_SCAN;
+			keyboard_time = timer_us(0);
+		}
+		(void)i8042_read_data_ps2();
+
+		break;
+
 	case STATE_DISABLE_SCAN:
 		(void)keyboard_cmd(I8042_KBCMD_DEFAULT_DIS);
 		next_state = STATE_DRAIN_INPUT;
@@ -394,7 +428,31 @@ static void keyboard_poll(void)
 		break;
 
 	case STATE_RUNNING:
-		/* TODO: Use echo command to detect detach. */
+		if (!i8042_data_ready_ps2()) {
+			if (timer_us(state_time) > 500*1000) {
+				i8042_write_data(I8042_KBCMD_ECHO);
+				next_state = STATE_RUNNING_ECHO;
+			}
+		} else {
+			state_time = timer_us(0);
+		}
+		break;
+
+	case STATE_RUNNING_ECHO:
+		if (!i8042_data_ready_ps2()) {
+			if (timer_us(state_time) > 200*1000) {
+				debug("INFO: Keyboard echo timed out.\n");
+				next_state = STATE_HOTPLUG;
+			}
+			break;
+		}
+
+		if (keyboard_peek_echo_result()) {
+			(void)i8042_read_data_ps2();
+			next_state = STATE_RUNNING;
+		}
+
+		state_time = timer_us(0);
 		break;
 
 	case STATE_IGNORE:
@@ -405,7 +463,10 @@ static void keyboard_poll(void)
 
 	switch (next_state) {
 	case STATE_INIT:
+	case STATE_HOTPLUG:
+	case STATE_HOTPLUG_ECHO:
 	case STATE_RUNNING:
+	case STATE_RUNNING_ECHO:
 	case STATE_IGNORE:
 		break;
 	default:
@@ -424,7 +485,9 @@ static void keyboard_poll(void)
 bool keyboard_havechar(void)
 {
 	keyboard_poll();
-	return keyboard_state == STATE_RUNNING && i8042_data_ready_ps2();
+	return i8042_data_ready_ps2() &&
+	       (keyboard_state == STATE_RUNNING ||
+		(keyboard_state == STATE_RUNNING_ECHO && !keyboard_peek_echo_result()));
 }
 
 unsigned char keyboard_get_scancode(void)