Index: kernel/genarch/src/drivers/i8042/i8042.c
===================================================================
--- kernel/genarch/src/drivers/i8042/i8042.c	(revision 336d2f520e09eb22615e1ef5eed791543683ae14)
+++ kernel/genarch/src/drivers/i8042/i8042.c	(revision 89128f37525649ee8a7c2d6155cdc0a5b27244e9)
@@ -44,4 +44,5 @@
 #include <mm/slab.h>
 #include <ddi/device.h>
+#include <time/delay.h>
 
 #define i8042_SET_COMMAND  0x60
@@ -51,4 +52,6 @@
 #define i8042_BUFFER_FULL_MASK  0x01
 #define i8042_WAIT_MASK         0x02
+
+#define i8042_TIMEOUT  65536
 
 static irq_ownership_t i8042_claim(irq_t *irq)
@@ -77,6 +80,24 @@
 static void i8042_clear_buffer(i8042_t *dev)
 {
-	while (pio_read_8(&dev->status) & i8042_BUFFER_FULL_MASK)
+	for (uint32_t i = 0; i < i8042_TIMEOUT; i++) {
+		if ((pio_read_8(&dev->status) & i8042_BUFFER_FULL_MASK) == 0)
+			break;
+		
 		(void) pio_read_8(&dev->data);
+		delay(50);  /* 50 us think time */
+	}
+}
+
+static void i8042_send_command(i8042_t *dev, uint8_t cmd)
+{
+	for (uint32_t i = 0; i < i8042_TIMEOUT; i++) {
+		if ((pio_read_8(&dev->status) & i8042_WAIT_MASK) == 0)
+			break;
+		
+		delay(50);  /* 50 us think time */
+	}
+	
+	pio_write_8(&dev->status, cmd);
+	delay(10000);  /* 10 ms think time */
 }
 
@@ -84,6 +105,6 @@
 i8042_instance_t *i8042_init(i8042_t *dev, inr_t inr)
 {
-	i8042_instance_t *instance
-	    = malloc(sizeof(i8042_instance_t), FRAME_ATOMIC);
+	i8042_instance_t *instance =
+	    malloc(sizeof(i8042_instance_t), FRAME_ATOMIC);
 	if (instance) {
 		instance->i8042 = dev;
@@ -96,5 +117,4 @@
 		instance->irq.handler = i8042_irq_handler;
 		instance->irq.instance = instance;
-		
 	}
 	
@@ -107,7 +127,8 @@
 	ASSERT(kbrdin);
 	
+	i8042_clear_buffer(instance->i8042);
+	
 	instance->kbrdin = kbrdin;
 	irq_register(&instance->irq);
-	i8042_clear_buffer(instance->i8042);
 }
 
@@ -116,9 +137,6 @@
 {
 	interrupts_disable();
-	
 	i8042_clear_buffer(dev);
-	
-	/* Reset CPU */
-	pio_write_8(&dev->status, i8042_CPU_RESET);
+	i8042_send_command(dev, i8042_CPU_RESET);
 }