1 files changed, 62 insertions, 43 deletions

diff --git a/drivers/usb/gadget/r8a66597-udc.h b/drivers/usb/gadget/r8a66597-udc.h
index 03087e7b9190..9a537aa07968 100644
--- a/drivers/usb/gadget/r8a66597-udc.h
+++ b/drivers/usb/gadget/r8a66597-udc.h
@@ -131,31 +131,48 @@ static inline u16 r8a66597_read(struct r8a66597 *r8a66597, unsigned long offset)
 }
 
 static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
-                                      unsigned long offset, u16 *buf,
+                                      unsigned long offset,
+                                      unsigned char *buf,
                                       int len)
 {
+        unsigned long fifoaddr = r8a66597->reg + offset;
+        unsigned int data;
+        int i;
+
         if (r8a66597->pdata->on_chip) {
-                unsigned long fifoaddr = r8a66597->reg + offset;
-                unsigned long count;
-                union {
-                        unsigned long dword;
-                        unsigned char byte[4];
-                } data;
-                unsigned char *pb;
-                int i;
-
-                count = len / 4;
-                insl(fifoaddr, buf, count);
-
-                if (len & 0x00000003) {
-                        data.dword = inl(fifoaddr);
-                        pb = (unsigned char *)buf + count * 4;
-                        for (i = 0; i < (len & 0x00000003); i++)
-                                pb[i] = data.byte[i];
+                /* 32-bit accesses for on_chip controllers */
+
+                /* aligned buf case */
+                if (len >= 4 && !((unsigned long)buf & 0x03)) {
+                        insl(fifoaddr, buf, len / 4);
+                        buf += len & ~0x03;
+                        len &= 0x03;
+                }
+
+                /* unaligned buf case */
+                for (i = 0; i < len; i++) {
+                        if (!(i & 0x03))
+                                data = inl(fifoaddr);
+
+                        buf[i] = (data >> ((i & 0x03) * 8)) & 0xff;
                 }
         } else {
-                len = (len + 1) / 2;
-                insw(r8a66597->reg + offset, buf, len);
+                /* 16-bit accesses for external controllers */
+
+                /* aligned buf case */
+                if (len >= 2 && !((unsigned long)buf & 0x01)) {
+                        insw(fifoaddr, buf, len / 2);
+                        buf += len & ~0x01;
+                        len &= 0x01;
+                }
+
+                /* unaligned buf case */
+                for (i = 0; i < len; i++) {
+                        if (!(i & 0x01))
+                                data = inw(fifoaddr);
+
+                        buf[i] = (data >> ((i & 0x01) * 8)) & 0xff;
+                }
         }
 }
 
@@ -166,38 +183,40 @@ static inline void r8a66597_write(struct r8a66597 *r8a66597, u16 val,
 }
 
 static inline void r8a66597_write_fifo(struct r8a66597 *r8a66597,
-                                       unsigned long offset, u16 *buf,
+                                       unsigned long offset,
+                                       unsigned char *buf,
                                        int len)
 {
         unsigned long fifoaddr = r8a66597->reg + offset;
+        int adj = 0;
+        int i;
 
         if (r8a66597->pdata->on_chip) {
-                unsigned long count;
-                unsigned char *pb;
-                int i;
-
-                count = len / 4;
-                outsl(fifoaddr, buf, count);
-
-                if (len & 0x00000003) {
-                        pb = (unsigned char *)buf + count * 4;
-                        for (i = 0; i < (len & 0x00000003); i++) {
-                                if (r8a66597_read(r8a66597, CFIFOSEL) & BIGEND)
-                                        outb(pb[i], fifoaddr + i);
-                                else
-                                        outb(pb[i], fifoaddr + 3 - i);
-                        }
+                /* 32-bit access only if buf is 32-bit aligned */
+                if (len >= 4 && !((unsigned long)buf & 0x03)) {
+                        outsl(fifoaddr, buf, len / 4);
+                        buf += len & ~0x03;
+                        len &= 0x03;
                 }
         } else {
-                int odd = len & 0x0001;
-
-                len = len / 2;
-                outsw(fifoaddr, buf, len);
-                if (unlikely(odd)) {
-                        buf = &buf[len];
-                        outb((unsigned char)*buf, fifoaddr);
+                /* 16-bit access only if buf is 16-bit aligned */
+                if (len >= 2 && !((unsigned long)buf & 0x01)) {
+                        outsw(fifoaddr, buf, len / 2);
+                        buf += len & ~0x01;
+                        len &= 0x01;
                 }
         }
+
+        /* adjust fifo address in the little endian case */
+        if (!(r8a66597_read(r8a66597, CFIFOSEL) & BIGEND)) {
+                if (r8a66597->pdata->on_chip)
+                        adj = 0x03; /* 32-bit wide */
+                else
+                        adj = 0x01; /* 16-bit wide */
+        }
+
+        for (i = 0; i < len; i++)
+                outb(buf[i], fifoaddr + adj - (i & adj));
 }
 
 static inline void r8a66597_mdfy(struct r8a66597 *r8a66597,