USB updates from our internal tree

- support MMC2 devices
- make usb stack more solid
- drop some unused functions
- fix lowspeed/speed naming
- add support for "quirks"
- improve usbhid driver

Signed-off-by: Stefan Reinauer <stepan@coresystems.de>
Acked-by: Joseph Smith <joe@settoplinux.org>


git-svn-id: svn://svn.coreboot.org/coreboot/trunk@5299 2b7e53f0-3cfb-0310-b3e9-8179ed1497e1

10 files changed, 734 insertions, 253 deletions

diff --git a/payloads/libpayload/drivers/Makefile.inc b/payloads/libpayload/drivers/Makefile.inc
index 708c36d45e..0c7a4bc905 100644
--- a/payloads/libpayload/drivers/Makefile.inc
+++ b/payloads/libpayload/drivers/Makefile.inc
@@ -56,6 +56,7 @@ TARGETS-$(CONFIG_COREBOOT_VIDEO_CONSOLE) += drivers/video/font8x16.o
 TARGETS-$(CONFIG_USB) += drivers/usb/usbinit.o
 TARGETS-$(CONFIG_USB) += drivers/usb/usb.o
 TARGETS-$(CONFIG_USB) += drivers/usb/usb_dev.o
+TARGETS-$(CONFIG_USB) += drivers/usb/quirks.o
 TARGETS-$(CONFIG_USB_HUB) += drivers/usb/usbhub.o
 TARGETS-$(CONFIG_USB_UHCI) += drivers/usb/uhci.o
 TARGETS-$(CONFIG_USB_UHCI) += drivers/usb/uhci_rh.o
diff --git a/payloads/libpayload/drivers/usb/quirks.c b/payloads/libpayload/drivers/usb/quirks.c
new file mode 100644
index 0000000000..87878b08ae
--- /dev/null
+++ b/payloads/libpayload/drivers/usb/quirks.c
@@ -0,0 +1,88 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright (C) 2010 coresystems GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ * 3. 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 AND CONTRIBUTORS ``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 OR CONTRIBUTORS 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.
+ */
+
+//#define USB_DEBUG
+
+#include <libpayload-config.h>
+#include <usb/usb.h>
+
+typedef struct {
+	u16 vendor, device;
+	u32 quirks;
+	int interface;
+} usb_quirks_t;
+
+// IDs without a quirk don't need to be mentioned in this list
+// but some are here for easier testing.
+
+usb_quirks_t usb_quirks[] = {
+	/* Working chips,... remove before next release */
+	{ 0x3538, 0x0054, USB_QUIRK_NONE, 0 },	// PQI 1GB 
+	{ 0x13fd, 0x0841, USB_QUIRK_NONE, 0 },	// Samsung SE-S084
+
+	/* Silence the warning for known devices with more
+	 * than one interface
+	 */
+	{ 0x1267, 0x0103, USB_QUIRK_NONE, 1 },	// Keyboard Trust KB-1800S
+	{ 0x0a12, 0x0001, USB_QUIRK_NONE, 1 },	// Bluetooth Allnet ALL1575 
+
+	/* Currently unsupported, possibly interesting devices:
+	 * FTDI serial: device 0x0403:0x6001 is USB 1.10 (class ff)
+	 * UPEK TouchChip: device 0x147e:0x2016 is USB 1.0 (class ff)
+	 */
+};
+
+u32 usb_quirk_check(u16 vendor, u16 device)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(usb_quirks); i++) {
+		if ((usb_quirks[i].vendor == vendor) &&
+				(usb_quirks[i].device == device)) {
+			debug("USB quirks enabled: %08x\n",
+					usb_quirks[i].quirks);
+			return usb_quirks[i].quirks;
+		}
+	}
+
+	return USB_QUIRK_NONE;
+}
+
+int usb_interface_check(u16 vendor, u16 device)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(usb_quirks); i++) {
+		if ((usb_quirks[i].vendor == vendor) &&
+				(usb_quirks[i].device == device)) {
+			return usb_quirks[i].interface;
+		}
+	}
+
+	return 0;
+}
+
diff --git a/payloads/libpayload/drivers/usb/uhci.c b/payloads/libpayload/drivers/usb/uhci.c
index def6b45084..b6ed994e93 100644
--- a/payloads/libpayload/drivers/usb/uhci.c
+++ b/payloads/libpayload/drivers/usb/uhci.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2010 coresystems GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -27,6 +27,8 @@
  * SUCH DAMAGE.
  */
 
+//#define USB_DEBUG
+
 #include <usb/usb.h>
 #include "uhci.h"
 #include <arch/virtual.h>
@@ -35,8 +37,6 @@ static void uhci_start (hci_t *controller);
 static void uhci_stop (hci_t *controller);
 static void uhci_reset (hci_t *controller);
 static void uhci_shutdown (hci_t *controller);
-static int uhci_packet (usbdev_t *dev, int endp, int pid, int toggle,
-			int length, u8 *data);
 static int uhci_bulk (endpoint_t *ep, int size, u8 *data, int finalize);
 static int uhci_control (usbdev_t *dev, pid_t dir, int drlen, void *devreq,
 			 int dalen, u8 *data);
@@ -128,6 +128,8 @@ hci_t *
 uhci_init (pcidev_t addr)
 {
 	int i;
+	u16 reg16;
+
 	hci_t *controller = new_controller ();
 
 	if (!controller)
@@ -141,7 +143,6 @@ uhci_init (pcidev_t addr)
 	controller->stop = uhci_stop;
 	controller->reset = uhci_reset;
 	controller->shutdown = uhci_shutdown;
-	controller->packet = uhci_packet;
 	controller->bulk = uhci_bulk;
 	controller->control = uhci_control;
 	controller->create_intr_queue = uhci_create_intr_queue;
@@ -160,7 +161,9 @@ uhci_init (pcidev_t addr)
 	uhci_stop (controller);
 	mdelay (1);
 	uhci_reg_write16 (controller, USBSTS, 0x3f);
-	pci_write_config32 (controller->bus_address, 0xc0, 0x8f00);
+	reg16 = pci_read_config16(controller->bus_address, 0xc0);
+	reg16 &= 0xdf80;
+	pci_write_config16 (controller->bus_address, 0xc0, reg16);
 
 	UHCI_INST (controller)->framelistptr = memalign (0x1000, 1024 * sizeof (flistp_t *));	/* 4kb aligned to 4kb */
 	if (! UHCI_INST (controller)->framelistptr)
@@ -277,18 +280,6 @@ wait_for_completed_qh (hci_t *controller, qh_t *qh)
 		0) ? 0 : GET_TD (phys_to_virt (qh->elementlinkptr.ptr));
 }
 
