diff options
| -rw-r--r-- | src/pc80/keyboard.c | 50 |
1 files changed, 35 insertions, 15 deletions
diff --git a/src/pc80/keyboard.c b/src/pc80/keyboard.c index d293f714de..f46d1ffa16 100644 --- a/src/pc80/keyboard.c +++ b/src/pc80/keyboard.c @@ -3,21 +3,45 @@ #include <device/device.h> #include <arch/io.h> +static int kbd_empty_input_buffer(void) +{ + unsigned long timeout; + for(timeout = 1000000; timeout && (inb(0x64) & 0x02); timeout--) { + post_code(0); + } + return !!timeout; +} + +static int kbd_empty_output_buffer(void) +{ + unsigned long timeout; + for(timeout = 1000000; timeout && ((inb(0x64) & 0x01) == 0); timeout--) { + post_code(0); + } + return !!timeout; +} + /* much better keyboard init courtesy ollie@sis.com.tw TODO: Typematic Setting, the keyboard is too slow for me */ static void pc_keyboard_init(struct pc_keyboard *keyboard) { - volatile unsigned char regval; + unsigned char regval; + unsigned long timeout; /* send cmd = 0xAA, self test 8042 */ outb(0xaa, 0x64); /* empty input buffer or any other command/data will be lost */ - while ((inb(0x64) & 0x02)) - post_code(0); + if (!kbd_empty_input_buffer()) { + printk_err("Keyboard input buffer would not empty\n"); + return; + } + /* empty output buffer or any other command/data will be lost */ - while ((inb(0x64) & 0x01) == 0) - post_code(1); + if (!kbd_empty_output_buffer()) { + printk_err("Keyboard output buffer would not empty\n"); + return; + } /* read self-test result, 0x55 should be returned form 0x60 */ if ((regval = inb(0x60) != 0x55)) @@ -25,29 +49,25 @@ static void pc_keyboard_init(struct pc_keyboard *keyboard) /* enable keyboard interface */ outb(0x60, 0x64); - while ((inb(0x64) & 0x02)) - post_code(2); + kbd_empty_input_buffer(); /* send cmd: enable IRQ 1 */ outb(0x61, 0x60); - while ((inb(0x64) & 0x02)) - post_code(3); + kbd_empty_input_buffer(); /* reset kerboard and self test (keyboard side) */ outb(0xff, 0x60); /* empty inut bufferm or any other command/data will be lost */ - while ((inb(0x64) & 0x02)) - post_code(4); + kbd_empty_input_buffer(); + /* empty output buffer or any other command/data will be lost */ - while ((inb(0x64) & 0x01) == 0) - post_code(5); + kbd_empty_output_buffer(); if ((regval = inb(0x60) != 0xfa)) return; - while ((inb(0x64) & 0x01) == 0) - post_code(6); + kbd_empty_output_buffer(); if ((regval = inb(0x60) != 0xaa)) return; } |