diff --git a/hw/usb-musb.c b/hw/usb-musb.c
index 09ec5a1..7f15842 100644
--- a/hw/usb-musb.c
+++ b/hw/usb-musb.c
@@ -24,6 +24,7 @@
 #include "qemu-timer.h"
 #include "usb.h"
 #include "irq.h"
+#include "hw.h"
 
 /* Common USB registers */
 #define MUSB_HDRC_FADDR		0x00	/* 8-bit */
@@ -248,6 +249,16 @@
 #define MGC_M_ULPI_REGCTL_COMPLETE	0x02
 #define MGC_M_ULPI_REGCTL_REG		0x01
 
+/* #define MUSB_DEBUG */
+
+#ifdef MUSB_DEBUG
+#define TRACE(fmt,...) fprintf(stderr, "%s@%d: " fmt "\n", __FUNCTION__, \
+                               __LINE__, ##__VA_ARGS__)
+#else
+#define TRACE(...)
+#endif
+
+
 static void musb_attach(USBPort *port, USBDevice *dev);
 
 typedef struct {
@@ -263,7 +274,7 @@
     uint8_t fifosize;
     int timeout[2];	/* Always in microframes */
 
-    uint32_t *buf[2];
+    uint8_t *buf[2];
     int fifolen[2];
     int fifostart[2];
     int fifoaddr[2];
@@ -299,7 +310,7 @@
     int setup_len;
     int session;
 
-    uint32_t buf[0x2000];
+    uint8_t buf[0x8000];
 
         /* Duplicating the world since 2008!...  probably we should have 32
          * logical, single endpoints instead.  */
@@ -774,7 +785,7 @@
     MUSBEndPoint *ep = s->ep + epnum;
     int pid;
     int total, valid = 0;
-
+    TRACE("start %d, len %d",  ep->fifostart[0], ep->fifolen[0] );
     ep->fifostart[0] += ep->fifolen[0];
     ep->fifolen[0] = 0;
 
@@ -789,18 +800,18 @@
     }
 
     /* If the packet is not fully ready yet, wait for a next segment.  */
-    if (epnum && (ep->fifostart[0] << 2) < total)
+    if (epnum && (ep->fifostart[0]) < total)
         return;
 
     if (!valid)
-        total = ep->fifostart[0] << 2;
+        total = ep->fifostart[0];
 
     pid = USB_TOKEN_OUT;
     if (!epnum && (ep->csr[0] & MGC_M_CSR0_H_SETUPPKT)) {
         pid = USB_TOKEN_SETUP;
-        if (total != 8)
-            printf("%s: illegal SETUPPKT length of %i bytes\n",
-                            __FUNCTION__, total);
+        if (total != 8) {
+            TRACE("illegal SETUPPKT length of %i bytes", total);
+        }
         /* Controller should retry SETUP packets three times on errors
          * but it doesn't make sense for us to do that.  */
     }
@@ -817,12 +828,13 @@
     /* If we already have a packet, which didn't fit into the
      * 64 bytes of the FIFO, only move the FIFO start and return. (Obsolete) */
     if (ep->packey[1].pid == USB_TOKEN_IN && ep->status[1] >= 0 &&
-                    (ep->fifostart[1] << 2) + ep->rxcount <
+                    (ep->fifostart[1]) + ep->rxcount <
                     ep->packey[1].len) {
-        ep->fifostart[1] += ep->rxcount >> 2;
+        TRACE("0x%08x, %d",  ep->fifostart[1], ep->rxcount );
+        ep->fifostart[1] += ep->rxcount;
         ep->fifolen[1] = 0;
 
-        ep->rxcount = MIN(ep->packey[0].len - (ep->fifostart[1] << 2),
+        ep->rxcount = MIN(ep->packey[0].len - (ep->fifostart[1]),
                         ep->maxp[1]);
 
         ep->csr[1] &= ~MGC_M_RXCSR_H_REQPKT;
@@ -872,6 +884,36 @@
                     total, musb_rx_packet_complete, 1);
 }
 
