Index: kernel/genarch/src/drivers/i8042/i8042.c
===================================================================
--- kernel/genarch/src/drivers/i8042/i8042.c	(revision c72dc15e1268838fa6c71bd50186bc53b39ee999)
+++ kernel/genarch/src/drivers/i8042/i8042.c	(revision 7aaed09d88be49fac8360d3017e3328ed9b0635c)
@@ -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);
 }
 
Index: kernel/genarch/src/fb/bfb.c
===================================================================
--- kernel/genarch/src/fb/bfb.c	(revision 7aaed09d88be49fac8360d3017e3328ed9b0635c)
+++ kernel/genarch/src/fb/bfb.c	(revision 7aaed09d88be49fac8360d3017e3328ed9b0635c)
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2006 Jakub Vana
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @addtogroup genarch
+ * @{
+ */
+/**
+ * @file
+ * @brief Boot framebuffer driver.
+ */
+
+#include <typedefs.h>
+#include <genarch/fb/fb.h>
+#include <genarch/fb/bfb.h>
+#include <console/console.h>
+
+uintptr_t bfb_addr = 0;
+uint32_t bfb_width = 0;
+uint32_t bfb_height = 0;
+uint16_t bfb_bpp = 0;
+uint32_t bfb_scanline = 0;
+
+uint8_t bfb_red_pos = 0;
+uint8_t bfb_red_size = 0;
+
+uint8_t bfb_green_pos = 0;
+uint8_t bfb_green_size = 0;
+
+uint8_t bfb_blue_pos = 0;
+uint8_t bfb_blue_size = 0;
+
+bool bfb_init(void)
+{
+	if ((bfb_addr == 0) || (bfb_width == 0) || (bfb_height == 0) ||
+	    (bfb_bpp == 0) || (bfb_scanline == 0))
+		return false;
+	
+	fb_properties_t bfb_props = {
+		.addr = bfb_addr,
+		.offset = 0,
+		.x = bfb_width,
+		.y = bfb_height,
+		.scan = bfb_scanline
+	};
+	
+	switch (bfb_bpp) {
+	case 8:
+		bfb_props.visual = VISUAL_INDIRECT_8;
+		break;
+	case 16:
+		if ((bfb_red_pos == 10) && (bfb_red_size == 5) &&
+		    (bfb_green_pos == 5) && (bfb_green_size == 5) &&
+		    (bfb_blue_pos == 0) && (bfb_blue_size == 5))
+			bfb_props.visual = VISUAL_RGB_5_5_5_LE;
+		else
+			bfb_props.visual = VISUAL_RGB_5_6_5_LE;
+		break;
+	case 24:
+		bfb_props.visual = VISUAL_BGR_8_8_8;
+		break;
+	case 32:
+		bfb_props.visual = VISUAL_BGR_8_8_8_0;
+		break;
+	default:
+		LOG("Unsupported bits per pixel.");
+		return false;
+	}
+	
+	outdev_t *fbdev = fb_init(&bfb_props);
+	if (!fbdev)
+		return false;
+	
+	stdout_wire(fbdev);
+	return true;
+}
+
+/** @}
+ */
Index: kernel/genarch/src/multiboot/multiboot.c
===================================================================
--- kernel/genarch/src/multiboot/multiboot.c	(revision c72dc15e1268838fa6c71bd50186bc53b39ee999)
+++ kernel/genarch/src/multiboot/multiboot.c	(revision 7aaed09d88be49fac8360d3017e3328ed9b0635c)
@@ -33,18 +33,17 @@
  */
 
+#include <typedefs.h>
 #include <genarch/multiboot/multiboot.h>
-#include <typedefs.h>
 #include <config.h>
 #include <str.h>
-#include <macros.h>
 
 /** Extract command name from the multiboot module command line.
  *
- * @param buf      Destination buffer (will always NULL-terminate).
- * @param sz       Size of destination buffer (in bytes).
+ * @param buf      Destination buffer (will be always NULL-terminated).
+ * @param size     Size of destination buffer (in bytes).
  * @param cmd_line Input string (the command line).
  *
  */
-static void extract_command(char *buf, size_t sz, const char *cmd_line)
+void multiboot_extract_command(char *buf, size_t size, const char *cmd_line)
 {
 	/* Find the first space. */
@@ -69,5 +68,41 @@
 	
 	/* Copy the command. */
-	str_ncpy(buf, sz, start, (size_t) (end - start));
+	str_ncpy(buf, size, start, (size_t) (end - start));
+}
+
+static void multiboot_modules(uint32_t count, multiboot_module_t *mods)
+{
+	for (uint32_t i = 0; i < count; i++) {
+		if (init.cnt >= CONFIG_INIT_TASKS)
+			break;
+		
+		init.tasks[init.cnt].addr = PA2KA(mods[i].start);
+		init.tasks[init.cnt].size = mods[i].end - mods[i].start;
+		
+		/* Copy command line, if available. */
+		if (mods[i].string) {
+			multiboot_extract_command(init.tasks[init.cnt].name,
+			    CONFIG_TASK_NAME_BUFLEN, MULTIBOOT_PTR(mods[i].string));
+		} else
+			init.tasks[init.cnt].name[0] = 0;
+		
+		init.cnt++;
+	}
+}
+
+static void multiboot_memmap(uint32_t length, multiboot_memmap_t *memmap)
+{
+	uint32_t pos = 0;
+	
+	while ((pos < length) && (e820counter < MEMMAP_E820_MAX_RECORDS)) {
+		e820table[e820counter] = memmap->mm_info;
+		
+		/* Compute address of next structure. */
+		uint32_t size = sizeof(memmap->size) + memmap->size;
+		memmap = (multiboot_memmap_t *) ((uintptr_t) memmap + size);
+		pos += size;
+		
+		e820counter++;
+	}
 }
 
@@ -78,59 +113,21 @@
  *
  * @param signature Should contain the multiboot signature.
- * @param mi        Pointer to the multiboot information structure.
+ * @param info      Multiboot information structure.
+ *
  */
-void multiboot_info_parse(uint32_t signature, const multiboot_info_t *mi)
+void multiboot_info_parse(uint32_t signature, const multiboot_info_t *info)
 {
-	uint32_t flags;
+	if (signature != MULTIBOOT_LOADER_MAGIC)
+		return;
 	
-	if (signature == MULTIBOOT_LOADER_MAGIC)
-		flags = mi->flags;
-	else {
-		/* No multiboot info available. */
-		flags = 0;
-	}
-	
-	/* Copy module information. */
-	uint32_t i;
-	if ((flags & MBINFO_FLAGS_MODS) != 0) {
-		init.cnt = min(mi->mods_count, CONFIG_INIT_TASKS);
-		multiboot_mod_t *mods
-		    = (multiboot_mod_t *) MULTIBOOT_PTR(mi->mods_addr);
-		
-		for (i = 0; i < init.cnt; i++) {
-			init.tasks[i].addr = PA2KA(mods[i].start);
-			init.tasks[i].size = mods[i].end - mods[i].start;
-			
-			/* Copy command line, if available. */
-			if (mods[i].string) {
-				extract_command(init.tasks[i].name,
-				    CONFIG_TASK_NAME_BUFLEN,
-				    MULTIBOOT_PTR(mods[i].string));
-			} else
-				init.tasks[i].name[0] = 0;
-		}
-	} else
-		init.cnt = 0;
+	/* Copy modules information. */
+	if ((info->flags & MULTIBOOT_INFO_FLAGS_MODS) != 0)
+		multiboot_modules(info->mods_count,
+		    (multiboot_module_t *) MULTIBOOT_PTR(info->mods_addr));
 	
 	/* Copy memory map. */
-	
-	if ((flags & MBINFO_FLAGS_MMAP) != 0) {
-		int32_t mmap_length = mi->mmap_length;
-		multiboot_mmap_t *mme = MULTIBOOT_PTR(mi->mmap_addr);
-		e820counter = 0;
-		
-		i = 0;
-		while ((mmap_length > 0) && (i < MEMMAP_E820_MAX_RECORDS)) {
-			e820table[i++] = mme->mm_info;
-			
-			/* Compute address of next structure. */
-			uint32_t size = sizeof(mme->size) + mme->size;
-			mme = ((void *) mme) + size;
-			mmap_length -= size;
-		}
-		
-		e820counter = i;
-	} else
-		e820counter = 0;
+	if ((info->flags & MULTIBOOT_INFO_FLAGS_MMAP) != 0)
+		multiboot_memmap(info->mmap_length,
+		    (multiboot_memmap_t *) MULTIBOOT_PTR(info->mmap_addr));
 }
 
Index: kernel/genarch/src/multiboot/multiboot2.c
===================================================================
--- kernel/genarch/src/multiboot/multiboot2.c	(revision 7aaed09d88be49fac8360d3017e3328ed9b0635c)
+++ kernel/genarch/src/multiboot/multiboot2.c	(revision 7aaed09d88be49fac8360d3017e3328ed9b0635c)
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2011 Martin Decky
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @addtogroup genarch
+ * @{
+ */
+/** @file
+ */
+
+#include <typedefs.h>
+#include <genarch/multiboot/multiboot.h>
+#include <genarch/multiboot/multiboot2.h>
+#include <genarch/fb/bfb.h>
+#include <config.h>
+#include <align.h>
+
+#define MULTIBOOT2_TAG_ALIGN  8
+
+static void multiboot2_module(const multiboot2_module_t *module)
+{
+	if (init.cnt < CONFIG_INIT_TASKS) {
+		init.tasks[init.cnt].addr = PA2KA(module->start);
+		init.tasks[init.cnt].size = module->end - module->start;
+		multiboot_extract_command(init.tasks[init.cnt].name,
+		    CONFIG_TASK_NAME_BUFLEN, module->string);
+		
+		init.cnt++;
+	}
+}
+
+static void multiboot2_memmap(uint32_t length, const multiboot2_memmap_t *memmap)
+{
+	multiboot2_memmap_entry_t *entry = (multiboot2_memmap_entry_t *)
+	    ((uintptr_t) memmap + sizeof(*memmap));
+	uint32_t pos = sizeof(*memmap);
+	
+	while ((pos < length) && (e820counter < MEMMAP_E820_MAX_RECORDS)) {
+		e820table[e820counter].base_address = entry->base_address;
+		e820table[e820counter].size = entry->size;
+		e820table[e820counter].type = entry->type;
+		
+		/* Compute address of next entry. */
+		entry = (multiboot2_memmap_entry_t *)
+		    ((uintptr_t) entry + memmap->entry_size);
+		pos += memmap->entry_size;
+		
+		e820counter++;
+	}
+}
+
+static void multiboot2_fbinfo(const multiboot2_fbinfo_t *fbinfo)
+{
+#ifdef CONFIG_FB
+	if (fbinfo->visual == MULTIBOOT2_VISUAL_RGB) {
+		bfb_addr = fbinfo->addr;
+		bfb_width = fbinfo->width;
+		bfb_height = fbinfo->height;
+		bfb_bpp = fbinfo->bpp;
+		bfb_scanline = fbinfo->scanline;
+		
+		bfb_red_pos = fbinfo->rgb.red_pos;
+		bfb_red_size = fbinfo->rgb.red_size;
+		
+		bfb_green_pos = fbinfo->rgb.green_pos;
+		bfb_green_size = fbinfo->rgb.green_size;
+		
+		bfb_blue_pos = fbinfo->rgb.blue_pos;
+		bfb_blue_size = fbinfo->rgb.blue_size;
+	}
+#endif
+}
+
+/** Parse multiboot2 information structure.
+ *
+ * If @a signature does not contain a valid multiboot2 signature,
+ * assumes no multiboot2 information is available.
+ *
+ * @param signature Should contain the multiboot2 signature.
+ * @param info      Multiboot2 information structure.
+ *
+ */
+void multiboot2_info_parse(uint32_t signature, const multiboot2_info_t *info)
+{
+	if (signature != MULTIBOOT2_LOADER_MAGIC)
+		return;
+	
+	const multiboot2_tag_t *tag = (const multiboot2_tag_t *)
+	    ALIGN_UP((uintptr_t) info + sizeof(*info), MULTIBOOT2_TAG_ALIGN);
+	
+	while (tag->type != MULTIBOOT2_TAG_TERMINATOR) {
+		switch (tag->type) {
+		case MULTIBOOT2_TAG_MODULE:
+			multiboot2_module(&tag->module);
+			break;
+		case MULTIBOOT2_TAG_MEMMAP:
+			multiboot2_memmap(tag->size, &tag->memmap);
+			break;
+		case MULTIBOOT2_TAG_FBINFO:
+			multiboot2_fbinfo(&tag->fbinfo);
+			break;
+		}
+		
+		tag = (const multiboot2_tag_t *)
+		    ALIGN_UP((uintptr_t) tag + tag->size, MULTIBOOT2_TAG_ALIGN);
+	}
+}
+
+/** @}
+ */