[coreboot-gerrit] New patch to review for coreboot: 6616d9c libpayload: Add PS/2 keyboard mode setting code back

[Andrew Wu (arw@dmp.com.tw)]

Andrew Wu (arw at dmp.com.tw) just uploaded a new patch set to gerrit, which you can find at http://review.coreboot.org/3956

-gerrit

commit 6616d9c9a93506e4a9177037e05a254ef901e7aa
Author: Andrew Wu <arw at dmp.com.tw>
Date:   Tue Oct 8 17:58:14 2013 +0800

    libpayload: Add PS/2 keyboard mode setting code back
    
    PS/2 keyboard controller may not set the 'XLATE' bit on boot by
    default.
    
    Revert keyboard.c to earlier version (commit a2d78), which sets
    the 'XLATE' bit. Add keyboard disabling/enabling during setting
    mode register to avoid scancode data interference, so the problem
    mentioned in commit dd6c4 is still fixed.
    
    Tested on Vortex86EX hardware with PS/2 keyboard.
    
    Change-Id: I42cedfdff2db5cfba39de8811284a72a9793f294
    Signed-off-by: Andrew Wu <arw at dmp.com.tw>
---
 payloads/libpayload/drivers/keyboard.c | 74 ++++++++++++++++++++++++++++++++++
 1 file changed, 74 insertions(+)

diff --git a/payloads/libpayload/drivers/keyboard.c b/payloads/libpayload/drivers/keyboard.c
index e65f085..5fa90dd 100644
--- a/payloads/libpayload/drivers/keyboard.c
+++ b/payloads/libpayload/drivers/keyboard.c
@@ -31,6 +31,13 @@
 #include <libpayload-config.h>
 #include <libpayload.h>
 
+#define I8042_CMD_READ_MODE  0x20
+#define I8042_CMD_WRITE_MODE 0x60
+#define I8042_CMD_DISABLE_KB 0xad
+#define I8042_CMD_ENABLE_KB  0xae
+
+#define I8042_MODE_XLATE     0x40
+
 struct layout_maps {
 	const char *country;
 	const unsigned short map[4][0x57];
@@ -256,6 +263,54 @@ int keyboard_getchar(void)
 	return ret;
 }
 
+static int keyboard_wait_read(void)
+{
+	int retries = 10000;
+
+	while(retries-- && !(inb(0x64) & 0x01))
+		udelay(50);
+
+	return (retries <= 0) ? -1 : 0;
+}
+
+static int keyboard_wait_write(void)
+{
+	int retries = 10000;
+
+	while(retries-- && (inb(0x64) & 0x02))
+		udelay(50);
+
+	return (retries <= 0) ? -1 : 0;
+}
+
+static unsigned char keyboard_get_mode(void)
+{
+	keyboard_wait_write();
+	outb(I8042_CMD_READ_MODE, 0x64);
+	keyboard_wait_read();
+	return inb(0x60);
+}
+
+static void keyboard_set_mode(unsigned char mode)
+{
+	keyboard_wait_write();
+	outb(I8042_CMD_WRITE_MODE, 0x64);
+	keyboard_wait_write();
+	outb(mode, 0x60);
+}
+
+static void keyboard_disable(void)
+{
+	keyboard_wait_write();
+	outb(I8042_CMD_DISABLE_KB, 0x64);
+}
+
+static void keyboard_enable(void)
+{
+	keyboard_wait_write();
+	outb(I8042_CMD_ENABLE_KB, 0x64);
+}
+
 /**
  * Set keyboard layout
  * @param country string describing the keyboard layout language.
@@ -287,16 +342,35 @@ static struct console_input_driver cons = {
 
 void keyboard_init(void)
 {
+	u8 mode;
 	map = &keyboard_layouts[0];
 
 	/* If 0x64 returns 0xff, then we have no keyboard
 	 * controller */
+
 	if (inb(0x64) == 0xFF)
 		return;
 
 	/* Empty keyboard buffer */
 	while (keyboard_havechar()) keyboard_getchar();
 
+	/* Before setup keyboard, lock keyboard input first */
+	keyboard_disable();
+
+	/* Read the current mode */
+	mode = keyboard_get_mode();
+
+	/* Turn on scancode translate mode so that we can
+	   use the scancode set 1 tables */
+
+	mode |= I8042_MODE_XLATE;
+
+	/* Write the new mode */
+	keyboard_set_mode(mode);
+
+	/* Now it is safe to enable keyboard input */
+	keyboard_enable();
+
 	console_add_input_driver(&cons);
 }