1 files changed, 21 insertions, 17 deletions

diff --git a/drivers/usb/gadget/m66592-udc.h b/drivers/usb/gadget/m66592-udc.h
index b85e05b2e97b..7b93d579af37 100644
--- a/drivers/usb/gadget/m66592-udc.h
+++ b/drivers/usb/gadget/m66592-udc.h
@@ -561,11 +561,26 @@ static inline void m66592_write(struct m66592 *m66592, u16 val,
         iowrite16(val, m66592->reg + offset);
 }
 
+static inline void m66592_mdfy(struct m66592 *m66592, u16 val, u16 pat,
+                unsigned long offset)
+{
+        u16 tmp;
+        tmp = m66592_read(m66592, offset);
+        tmp = tmp & (~pat);
+        tmp = tmp | val;
+        m66592_write(m66592, tmp, offset);
+}
+
+#define m66592_bclr(m66592, val, offset)        \
+                        m66592_mdfy(m66592, 0, val, offset)
+#define m66592_bset(m66592, val, offset)        \
+                        m66592_mdfy(m66592, val, 0, offset)
+
 static inline void m66592_write_fifo(struct m66592 *m66592,
-                unsigned long offset,
+                struct m66592_ep *ep,
                 void *buf, unsigned long len)
 {
-        void __iomem *fifoaddr = m66592->reg + offset;
+        void __iomem *fifoaddr = m66592->reg + ep->fifoaddr;
 
         if (m66592->pdata->on_chip) {
                 unsigned long count;
@@ -591,26 +606,15 @@ static inline void m66592_write_fifo(struct m66592 *m66592,
                 iowrite16_rep(fifoaddr, buf, len);
                 if (odd) {
                         unsigned char *p = buf + len*2;
+                        if (m66592->pdata->wr0_shorted_to_wr1)
+                                m66592_bclr(m66592, M66592_MBW_16, ep->fifosel);
                         iowrite8(*p, fifoaddr);
+                        if (m66592->pdata->wr0_shorted_to_wr1)
+                                m66592_bset(m66592, M66592_MBW_16, ep->fifosel);
                 }
         }
 }
 
-static inline void m66592_mdfy(struct m66592 *m66592, u16 val, u16 pat,
-                unsigned long offset)
-{
-        u16 tmp;
-        tmp = m66592_read(m66592, offset);
-        tmp = tmp & (~pat);
-        tmp = tmp | val;
-        m66592_write(m66592, tmp, offset);
-}
-
-#define m66592_bclr(m66592, val, offset)        \
-                        m66592_mdfy(m66592, 0, val, offset)
-#define m66592_bset(m66592, val, offset)        \
-                        m66592_mdfy(m66592, val, 0, offset)
-
 #endif  /* ifndef __M66592_UDC_H__ */