+static uint8_t musb_read_fifo(MUSBEndPoint *ep)
+{
+    uint8_t value;
+    if (ep->fifolen[1] >= 64) {
+        /* We have a FIFO underrun */
+        TRACE("EP%d FIFO is now empty, stop reading", ep->epnum);
+        return 0x00000000;
+    }
+    /* In DMA mode clear RXPKTRDY and set REQPKT automatically
+     * (if AUTOREQ is set) */
+
+    ep->csr[1] &= ~MGC_M_RXCSR_FIFOFULL;
+    value=ep->buf[1][ep->fifostart[1] + ep->fifolen[1] ++];
+    TRACE("EP%d 0x%02x, %d", ep->epnum, value, ep->fifolen[1] );
+    return value;
+}
+
+static void musb_write_fifo(MUSBEndPoint *ep, uint8_t value)
+{
+    TRACE("EP%d = %02x", ep->epnum, value);
+    if (ep->fifolen[0] >= 64) {
+        /* We have a FIFO overrun */
+        TRACE("EP%d FIFO exceeded 64 bytes, stop feeding data", ep->epnum);
+        return;
+     }
+
+     ep->buf[0][ep->fifostart[0] + ep->fifolen[0] ++] = value;
+     ep->csr[0] |= MGC_M_TXCSR_FIFONOTEMPTY;
+}
+
 static void musb_ep_frame_cancel(MUSBEndPoint *ep, int dir)
 {
     if (ep->intv_timer[dir])
@@ -895,7 +937,7 @@
         return s->ep[ep].hport[1];
 
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, addr);
+        TRACE("unknown register 0x%02x", addr);
         return 0x00;
     };
 }
@@ -905,6 +947,12 @@
     MUSBState *s = (MUSBState *) opaque;
 
     switch (addr) {
+    case MUSB_HDRC_TXFUNCADDR:
+        s->ep[ep].faddr[0] = value;
+        break;
+    case MUSB_HDRC_RXFUNCADDR:
+        s->ep[ep].faddr[1] = value;
+        break;
     case MUSB_HDRC_TXHUBADDR:
         s->ep[ep].haddr[0] = value;
         break;
@@ -919,7 +967,8 @@
         break;
 
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, addr);
+        TRACE("unknown register 0x%02x", addr);
+        break;
     };
 }
 
@@ -975,9 +1024,11 @@
         return 0x00;
     case MUSB_HDRC_FIFOSIZE:
         return ep ? s->ep[ep].fifosize : s->ep[ep].config;
+    case MUSB_HDRC_RXCOUNT:
+        return s->ep[ep].rxcount;
 
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, addr);
+        TRACE("unknown register 0x%02x", addr);
         return 0x00;
     };
 }
@@ -1004,13 +1055,12 @@
     case (MUSB_HDRC_FIFOSIZE & ~1):
         break;
     case MUSB_HDRC_FIFOSIZE:
-        printf("%s: somebody messes with fifosize (now %i bytes)\n",
-                        __FUNCTION__, value);
+        TRACE("somebody messes with fifosize (now %i bytes)", value);
         s->ep[ep].fifosize = value;
         break;
-
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, addr);
+        TRACE("unknown register 0x%02x", addr);
+        break;
     };
 }
 
@@ -1194,8 +1244,12 @@
         ep = (addr >> 4) & 0xf;
         return musb_ep_readb(s, ep, addr & 0xf);
 
+    case MUSB_HDRC_FIFO ... (MUSB_HDRC_FIFO + 0x3f):
+        ep = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
+        return musb_read_fifo(s->ep + ep);
+
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, (int) addr);
+        TRACE("unknown register 0x%02x", (int) addr);
         return 0x00;
     };
 }
@@ -1276,8 +1330,14 @@
         musb_ep_writeb(s, ep, addr & 0xf, value);
         break;
 
+    case MUSB_HDRC_FIFO ... (MUSB_HDRC_FIFO + 0x3f):
+        ep = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
+        musb_write_fifo(s->ep + ep, value & 0xff);
+        break;
+
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, (int) addr);
+        TRACE("unknown register 0x%02x", (int) addr);
+        break;
     };
 }
 
@@ -1326,6 +1386,10 @@
         ep = (addr >> 4) & 0xf;
         return musb_ep_readh(s, ep, addr & 0xf);
 
+    case MUSB_HDRC_FIFO ... (MUSB_HDRC_FIFO + 0x3f):
+        ep = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
+        return (musb_read_fifo(s->ep + ep) | musb_read_fifo(s->ep + ep) << 8);
+
     default:
         return musb_readb(s, addr) | (musb_readb(s, addr | 1) << 8);
     };