-static void
-wait_for_completed_td (hci_t *controller, td_t *td)
-{
-	int timeout = 10000;
-	while ((td->status_active == 1)
-	       && ((uhci_reg_read16 (controller, USBSTS) & 2) == 0)
-	       && (timeout-- > 0)) {
-		uhci_reg_mask16 (controller, USBSTS, ~0, 0);	// clear resettable registers
-		udelay (10);
-	}
-}
-
 static int
 maxlen (int size)
 {
@@ -331,7 +322,7 @@ uhci_control (usbdev_t *dev, pid_t dir, int drlen, void *devreq, int dalen,
 	tds[0].maxlen = maxlen (drlen);
 	tds[0].counter = 3;
 	tds[0].data_toggle = 0;
-	tds[0].lowspeed = dev->lowspeed;
+	tds[0].lowspeed = dev->speed;
 	tds[0].bufptr = virt_to_phys (devreq);
 	tds[0].status_active = 1;
 
@@ -343,7 +334,7 @@ uhci_control (usbdev_t *dev, pid_t dir, int drlen, void *devreq, int dalen,
 		tds[i].maxlen = maxlen (min (mlen, dalen));
 		tds[i].counter = 3;
 		tds[i].data_toggle = toggle;
-		tds[i].lowspeed = dev->lowspeed;
+		tds[i].lowspeed = dev->speed;
 		tds[i].bufptr = virt_to_phys (data);
 		tds[i].status_active = 1;
 		toggle ^= 1;
@@ -357,7 +348,8 @@ uhci_control (usbdev_t *dev, pid_t dir, int drlen, void *devreq, int dalen,
 	tds[count].maxlen = maxlen (0);
 	tds[count].counter = 0;	/* as per linux 2.4.10 */
 	tds[count].data_toggle = 1;
-	tds[count].lowspeed = dev->lowspeed, tds[count].bufptr = 0;
+	tds[count].lowspeed = dev->speed;
+	tds[count].bufptr = 0;
 	tds[count].status_active = 1;
 	UHCI_INST (dev->controller)->qh_data->elementlinkptr.ptr =
 		virt_to_phys (tds);
@@ -378,48 +370,6 @@ uhci_control (usbdev_t *dev, pid_t dir, int drlen, void *devreq, int dalen,
 	return result;
 }
 
-static int
-uhci_packet (usbdev_t *dev, int endp, int pid, int toggle, int length,
-	     unsigned char *data)
-{
-	static td_t *td = 0;
-	if (td == 0)
-		td = memalign (16, sizeof (td_t));
-
-	memset (td, 0, sizeof (td_t));
-	td->ptr = 0;
-	td->terminate = 1;
-	td->queue_head = 0;
-
-	td->pid = pid;
-	td->dev_addr = dev->address;
-	td->endp = endp & 0xf;
-	td->maxlen = maxlen (length);
-	if (pid == SETUP)
-		td->counter = 3;
-	else
-		td->counter = 0;
-	td->data_toggle = toggle & 1;
-	td->lowspeed = dev->lowspeed;
-	td->bufptr = virt_to_phys (data);
-
-	td->status_active = 1;
-
-	UHCI_INST (dev->controller)->qh_data->elementlinkptr.ptr =
-		virt_to_phys (td);
-	UHCI_INST (dev->controller)->qh_data->elementlinkptr.queue_head = 0;
-	UHCI_INST (dev->controller)->qh_data->elementlinkptr.terminate = 0;
-	wait_for_completed_td (dev->controller, td);
-	if ((td->status & 0x7f) == 0) {
-		//printf("successfully sent a %x packet to %x.%x\n",pid, dev->address,endp);
-		// success
-		return 0;
-	} else {
-		td_dump (td);
-		return 1;
-	}
-}
-
 static td_t *
 create_schedule (int numpackets)
 {
@@ -454,7 +404,7 @@ fill_schedule (td_t *td, endpoint_t *ep, int length, unsigned char *data,
 	else
 		td->counter = 0;
 	td->data_toggle = *toggle & 1;
-	td->lowspeed = ep->dev->lowspeed;
+	td->lowspeed = ep->dev->speed;
 	td->bufptr = virt_to_phys (data);
 
 	td->status_active = 1;
@@ -484,7 +434,7 @@ uhci_bulk (endpoint_t *ep, int size, u8 *data, int finalize)
 {
 	int maxpsize = ep->maxpacketsize;
 	if (maxpsize == 0)
-		fatal ("MaxPacketSize == 0!!!");
+		usb_fatal ("MaxPacketSize == 0!!!");
 	int numpackets = (size + maxpsize - 1 + finalize) / maxpsize;
 	if (numpackets == 0)
 		return 0;
@@ -498,6 +448,7 @@ uhci_bulk (endpoint_t *ep, int size, u8 *data, int finalize)
 		size -= maxpsize;
 	}
 	if (run_schedule (ep->dev, tds) == 1) {
+		debug("Stalled. Trying to clean up.\n");
 		clear_stall (ep);
 		free (tds);
 		return 1;
@@ -557,7 +508,7 @@ uhci_create_intr_queue (endpoint_t *ep, int reqsize, int reqcount, int reqtiming
 		tds[i].maxlen = maxlen (reqsize);
 		tds[i].counter = 0;
 		tds[i].data_toggle = ep->toggle & 1;
-		tds[i].lowspeed = ep->dev->lowspeed;
+		tds[i].lowspeed = ep->dev->speed;
 		tds[i].bufptr = virt_to_phys (data);
 		tds[i].status_active = 1;
 		ep->toggle ^= 1;
diff --git a/payloads/libpayload/drivers/usb/uhci.h b/payloads/libpayload/drivers/usb/uhci.h
index dd015eea3a..b46068bb40 100644
--- a/payloads/libpayload/drivers/usb/uhci.h
+++ b/payloads/libpayload/drivers/usb/uhci.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2010 coresystems GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/payloads/libpayload/drivers/usb/uhci_rh.c b/payloads/libpayload/drivers/usb/uhci_rh.c
index 2f4c7d839c..3957275bf0 100644
--- a/payloads/libpayload/drivers/usb/uhci_rh.c
+++ b/payloads/libpayload/drivers/usb/uhci_rh.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2010 coresystems GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -27,6 +27,8 @@
  * SUCH DAMAGE.
  */
 
+//#define USB_DEBUG
+
 #include <libpayload.h>
 #include "uhci.h"
 
@@ -43,8 +45,13 @@ uhci_rh_enable_port (usbdev_t *dev, int port)
 	hci_t *controller = dev->controller;
 	if (port == 1)
 		port = PORTSC1;
-	else
+	else if (port == 2)
 		port = PORTSC2;
+	else {
+		printf("Invalid port %d\n", port);
+		return;
+	}
+
 	uhci_reg_mask16 (controller, port, ~(1 << 12), 0);	/* wakeup */
 
 	uhci_reg_mask16 (controller, port, ~0, 1 << 9);	/* reset */
@@ -85,8 +92,10 @@ uhci_rh_scanport (usbdev_t *dev, int port)
 	} else if (port == 2) {
 		portsc = PORTSC2;
 		offset = 1;
-	} else
+	} else {
+		printf("Invalid port %d\n", port);
 		return;
+	}
 	int devno = RH_INST (dev)->port[offset];
 	if ((dev->controller->devices[devno] != 0) && (devno != -1)) {
 		usb_detach_device(dev->controller, devno);
@@ -94,16 +103,17 @@ uhci_rh_scanport (usbdev_t *dev, int port)
 	}
 	uhci_reg_mask16 (dev->controller, portsc, ~0, (1 << 3) | (1 << 2));	// clear port state change, enable port
 
+	mdelay(100); // wait for signal to stabilize
+
 	if ((uhci_reg_read16 (dev->controller, portsc) & 1) != 0) {
 		// device attached
 
 		uhci_rh_disable_port (dev, port);
 		uhci_rh_enable_port (dev, port);
 
-		int lowspeed =
-			(uhci_reg_read16 (dev->controller, portsc) >> 8) & 1;
+		int speed = ((uhci_reg_read16 (dev->controller, portsc) >> 8) & 1);
 
-		RH_INST (dev)->port[offset] = usb_attach_device(dev->controller, dev->address, portsc, lowspeed);
+		RH_INST (dev)->port[offset] = usb_attach_device(dev->controller, dev->address, portsc, speed);
 	}
 }
 
@@ -114,21 +124,30 @@ uhci_rh_report_port_changes (usbdev_t *dev)
 
 	stored = (RH_INST (dev)->port[0] == -1);
 	real = ((uhci_reg_read16 (dev->controller, PORTSC1) & 1) == 0);
-	if (stored != real)
+	if (stored != real) {
+		debug("change on port 1\n");
 		return 1;
+	}
 
 	stored = (RH_INST (dev)->port[1] == -1);
 	real = ((uhci_reg_read16 (dev->controller, PORTSC2) & 1) == 0);
-	if (stored != real)
+	if (stored != real) {
+		debug("change on port 2\n");
 		return 2;
+	}
+
+	// maybe detach+attach happened between two scans?
 
-// maybe detach+attach happened between two scans?
-	if ((uhci_reg_read16 (dev->controller, PORTSC1) & 2) > 0)
+	if ((uhci_reg_read16 (dev->controller, PORTSC1) & 2) > 0) {
+		debug("possibly re-attached on port 1\n");
 		return 1;
-	if ((uhci_reg_read16 (dev->controller, PORTSC2) & 2) > 0)
+	}
+	if ((uhci_reg_read16 (dev->controller, PORTSC2) & 2) > 0) {
+		debug("possibly re-attached on port 2\n");
 		return 2;
+	}
 
-// no change
+	// no change
 	return -1;
 }
 
diff --git a/payloads/libpayload/drivers/usb/usb.c b/payloads/libpayload/drivers/usb/usb.c
index 25e80065d7..421a80ec04 100644
--- a/payloads/libpayload/drivers/usb/usb.c
+++ b/payloads/libpayload/drivers/usb/usb.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2010 coresystems GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -27,6 +27,8 @@
  * SUCH DAMAGE.
  */
 
+//#define USB_DEBUG
+
 #include <libpayload-config.h>
 #include <usb/usb.h>
 
@@ -69,7 +71,7 @@ detach_controller (hci_t *controller)
  * Polls all hubs on all USB controllers, to find out about device changes
  */
 void
-usb_poll ()
+usb_poll (void)
 {
 	if (usb_hcs == 0)
 		return;
@@ -78,8 +80,7 @@ usb_poll ()
 		int i;
 		for (i = 0; i < 128; i++) {
 			if (controller->devices[i] != 0) {
-				controller->devices[i]->poll (controller->
-							     devices[i]);
+				controller->devices[i]->poll (controller->devices[i]);
 			}
 		}
 		controller = controller->next;
@@ -150,7 +151,7 @@ get_descriptor (usbdev_t *dev, unsigned char bmRequestType, int descType,
 
 	if (descType == 1) {
 		device_descriptor_t *dd = (device_descriptor_t *) buf;
-		printf ("maxPacketSize0: %x\n", dd->bMaxPacketSize0);
+		debug ("maxPacketSize0: %x\n", dd->bMaxPacketSize0);
 		if (dd->bMaxPacketSize0 != 0)
 			dev->endpoints[0].maxpacketsize = dd->bMaxPacketSize0;
 	}
@@ -204,6 +205,7 @@ clear_stall (endpoint_t *ep)
 	dr.wIndex = endp;
 	dr.wLength = 0;
 	dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0);
+	ep->toggle = 0;
 	return 0;
 }
 
@@ -221,7 +223,7 @@ get_free_address (hci_t *controller)
 }
 
 int
-set_address (hci_t *controller, int lowspeed)
+set_address (hci_t *controller, int speed)
 {
 	int adr = get_free_address (controller);	// address to set
 	dev_req_t dr;
@@ -241,7 +243,7 @@ set_address (hci_t *controller, int lowspeed)
 	usbdev_t *dev = controller->devices[adr];
 	// dummy values for registering the address
 	dev->address = 0;
-	dev->lowspeed = lowspeed;
+	dev->speed = speed;
 	dev->endpoints[0].dev = dev;
 	dev->endpoints[0].endpoint = 0;
 	dev->endpoints[0].maxpacketsize = 8;
@@ -254,26 +256,25 @@ set_address (hci_t *controller, int lowspeed)
 	}
 	mdelay (50);
 	dev->address = adr;
-	dev->descriptor =
-		get_descriptor (dev,
-				gen_bmRequestType (device_to_host,
-						   standard_type, dev_recp),
-				1, 0, 0);
+	dev->descriptor = get_descriptor (dev, gen_bmRequestType 
+		(device_to_host, standard_type, dev_recp), 1, 0, 0);
 	dd = (device_descriptor_t *) dev->descriptor;
-	printf ("device version: %x.%x\n", dd->bcdUSB >> 8,
-		dd->bcdUSB & 0xff);
-	printf ("device has %x configurations\n", dd->bNumConfigurations);
+
+	printf ("device 0x%04x:0x%04x is USB %x.%x ",
+		 dd->idVendor, dd->idProduct,	
+		 dd->bcdUSB >> 8, dd->bcdUSB & 0xff);
+	dev->quirks = usb_quirk_check(dd->idVendor, dd->idProduct);
+
+	debug ("\ndevice has %x configurations\n", dd->bNumConfigurations);
 	if (dd->bNumConfigurations == 0) {
 		/* device isn't usable */
-		printf ("no usable configuration!\n");
+		printf ("... no usable configuration!\n");
 		dev->address = 0;
 		return -1;
 	}
-	dev->configuration =
-		get_descriptor (dev,
-				gen_bmRequestType (device_to_host,
-						   standard_type, dev_recp),
-				2, 0, 0);
+
+	dev->configuration = get_descriptor (dev, gen_bmRequestType
+		(device_to_host, standard_type, dev_recp), 2, 0, 0);
 	cd = (configuration_descriptor_t *) dev->configuration;
 	set_configuration (dev);
 	interface_descriptor_t *interface =
@@ -282,24 +283,33 @@ set_address (hci_t *controller, int lowspeed)
 		int i;
 		int num = cd->bNumInterfaces;
 		interface_descriptor_t *current = interface;
-		printf ("device has %x interfaces\n", num);
-		if (num>1)
-			printf ("NOTICE: This driver defaults to using the first interface.\n"
-				"This might be the wrong choice and lead to limited functionality\n"
-				"of the device. Please report such a case to coreboot@coreboot.org\n"
-				"as you might be the first.\n");
-		/* we limit to the first interface, as there was no need to
-		   implement something else for the time being. If you need
-		   it, see the SetInterface and GetInterface functions in
-		   the USB specification, and adapt appropriately. */
-		num = (num > 1) ? 1 : num;
+		debug ("device has %x interfaces\n", num);
+		if (num > 1) {
+			int interfaces = usb_interface_check(dd->idVendor, dd->idProduct);
+			if (interfaces) {
+				/* Well known device, don't warn */
+				num = interfaces;
+			} else {
+
+				printf ("\nNOTICE: This driver defaults to using the first interface.\n"
+					"This might be the wrong choice and lead to limited functionality\n"
+					"of the device. Please report such a case to coreboot@coreboot.org\n"
+					"as you might be the first.\n");
+				/* we limit to the first interface, as there was no need to
+				 * implement something else for the time being. If you need
+				 * it, see the SetInterface and GetInterface functions in
+				 * the USB specification, and adapt appropriately.
+				 */
+				num = (num > 1) ? 1 : num;
+			}
+		}
 		for (i = 0; i < num; i++) {
 			int j;
-			printf (" #%x has %x endpoints, interface %x:%x, protocol %x\n", current->bInterfaceNumber, current->bNumEndpoints, current->bInterfaceClass, current->bInterfaceSubClass, current->bInterfaceProtocol);
+			debug (" #%x has %x endpoints, interface %x:%x, protocol %x\n",
+					current->bInterfaceNumber, current->bNumEndpoints, current->bInterfaceClass, current->bInterfaceSubClass, current->bInterfaceProtocol);
 			endpoint_descriptor_t *endp =
 				(endpoint_descriptor_t *) (((char *) current)
-							   +
-							   current->bLength);
+							   + current->bLength);
 			if (interface->bInterfaceClass == 0x3)
 				endp = (endpoint_descriptor_t *) (((char *) endp) + ((char *) endp)[0]);	// ignore HID descriptor
 			memset (dev->endpoints, 0, sizeof (dev->endpoints));
@@ -309,11 +319,12 @@ set_address (hci_t *controller, int lowspeed)
 			dev->endpoints[0].direction = SETUP;
 			dev->endpoints[0].type = CONTROL;
 			for (j = 1; j <= current->bNumEndpoints; j++) {
-				static const char *transfertypes[4] =
-					{ "control", "isochronous", "bulk",
-					"interrupt"
+#ifdef USB_DEBUG
+				static const char *transfertypes[4] = {
+					"control", "isochronous", "bulk", "interrupt"
 				};
-				printf ("   #%x: Endpoint %x (%s), max packet size %x, type %s\n", j, endp->bEndpointAddress & 0x7f, ((endp->bEndpointAddress & 0x80) != 0) ? "in" : "out", endp->wMaxPacketSize, transfertypes[endp->bmAttributes]);
+				debug ("   #%x: Endpoint %x (%s), max packet size %x, type %s\n", j, endp->bEndpointAddress & 0x7f, ((endp->bEndpointAddress & 0x80) != 0) ? "in" : "out", endp->wMaxPacketSize, transfertypes[endp->bmAttributes]);
+#endif
 				endpoint_t *ep =
 					&dev->endpoints[dev->num_endp++];
 				ep->dev = dev;
@@ -330,36 +341,94 @@ set_address (hci_t *controller, int lowspeed)
 			current = (interface_descriptor_t *) endp;
 		}
 	}
+
 	int class = dd->bDeviceClass;
 	if (class == 0)
 		class = interface->bInterfaceClass;
 
-	enum { hid_device = 0x3, msc_device = 0x8, hub_device = 0x9 };
+	enum {
+		audio_device      = 0x01,
+		comm_device       = 0x02,
+		hid_device        = 0x03,
+		physical_device   = 0x05,
+		imaging_device    = 0x06,
+		printer_device    = 0x07,
+		msc_device        = 0x08,
+		hub_device        = 0x09,
+		cdc_device        = 0x0a,
+		ccid_device       = 0x0b,
+		security_device   = 0x0d,
+		video_device      = 0x0e,
+		healthcare_device = 0x0f,
+		diagnostic_device = 0xdc,
+		wireless_device   = 0xe0,
+		misc_device       = 0xef,
+	};
 
-	printf ("device of class %x found\n", class);
-	if (class == hub_device) {
-		printf ("hub found\n");
-#ifdef CONFIG_USB_HUB
-		controller->devices[adr]->init = usb_hub_init;
-#else
-		printf ("support not compiled in\n");
-#endif
-	}
-	if (class == hid_device) {
-		printf ("HID found\n");
+	switch (class) {
+	case audio_device:
+		printf("(Audio)\n");
+		break;
+	case comm_device:
+		printf("(Communication)\n");
+		break;
+	case hid_device:
+		printf ("(HID)\n");
 #ifdef CONFIG_USB_HID
 		controller->devices[adr]->init = usb_hid_init;
 #else
-		printf ("support not compiled in\n");
+		printf ("NOTICE: USB HID support not compiled in\n");
 #endif
-	}
-	if (class == msc_device) {
-		printf ("MSC found\n");
+		break;
+	case physical_device:
+		printf("(Physical)\n");
+		break;
+	case imaging_device:
+		printf("(Camera)\n");
+		break;
+	case printer_device:
+		printf("(Printer)\n");
+		break;
+	case msc_device:
+		printf ("(MSC)\n");
 #ifdef CONFIG_USB_MSC
 		controller->devices[adr]->init = usb_msc_init;
 #else
-		printf ("support not compiled in\n");
+		printf ("NOTICE: USB MSC support not compiled in\n");
+#endif
+		break;
+	case hub_device:
+		printf ("(Hub)\n");
+#ifdef CONFIG_USB_HUB
+		controller->devices[adr]->init = usb_hub_init;
+#else
+		printf ("NOTICE: USB hub support not compiled in.\n");
 #endif
+		break;
+	case cdc_device:
+		printf("(CDC)\n");
+		break;
+	case ccid_device:
+		printf ("(Smart Card / CCID)\n");
+		break;
+	case security_device:
+		printf("(Content Security)\n");
+		break;
+	case video_device:
+		printf("(Video)\n");
+		break;
+	case healthcare_device:
+		printf("(Healthcare)\n");
+		break;
+	case diagnostic_device:
+		printf("(Diagnostic)\n");
+		break;
+	case wireless_device:
+		printf("(Wireless)\n");
+		break;
+	default:
+		printf ("(unsupported class %x)\n", class);
+		break;
 	}
 	return adr;
 }
@@ -373,10 +442,11 @@ usb_detach_device(hci_t *controller, int devno)
 }
 
 int
-usb_attach_device(hci_t *controller, int hubaddress, int port, int lowspeed)
+usb_attach_device(hci_t *controller, int hubaddress, int port, int speed)
 {
-	printf ("%sspeed device\n", (lowspeed == 1) ? "low" : "full");
-	int newdev = set_address (controller, lowspeed);
+	static const char* speeds[] = { "full", "low", "high" };
+	printf ("%sspeed device\n", (speed <= 2) ? speeds[speed] : "invalid value - no");
+	int newdev = set_address (controller, speed);
 	if (newdev == -1)
 		return -1;
 	usbdev_t *newdev_t = controller->devices[newdev];
diff --git a/payloads/libpayload/drivers/usb/usbhid.c b/payloads/libpayload/drivers/usb/usbhid.c
index ee217c36d8..7c638298da 100644
--- a/payloads/libpayload/drivers/usb/usbhid.c
+++ b/payloads/libpayload/drivers/usb/usbhid.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2010 coresystems GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -27,6 +27,8 @@
  * SUCH DAMAGE.
  */
 
+// #define USB_DEBUG
+
 #include <usb/usb.h>
 #include <curses.h>
 
@@ -35,7 +37,9 @@ typedef enum { hid_proto_boot = 0, hid_proto_report = 1 } hid_proto;
 enum { hid_boot_proto_none = 0, hid_boot_proto_keyboard =
 		1, hid_boot_proto_mouse = 2
 };
+#ifdef USB_DEBUG
 static const char *boot_protos[3] = { "(none)", "keyboard", "mouse" };
+#endif
 enum { GET_REPORT = 0x1, GET_IDLE = 0x2, GET_PROTOCOL = 0x3, SET_REPORT =
 		0x9, SET_IDLE = 0xa, SET_PROTOCOL = 0xb
 };
@@ -48,72 +52,295 @@ usb_hid_destroy (usbdev_t *dev)
 
 typedef struct {
 	void* queue;
+	hid_descriptor_t *descriptor;
 } usbhid_inst_t;
 
 #define HID_INST(dev) ((usbhid_inst_t*)(dev)->data)
 
-/* buffer is global to all keyboard drivers */
-int count;
-short keybuffer[16];
+/* keybuffer is global to all USB keyboards */
+static int keycount;
+#define KEYBOARD_BUFFER_SIZE 16
+static short keybuffer[KEYBOARD_BUFFER_SIZE];
+
+char *countries[36][2] = {
+	{ "not supported", "us" },
+	{ "Arabic", "ae" },
+	{ "Belgian", "be" },
+	{ "Canadian-Bilingual", "ca" },
+	{ "Canadian-French", "ca" },
+	{ "Czech Republic", "cz" },
+	{ "Danish", "dk" },
+	{ "Finnish", "fi" },
+	{ "French", "fr" },
+	{ "German", "de" },
+	{ "Greek", "gr" },
+	{ "Hebrew", "il" },
+	{ "Hungary", "hu" },
+	{ "International (ISO)", "iso" },
+	{ "Italian", "it" },
+	{ "Japan (Katakana)", "jp" },
+	{ "Korean", "us" },
+	{ "Latin American", "us" },
+	{ "Netherlands/Dutch", "nl" },
+	{ "Norwegian", "no" },
+	{ "Persian (Farsi)", "ir" },
+	{ "Poland", "pl" },
+	{ "Portuguese", "pt" },
+	{ "Russia", "ru" }, 
+	{ "Slovakia", "sl" },
+	{ "Spanish", "es" },
+	{ "Swedish", "se" },
+	{ "Swiss/French", "ch" },
+	{ "Swiss/German", "ch" },
+	{ "Switzerland", "ch" },
+	{ "Taiwan", "tw" },
+	{ "Turkish-Q", "tr" },
+	{ "UK", "uk" },
+	{ "US", "us" },
+	{ "Yugoslavia", "yu" },
+	{ "Turkish-F", "tr" },
+	/* 36 - 255: Reserved */
+};
+
+
 
-int keypress;
-short keymap[256] = {
+struct layout_maps {
+	char *country;
+	short map[4][0x80];
+};
+
+static struct layout_maps *map;
+
+static struct layout_maps keyboard_layouts[] = {
+// #ifdef CONFIG_PC_KEYBOARD_LAYOUT_US
+{ .country = "us", .map = {
+	{ /* No modifier */
 	-1, -1, -1, -1, 'a', 'b', 'c', 'd',
 	'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l',
-
+	/* 0x10 */
 	'm', 'n', 'o', 'p', 'q', 'r', 's', 't',
 	'u', 'v', 'w', 'x', 'y', 'z', '1', '2',
-
+	/* 0x20 */
 	'3', '4', '5', '6', '7', '8', '9', '0',
 	'\n', '\e', '\b', '\t', ' ', '-', '=', '[',
-
+	/* 0x30 */
 	']', '\\', -1, ';', '\'', '`', ',', '.',
-	'/', -1, KEY_F(1), KEY_F(2), KEY_F(3), KEY_F(4), KEY_F(5), KEY_F(6),
-
-	KEY_F(7), KEY_F(8), KEY_F(9), KEY_F(10), KEY_F(11), KEY_F(12), -1, -1,
+	'/', -1 /* CapsLk */, KEY_F(1), KEY_F(2), KEY_F(3), KEY_F(4), KEY_F(5), KEY_F(6),
+	/* 0x40 */
+	KEY_F(7), KEY_F(8), KEY_F(9), KEY_F(10), KEY_F(11), KEY_F(12), KEY_PRINT, -1 /* ScrLk */,
+	KEY_BREAK, KEY_IC, KEY_HOME, KEY_PPAGE, KEY_DC, KEY_END, KEY_NPAGE, KEY_RIGHT,
+	/* 50 */
+	KEY_LEFT, KEY_DOWN, KEY_UP, -1 /*NumLck*/, '/', '*', '-' /* = ? */, '+',
+	KEY_ENTER, KEY_END, KEY_DOWN, KEY_NPAGE, KEY_LEFT, -1, KEY_RIGHT, KEY_HOME,
+	/* 60 */
+	KEY_UP, KEY_PPAGE, -1, KEY_DC, -1 /* < > | */, -1 /* Win Key Right */, -1, -1,
 	-1, -1, -1, -1, -1, -1, -1, -1,
-/* 50 */
-	-1, -1, -1, -1, -1, '*', '-', '+',
-	-1, KEY_END, KEY_DOWN, KEY_NPAGE, KEY_LEFT, -1, KEY_RIGHT, KEY_HOME,
-
-	KEY_UP, KEY_PPAGE, -1, -1, -1, -1, -1, -1,
+	/* 70 */
 	-1, -1, -1, -1, -1, -1, -1, -1,
-
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	 },
+	{ /* Shift modifier */
+	-1, -1, -1, -1, 'A', 'B', 'C', 'D',
+	'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L',
+	/* 0x10 */
+	'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T',
+	'U', 'V', 'W', 'X', 'Y', 'Z', '!', '@',
+	/* 0x20 */
+	'#', '$', '%', '^', '&', '*', '(', ')',
+	'\n', '\e', '\b', '\t', ' ', '-', '=', '[',
+	/* 0x30 */
+	']', '\\', -1, ':', '\'', '`', ',', '.',
+	'/', -1 /* CapsLk */, KEY_F(1), KEY_F(2), KEY_F(3), KEY_F(4), KEY_F(5), KEY_F(6),
+	/* 0x40 */
+	KEY_F(7), KEY_F(8), KEY_F(9), KEY_F(10), KEY_F(11), KEY_F(12), KEY_PRINT, -1 /* ScrLk */,
+	KEY_BREAK, KEY_IC, KEY_HOME, KEY_PPAGE, KEY_DC, KEY_END, KEY_NPAGE, KEY_RIGHT,
+	/* 50 */
+	KEY_LEFT, KEY_DOWN, KEY_UP, -1 /*NumLck*/, '/', '*', '-' /* = ? */, '+',
+	KEY_ENTER, KEY_END, KEY_DOWN, KEY_NPAGE, KEY_LEFT, -1, KEY_RIGHT, KEY_HOME,
+	/* 60 */
+	KEY_UP, KEY_PPAGE, -1, KEY_DC, -1 /* < > | */, -1 /* Win Key Right */, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	/* 70 */
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	 },
+	{ /* Alt */
+	-1, -1, -1, -1, 'a', 'b', 'c', 'd',
+	'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l',
+	/* 0x10 */
+	'm', 'n', 'o', 'p', 'q', 'r', 's', 't',
+	'u', 'v', 'w', 'x', 'y', 'z', '1', '2',
+	/* 0x20 */
+	'3', '4', '5', '6', '7', '8', '9', '0',
+	'\n', '\e', '\b', '\t', ' ', '-', '=', '[',
+	/* 0x30 */
+	']', '\\', -1, ';', '\'', '`', ',', '.',
+	'/', -1 /* CapsLk */, KEY_F(1), KEY_F(2), KEY_F(3), KEY_F(4), KEY_F(5), KEY_F(6),
+	/* 0x40 */
+	KEY_F(7), KEY_F(8), KEY_F(9), KEY_F(10), KEY_F(11), KEY_F(12), KEY_PRINT, -1 /* ScrLk */,
+	KEY_BREAK, KEY_IC, KEY_HOME, KEY_PPAGE, KEY_DC, KEY_END, KEY_NPAGE, KEY_RIGHT,
+	/* 50 */
+	KEY_LEFT, KEY_DOWN, KEY_UP, -1 /*NumLck*/, '/', '*', '-' /* = ? */, '+',
+	KEY_ENTER, KEY_END, KEY_DOWN, KEY_NPAGE, KEY_LEFT, -1, KEY_RIGHT, KEY_HOME,
+	/* 60 */
+	KEY_UP, KEY_PPAGE, -1, KEY_DC, -1 /* < > | */, -1 /* Win Key Right */, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	/* 70 */
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	 },
+	{ /* Shift+Alt modifier */
+	-1, -1, -1, -1, 'A', 'B', 'C', 'D',
+	'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L',
+	/* 0x10 */
+	'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T',
+	'U', 'V', 'W', 'X', 'Y', 'Z', '!', '@',
+	/* 0x20 */
+	'#', '$', '%', '^', '&', '*', '(', ')',
+	'\n', '\e', '\b', '\t', ' ', '-', '=', '[',
+	/* 0x30 */
+	']', '\\', -1, ':', '\'', '`', ',', '.',
+	'/', -1 /* CapsLk */, KEY_F(1), KEY_F(2), KEY_F(3), KEY_F(4), KEY_F(5), KEY_F(6),
+	/* 0x40 */
+	KEY_F(7), KEY_F(8), KEY_F(9), KEY_F(10), KEY_F(11), KEY_F(12), KEY_PRINT, -1 /* ScrLk */,
+	KEY_BREAK, KEY_IC, KEY_HOME, KEY_PPAGE, KEY_DC, KEY_END, KEY_NPAGE, KEY_RIGHT,
+	/* 50 */
+	KEY_LEFT, KEY_DOWN, KEY_UP, -1 /*NumLck*/, '/', '*', '-' /* = ? */, '+',
+	KEY_ENTER, KEY_END, KEY_DOWN, KEY_NPAGE, KEY_LEFT, -1, KEY_RIGHT, KEY_HOME,
+	/* 60 */
+	KEY_UP, KEY_PPAGE, -1, KEY_DC, -1 /* < > | */, -1 /* Win Key Right */, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	/* 70 */
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1,
+	 }
+}},
+//#endif
 };
 
+#define MOD_SHIFT    (1 << 0)
+#define MOD_ALT      (1 << 1)
+#define MOD_CTRL     (1 << 2)
 
-static void
-usb_hid_poll (usbdev_t *dev)
+static void usb_hid_keyboard_queue(int ch) {
+	/* ignore key presses if buffer full */
+	if (keycount < KEYBOARD_BUFFER_SIZE)
+		keybuffer[keycount++] = ch;
+}
+
+typedef union {
+	struct {
+		u8 modifiers;
+		u8 repeats;
+		u8 keys[6];
+	};
+	u8 buffer[8];
+} usb_hid_keyboard_event_t;
+
+#define KEYBOARD_REPEAT_MS	30
+#define INITIAL_REPEAT_DELAY	10
+#define REPEAT_DELAY		 2
+
+static void 
+usb_hid_process_keyboard_event(usb_hid_keyboard_event_t *current, 
+		usb_hid_keyboard_event_t *previous)
 {
-	u8* buf;
-	while ((buf=dev->controller->poll_intr_queue (HID_INST(dev)->queue))) {
-		// FIXME: manage buf[0]=special keys, too
-		int i;
-		keypress = 0;
-		for (i=2; i<9; i++) {
-			if (buf[i] != 0)
-				keypress = keymap[buf[i]];
-			else
+	int i, keypress = 0, modifiers = 0;
+	static int lastkeypress = 0, repeat_delay = INITIAL_REPEAT_DELAY;
+
+	if (current->modifiers & 0x01) /* Left-Ctrl */   modifiers |= MOD_CTRL;
+	if (current->modifiers & 0x02) /* Left-Shift */  modifiers |= MOD_SHIFT;
+	if (current->modifiers & 0x04) /* Left-Alt */    modifiers |= MOD_ALT;
+	if (current->modifiers & 0x08) /* Left-GUI */    ;
+	if (current->modifiers & 0x10) /* Right-Ctrl */  modifiers |= MOD_CTRL;
+	if (current->modifiers & 0x20) /* Right-Shift */ modifiers |= MOD_SHIFT;
+	if (current->modifiers & 0x40) /* Right-AltGr */ modifiers |= MOD_ALT;
+	if (current->modifiers & 0x80) /* Right-GUI */   ;
+
+	if ((current->modifiers & 0x05) && ((current->keys[0] == 0x4c) ||
+				(current->keys[0]==0x63))) {
+		/* vulcan nerve pinch */
+		if (reset_handler)
+			reset_handler();
+	}
+
+	/* Did the event change at all? */
+	if (lastkeypress && !memcmp(current, previous, sizeof(usb_hid_keyboard_event_t))) {
+		/* No. Then it's a key repeat event. */
+		if (repeat_delay) {
+			repeat_delay--;
+		} else {
+			usb_hid_keyboard_queue(lastkeypress);
+			repeat_delay = REPEAT_DELAY;
+		}
+
+		return;
+	}
+
+	lastkeypress = 0;
+
+	for (i=0; i<6; i++) {
+		int j;
+		int skip = 0;
+		// No more keys? skip
+		if (current->keys[i] == 0)
+			return;
+
+		for (j=0; j<6; j++) {
+			if (current->keys[i] == previous->keys[j]) {
+				skip = 1;
 				break;
+			}
 		}
-		if ((keypress == -1) && (buf[2] != 0)) {
-			printf ("%x %x %x %x %x %x %x %x\n", buf[0], buf[1], buf[2],
-				buf[3], buf[4], buf[5], buf[6], buf[7]);
+		if (skip)
+			continue;
+
+
+		/* Mask off MOD_CTRL */
+		keypress = map->map[modifiers & 0x03][current->keys[i]];
+
+		if (modifiers & MOD_CTRL) {
+			switch (keypress) {
+			case 'a' ... 'z':
+				keypress &= 0x1f;
+				break;
+			default:
+				continue;
+			}
 		}
-		if (keypress != -1) {
-			/* ignore key presses if buffer full */
-			if (count < 16)
-				keybuffer[count++] = keypress;
+
+		if (keypress == -1) {
+			/* Debug: Print unknown keys */
+			debug ("usbhid: <%x> %x [ %x %x %x %x %x %x ] %d\n",
+				current->modifiers, current->repeats,
+			current->keys[0], current->keys[1],
+			current->keys[2], current->keys[3],
+			current->keys[4], current->keys[5], i);
+
+			/* Unknown key? Try next one in the queue */
+			continue;
 		}
+
+		usb_hid_keyboard_queue(keypress);
+
+		/* Remember for authentic key repeat */
+		lastkeypress = keypress;
+		repeat_delay = INITIAL_REPEAT_DELAY;
+	}
+}
+
+static void
+usb_hid_poll (usbdev_t *dev)
+{
+	usb_hid_keyboard_event_t current;
+	static usb_hid_keyboard_event_t previous = {
+		.buffer = { 0, 0, 0, 0, 0, 0, 0, 0}
+	};
+	u8* buf;
+	while ((buf=dev->controller->poll_intr_queue (HID_INST(dev)->queue))) {
+		memcpy(&current.buffer, buf, 8);
+		usb_hid_process_keyboard_event(&current, &previous);
+		previous = current;
 	}
 }
 
@@ -150,6 +377,30 @@ static struct console_input_driver cons = {
 	.getchar = usbhid_getchar
 };
 
+
+int usb_hid_set_layout (char *country)
+{
+	/* FIXME should be per keyboard */
+	int i;
+
+	for (i=0; i<ARRAY_SIZE(keyboard_layouts); i++) {
+		if (strncmp(keyboard_layouts[i].country, country,
+					strlen(keyboard_layouts[i].country)))
+			continue;
+
+		/* Found, changing keyboard layout */
+		map = &keyboard_layouts[i];
+		printf("  Keyboard layout '%s'\n", map->country);
+		return 0;
+	}
+
+	printf("Keyboard layout '%s' not found, using '%s'\n",
+			country, map->country);
+
+	/* Nothing found, not changed */
+	return -1;
+}
+
 void
 usb_hid_init (usbdev_t *dev)
 {
@@ -164,17 +415,34 @@ usb_hid_init (usbdev_t *dev)
 	interface_descriptor_t *interface = (interface_descriptor_t*)(((char *) cd) + cd->bLength);
 
 	if (interface->bInterfaceSubClass == hid_subclass_boot) {
-		printf ("  supports boot interface..\n");
-		printf ("  it's a %s\n",
+		u8 countrycode;
+		debug ("  supports boot interface..\n");
+		debug ("  it's a %s\n",
 			boot_protos[interface->bInterfaceProtocol]);
-		if (interface->bInterfaceProtocol == hid_boot_proto_keyboard) {
+		switch (interface->bInterfaceProtocol) {
+		case hid_boot_proto_keyboard:
 			dev->data = malloc (sizeof (usbhid_inst_t));
 			if (!dev->data)
 				usb_fatal("Not enough memory for USB HID device.\n");
-			printf ("  configuring...\n");
+			debug ("  configuring...\n");
 			usb_hid_set_protocol(dev, interface, hid_proto_boot);
-			usb_hid_set_idle(dev, interface, 0);
-			printf ("  activating...\n");
+			usb_hid_set_idle(dev, interface, KEYBOARD_REPEAT_MS);
+			debug ("  activating...\n");
+
+			HID_INST (dev)->descriptor = 
+				(hid_descriptor_t *)
+					get_descriptor(dev, gen_bmRequestType
+					(device_to_host, standard_type, iface_recp), 
+					0x21, 0, 0);
+			countrycode = HID_INST(dev)->descriptor->bCountryCode;
+			/* 35 countries defined: */
+			if (countrycode > 35)
+				countrycode = 0;
+			printf ("  Keyboard has %s layout (country code %02x)\n", 
+					countries[countrycode][0], countrycode);
+
+			/* Set keyboard layout accordingly */
+			usb_hid_set_layout(countries[countrycode][1]);
 
 			// only add here, because we only support boot-keyboard HID devices
 			dev->destroy = usb_hid_destroy;
@@ -189,24 +457,33 @@ usb_hid_init (usbdev_t *dev)
 					continue;
 				break;
 			}
-			printf ("  found endpoint %x for interrupt-in\n", i);
+			debug ("  found endpoint %x for interrupt-in\n", i);
 			/* 20 buffers of 8 bytes, for every 10 msecs */
 			HID_INST(dev)->queue = dev->controller->create_intr_queue (&dev->endpoints[i], 8, 20, 10);
-			count = 0;
-			printf ("  configuration done.\n");
+			keycount = 0;
+			debug ("  configuration done.\n");
+			break;
+		case hid_boot_proto_mouse:
+			printf("NOTICE: USB mice are not supported.\n");
+			break;
 		}
 	}
 }
 
 int usbhid_havechar (void)
 {
-	return (count != 0);
+	return (keycount != 0);
 }
 
 int usbhid_getchar (void)
 {
-	if (count == 0) return 0;
-	short ret = keybuffer[0];
-	memmove (keybuffer, keybuffer+1, --count);
-	return ret;
+	short ret;
+
+	if (keycount == 0)
+		return 0;
+	ret = keybuffer[0];
+	memmove(keybuffer, keybuffer + 1, --keycount);
+
+	return (int)ret;
 }
+
diff --git a/payloads/libpayload/drivers/usb/usbhub.c b/payloads/libpayload/drivers/usb/usbhub.c
index 0a5d0ebcb5..fcf4f36d5f 100644
--- a/payloads/libpayload/drivers/usb/usbhub.c
+++ b/payloads/libpayload/drivers/usb/usbhub.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2010 coresystems GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -74,9 +74,15 @@ usb_hub_scanport (usbdev_t *dev, int port)
 	mdelay (20);
 
 	get_status (dev, port, DR_PORT, 4, buf);
-	int lowspeed = (buf[0] >> 9) & 1;
 
-	HUB_INST (dev)->ports[port] = usb_attach_device(dev->controller, dev->address, port, lowspeed);
+	/* bit  10  9
+	 *      0   0  full speed
+	 *      0   1  low speed
+	 *      1   0  high speed
+	 */
+	int speed = ((buf[0] >> 9) & 3) ;
+
+	HUB_INST (dev)->ports[port] = usb_attach_device(dev->controller, dev->address, port, speed);
 }
 
 static int
@@ -93,7 +99,7 @@ usb_hub_report_port_changes (usbdev_t *dev)
 			return port;
 	}
 
-// no change
+	// no change
 	return -1;
 }
 
@@ -131,12 +137,8 @@ usb_hub_init (usbdev_t *dev)
 	if (!dev->data)
 		usb_fatal("Not enough memory for USB hub.\n");
 
-	HUB_INST (dev)->descriptor =
-		(hub_descriptor_t *) get_descriptor (dev,
-						     gen_bmRequestType
-						     (device_to_host,
-						      class_type, dev_recp),
-						     0x29, 0, 0);
+	HUB_INST (dev)->descriptor = (hub_descriptor_t *) get_descriptor(dev,
+		gen_bmRequestType(device_to_host, class_type, dev_recp), 0x29, 0, 0);
 	HUB_INST (dev)->num_ports = HUB_INST (dev)->descriptor->bNbrPorts;
 	HUB_INST (dev)->ports =
 		malloc (sizeof (int) * (HUB_INST (dev)->num_ports + 1));
diff --git a/payloads/libpayload/drivers/usb/usbinit.c b/payloads/libpayload/drivers/usb/usbinit.c
index ead98469f7..adaba34602 100644
--- a/payloads/libpayload/drivers/usb/usbinit.c
+++ b/payloads/libpayload/drivers/usb/usbinit.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the libpayload project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2010 coresystems GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -30,6 +30,9 @@
 #include <libpayload-config.h>
 #include <usb/usb.h>
 #include "uhci.h"
+//#include "ohci.h"
+//#include "ehci.h"
+//#include "xhci.h"
 #include <usb/usbdisk.h>
 
 /**
@@ -58,20 +61,19 @@ usb_controller_initialize (int bus, int dev, int func)
 	/* enable busmaster */
 #define PCI_COMMAND 4
 #define PCI_COMMAND_MASTER 4
-	pci_write_config32 (addr, PCI_COMMAND,
-			    pci_read_config32 (addr,
-					       PCI_COMMAND) |
-			    PCI_COMMAND_MASTER);
-
 	if (devclass == 0xc03) {
+		u32 pci_command;
+
+		pci_command =pci_read_config32(addr, PCI_COMMAND);
+		pci_command |= PCI_COMMAND_MASTER;
+		pci_write_config32(addr, PCI_COMMAND, pci_command);
+
 		printf ("%02x:%02x.%x %04x:%04x.%d ", 0, dev, func,
 			pciid >> 16, pciid & 0xFFFF, func);
 		if (prog_if == 0) {
 			printf ("UHCI controller\n");
 #ifdef CONFIG_USB_UHCI
 			uhci_init (addr);
-			usb_poll ();
-			usb_poll ();
 #else
 			printf ("Not supported.\n");
 #endif
@@ -79,7 +81,8 @@ usb_controller_initialize (int bus, int dev, int func)
 		if (prog_if == 0x10) {
 			printf ("OHCI controller\n");
 #ifdef CONFIG_USB_OHCI
-			// ohci_init(addr);
+			//ohci_init(addr);
+			printf ("Not supported.\n");
 #else
 			printf ("Not supported.\n");
 #endif
@@ -88,7 +91,18 @@ usb_controller_initialize (int bus, int dev, int func)
 		if (prog_if == 0x20) {
 			printf ("EHCI controller\n");
 #ifdef CONFIG_USB_EHCI
-			// ehci_init(addr);
+			//ehci_init(addr);
+			printf ("Not supported.\n");
+#else
+			printf ("Not supported.\n");
+#endif
+
+		}
+		if (prog_if == 0x30) {
+			printf ("XHCI controller\n");
+#ifdef CONFIG_USB_XHCI
+			//xhci_init(addr);
+			printf ("Not supported.\n");
 #else
 			printf ("Not supported.\n");
 #endif
@@ -106,10 +120,17 @@ int
 usb_initialize (void)
 {
 	int bus, dev, func;
+	/* EHCI is defined by standards to be at a
+	 * higher function than the USB1 controllers.
+	 * We don't want to init USB1 + devices just to
+	 * "steal" those for USB2, so make sure USB2
+	 * comes first.
+	 */
 	for (bus = 0; bus < 256; bus++)
 		for (dev = 0; dev < 32; dev++)
-			for (func = 0; func < 8; func++)
+			for (func = 7; func >= 0 ; func--)
 				usb_controller_initialize (bus, dev, func);
+	usb_poll();
 	return 0;
 }
 
diff --git a/payloads/libpayload/drivers/usb/usbmsc.c b/payloads/libpayload/drivers/usb/usbmsc.c
index f24bd6d857..5af3330b2f 100644
--- a/payloads/libpayload/drivers/usb/usbmsc.c
+++ b/payloads/libpayload/drivers/usb/usbmsc.c
@@ -40,6 +40,7 @@ enum {
 	msc_subclass_sff8070i = 0x5,
 	msc_subclass_scsitrans = 0x6
 };
+
 static const char *msc_subclass_strings[7] = {
 	"(none)",
 	"RBC",
@@ -96,19 +97,20 @@ typedef struct {
 	unsigned long bCBWCBLength:5;
 	unsigned long:3;
 	unsigned char CBWCB[31 - 15];
-} __attribute__ ((packed))
-     cbw_t;
-
-     typedef struct {
-	     unsigned int dCSWSignature;
-	     unsigned int dCSWTag;
-	     unsigned int dCSWDataResidue;
-	     unsigned char bCSWStatus;
-     } __attribute__ ((packed))
-     csw_t;
-
-     static void
-       reset_transport (usbdev_t *dev)
+} __attribute__ ((packed)) cbw_t;
+
+typedef struct {
+	unsigned int dCSWSignature;
+	unsigned int dCSWTag;
+	unsigned int dCSWDataResidue;
+	unsigned char bCSWStatus;
+} __attribute__ ((packed)) csw_t;
+
+static int
+request_sense (usbdev_t *dev);
+
+static void
+reset_transport (usbdev_t *dev)
 {
 	dev_req_t dr;
 	memset (&dr, 0, sizeof (dr));
@@ -171,7 +173,8 @@ wrap_cbw (cbw_t *cbw, int datalen, cbw_direction dir, const u8 *cmd,
 static void
 get_csw (endpoint_t *ep, csw_t *csw)
 {
-	ep->dev->controller->bulk (ep, sizeof (csw_t), (u8 *) csw, 1);
+	if (ep->dev->controller->bulk (ep, sizeof (csw_t), (u8 *) csw, 1))
+		clear_stall (ep);
 }
 
 static int
@@ -188,21 +191,23 @@ execute_command (usbdev_t *dev, cbw_direction dir, const u8 *cb, int cblen,
 	wrap_cbw (&cbw, buflen, dir, cb, cblen);
 	if (dev->controller->
 	    bulk (MSC_INST (dev)->bulk_out, sizeof (cbw), (u8 *) &cbw, 0)) {
-		clear_stall (MSC_INST (dev)->bulk_out);
+		reset_transport (dev);
 		return 1;
 	}
 	mdelay (10);
-	if (dir == cbw_direction_data_in) {
-		if (dev->controller->
-		    bulk (MSC_INST (dev)->bulk_in, buflen, buf, 0)) {
-			clear_stall (MSC_INST (dev)->bulk_in);
-			return 1;
-		}
-	} else {
-		if (dev->controller->
-		    bulk (MSC_INST (dev)->bulk_out, buflen, buf, 0)) {
-			clear_stall (MSC_INST (dev)->bulk_out);
-			return 1;
+	if (buflen > 0) {
+		if (dir == cbw_direction_data_in) {
+			if (dev->controller->
+			    bulk (MSC_INST (dev)->bulk_in, buflen, buf, 0)) {
+				clear_stall (MSC_INST (dev)->bulk_in);
+				return 1;
+			}
+		} else {
+			if (dev->controller->
+			    bulk (MSC_INST (dev)->bulk_out, buflen, buf, 0)) {
+				clear_stall (MSC_INST (dev)->bulk_out);
+				return 1;
+			}
 		}
 	}
 	get_csw (MSC_INST (dev)->bulk_in, &csw);
@@ -220,6 +225,7 @@ execute_command (usbdev_t *dev, cbw_direction dir, const u8 *cb, int cblen,
 		return 0;
 	}
 	// error "check condition" or reserved error
+	request_sense (dev);
 	return 1;
 }
 
@@ -241,6 +247,27 @@ typedef struct {
 	unsigned char res4;	//5
 } __attribute__ ((packed)) cmdblock6_t;
 
+/**
+ * Like readwrite_blocks, but for soft-sectors of 512b size. Converts the
+ * start and count from 512b units.
+ * Start and count must be aligned so that they match the native
+ * sector size.
+ *
+ * @param dev device to access
+ * @param start first sector to access
+ * @param n number of sectors to access
+ * @param dir direction of access: cbw_direction_data_in == read, cbw_direction_data_out == write
+ * @param buf buffer to read into or write from. Must be at least n*512 bytes
+ * @return 0 on success, 1 on failure
+ */
+int
+readwrite_blocks_512 (usbdev_t *dev, int start, int n,
+	cbw_direction dir, u8 *buf)
+{
+	int blocksize_divider = MSC_INST(dev)->blocksize / 512;
+	return readwrite_blocks (dev, start / blocksize_divider,
+		n / blocksize_divider, dir, buf);
+}
 
 /**
  * Reads or writes a number of sequential blocks on a USB storage device.
@@ -251,7 +278,7 @@ typedef struct {
  * @param start first sector to access
  * @param n number of sectors to access
  * @param dir direction of access: cbw_direction_data_in == read, cbw_direction_data_out == write
- * @param buf buffer to read into or write from. Must be at least n*512 bytes
+ * @param buf buffer to read into or write from. Must be at least n*sectorsize bytes
  * @return 0 on success, 1 on failure
  */
 int
@@ -266,10 +293,26 @@ readwrite_blocks (usbdev_t *dev, int start, int n, cbw_direction dir, u8 *buf)
 		// write
 		cb.command = 0x2a;
 	}
-	cb.block = ntohl (start);
-	cb.numblocks = ntohw (n);
+	cb.block = htonl (start);
+	cb.numblocks = htonw (n);
+		
 	return execute_command (dev, dir, (u8 *) &cb, sizeof (cb), buf,
-				n * 512);
+				n * MSC_INST(dev)->blocksize);
+}
+
+/* Only request it, we don't interpret it.
+   On certain errors, that's necessary to get devices out of
+   a special state called "Contingent Allegiance Condition" */
+static int
+request_sense (usbdev_t *dev)
+{
+	u8 buf[19];
+	cmdblock6_t cb;
+	memset (&cb, 0, sizeof (cb));
+	cb.command = 0x3;
+	
+	return execute_command (dev, cbw_direction_data_in, (u8 *) &cb,
+				sizeof (cb), buf, 19);
 }
 
 static int
@@ -338,17 +381,25 @@ usb_msc_init (usbdev_t *dev)
 	printf ("  it uses %s protocol\n",
 		msc_protocol_strings[interface->bInterfaceProtocol]);
 
-	if ((interface->bInterfaceProtocol != 0x50)
-	    || (interface->bInterfaceSubClass != 6)) {
+
+	if (interface->bInterfaceProtocol != 0x50) {
+		printf ("  Protocol not supported.\n");
+		return;
+	}
+
+	if ((interface->bInterfaceSubClass != 2) &&	// ATAPI 8020
+		(interface->bInterfaceSubClass != 5) &&	// ATAPI 8070
+		(interface->bInterfaceSubClass != 6)) {	// SCSI
 		/* Other protocols, such as ATAPI don't seem to be very popular. looks like ATAPI would be really easy to add, if necessary. */
-		printf ("  Only SCSI over Bulk is supported.\n");
+		printf ("  Interface SubClass not supported.\n");
 		return;
 	}
 
 	dev->data = malloc (sizeof (usbmsc_inst_t));
 	if (!dev->data)
-		usb_fatal("Not enough memory for USB MSC device.\n");
+		usb_fatal ("Not enough memory for USB MSC device.\n");
 
+	MSC_INST (dev)->protocol = interface->bInterfaceSubClass;
 	MSC_INST (dev)->bulk_in = 0;
 	MSC_INST (dev)->bulk_out = 0;
 
@@ -376,10 +427,11 @@ usb_msc_init (usbdev_t *dev)
 	printf ("  has %d luns\n", get_max_luns (dev) + 1);
 
 	printf ("  Waiting for device to become ready... ");
-	timeout = 10;
+	timeout = 30 * 10; /* SCSI/ATA specs say we have to wait up to 30s. Ugh */
 	while (test_unit_ready (dev) && --timeout) {
 		mdelay (100);
-		printf (".");
+		if (!(timeout % 10)) 
+			printf (".");
 	}
 	if (test_unit_ready (dev)) {
 		printf ("timeout. Device not ready. Still trying...\n");