usb: musb: only write CLRDATATOG when appropriate

This is a change similar to what is already in the Linux driver.  We
should only program the CLRDATATOG bit when the current mode indicates
that it is needed.

Signed-off-by: Bryan Wu <bryan.wu@analog.com>
Signed-off-by: Cliff Cai <cliff.cai@analog.com>
Signed-off-by: Mike Frysinger <vapier@gentoo.org>

diff --git a/drivers/usb/musb/musb_hcd.c b/drivers/usb/musb/musb_hcd.c
index af989aa..f38b279 100644 (file)
--- a/drivers/usb/musb/musb_hcd.c
+++ b/drivers/usb/musb/musb_hcd.c
@@ -144,19 +144,28 @@ static void write_toggle(struct usb_device *dev, u8 ep, u8 dir_out)
        u16 csr;
 
        if (dir_out) {
-               if (!toggle)
-                       writew(MUSB_TXCSR_CLRDATATOG, &musbr->txcsr);
-               else {
-                       csr = readw(&musbr->txcsr);
+               csr = readw(&musbr->txcsr);
+               if (!toggle) {
+                       if (csr & MUSB_TXCSR_MODE)
+                               csr = MUSB_TXCSR_CLRDATATOG;
+                       else
+                               csr = 0;
+                       writew(csr, &musbr->txcsr);
+               } else {
                        csr |= MUSB_TXCSR_H_WR_DATATOGGLE;
                        writew(csr, &musbr->txcsr);
                        csr |= (toggle << MUSB_TXCSR_H_DATATOGGLE_SHIFT);
                        writew(csr, &musbr->txcsr);
                }
        } else {
-               if (!toggle)
-                       writew(MUSB_RXCSR_CLRDATATOG, &musbr->rxcsr);
-               else {
+               if (!toggle) {
+                       csr = readw(&musbr->txcsr);
+                       if (csr & MUSB_TXCSR_MODE)
+                               csr = MUSB_RXCSR_CLRDATATOG;
+                       else
+                               csr = 0;
+                       writew(csr, &musbr->rxcsr);
+               } else {
                        csr = readw(&musbr->rxcsr);
                        csr |= MUSB_RXCSR_H_WR_DATATOGGLE;
                        writew(csr, &musbr->rxcsr);