@@ -1353,12 +1417,12 @@
     case MUSB_HDRC_TXFIFOADDR:
         s->ep[s->idx].fifoaddr[0] = value;
         s->ep[s->idx].buf[0] =
-                s->buf + ((value << 1) & (sizeof(s->buf) / 4 - 1));
+                s->buf + ((value << 3) & 0x7ff );
         break;
     case MUSB_HDRC_RXFIFOADDR:
         s->ep[s->idx].fifoaddr[1] = value;
         s->ep[s->idx].buf[1] =
-                s->buf + ((value << 1) & (sizeof(s->buf) / 4 - 1));
+                s->buf + ((value << 3) & 0x7ff);
         break;
 
     case MUSB_HDRC_EP_IDX ... (MUSB_HDRC_EP_IDX + 0xf):
@@ -1375,6 +1439,12 @@
         musb_ep_writeh(s, ep, addr & 0xf, value);
         break;
 
+    case MUSB_HDRC_FIFO ... (MUSB_HDRC_FIFO + 0x3f):
+        ep = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
+        musb_write_fifo(s->ep + ep, value & 0xff);
+        musb_write_fifo(s->ep + ep, (value >> 8) & 0xff);
+        break;
+
     default:
         musb_writeb(s, addr, value & 0xff);
         musb_writeb(s, addr | 1, value >> 8);
@@ -1384,28 +1454,17 @@
 static uint32_t musb_readw(void *opaque, target_phys_addr_t addr)
 {
     MUSBState *s = (MUSBState *) opaque;
-    MUSBEndPoint *ep;
-    int epnum;
+    int ep;
 
     switch (addr) {
     case MUSB_HDRC_FIFO ... (MUSB_HDRC_FIFO + 0x3f):
-        epnum = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
-        ep = s->ep + epnum;
-
-        if (ep->fifolen[1] >= 16) {
-            /* We have a FIFO underrun */
-            printf("%s: EP%i FIFO is now empty, stop reading\n",
-                            __FUNCTION__, epnum);
-            return 0x00000000;
-        }
-        /* In DMA mode clear RXPKTRDY and set REQPKT automatically
-         * (if AUTOREQ is set) */
-
-        ep->csr[1] &= ~MGC_M_RXCSR_FIFOFULL;
-        return ep->buf[1][ep->fifostart[1] + ep->fifolen[1] ++];
-
+        ep = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
+        return ( musb_read_fifo(s->ep + ep)       |
+                 musb_read_fifo(s->ep + ep) << 8  |
+                 musb_read_fifo(s->ep + ep) << 16 |
+                 musb_read_fifo(s->ep + ep) << 24 );
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, (int) addr);
+        TRACE("unknown register 0x%02x", (int) addr);
         return 0x00000000;
     };
 }
@@ -1413,28 +1472,19 @@
 static void musb_writew(void *opaque, target_phys_addr_t addr, uint32_t value)
 {
     MUSBState *s = (MUSBState *) opaque;
-    MUSBEndPoint *ep;
-    int epnum;
+    int ep;
 
     switch (addr) {
     case MUSB_HDRC_FIFO ... (MUSB_HDRC_FIFO + 0x3f):
-        epnum = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
-        ep = s->ep + epnum;
-
-        if (ep->fifolen[0] >= 16) {
-            /* We have a FIFO overrun */
-            printf("%s: EP%i FIFO exceeded 64 bytes, stop feeding data\n",
-                            __FUNCTION__, epnum);
+        ep = ((addr - MUSB_HDRC_FIFO) >> 2) & 0xf;
+        musb_write_fifo(s->ep + ep, value & 0xff);
+        musb_write_fifo(s->ep + ep, (value >> 8 ) & 0xff);
+        musb_write_fifo(s->ep + ep, (value >> 16) & 0xff);
+        musb_write_fifo(s->ep + ep, (value >> 24) & 0xff);
             break;
-        }
-
-        ep->buf[0][ep->fifostart[0] + ep->fifolen[0] ++] = value;
-        if (epnum)
-            ep->csr[0] |= MGC_M_TXCSR_FIFONOTEMPTY;
-        break;
-
     default:
-        printf("%s: unknown register at %02x\n", __FUNCTION__, (int) addr);
+        TRACE("unknown register 0x%02x", (int) addr);
+        break;
     };
 }