usb: mtu3: add a new function to do status stage

Exact a new static function to do status stage

Signed-off-by: Chunfeng Yun <chunfeng.yun@mediatek.com>
Link: https://lore.kernel.org/r/1570611900-7112-1-git-send-email-chunfeng.yun@mediatek.com
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/usb/mtu3/mtu3_gadget_ep0.c b/drivers/usb/mtu3/mtu3_gadget_ep0.c
index 4da216c997263b7d2fdd3493178a6302e7c2f44b..df3fd055792f64ec5434d6d8abee7161b9255ec2 100644 (file)
--- a/drivers/usb/mtu3/mtu3_gadget_ep0.c
+++ b/drivers/usb/mtu3/mtu3_gadget_ep0.c
@@ -153,6 +153,15 @@ static void ep0_stall_set(struct mtu3_ep *mep0, bool set, u32 pktrdy)
                set ? "SEND" : "CLEAR", decode_ep0_state(mtu));
 }
 
+static void ep0_do_status_stage(struct mtu3 *mtu)
+{
+       void __iomem *mbase = mtu->mac_base;
+       u32 value;
+
+       value = mtu3_readl(mbase, U3D_EP0CSR) & EP0_W1C_BITS;
+       mtu3_writel(mbase, U3D_EP0CSR, value | EP0_SETUPPKTRDY | EP0_DATAEND);
+}
+
 static int ep0_queue(struct mtu3_ep *mep0, struct mtu3_request *mreq);
 
 static void ep0_dummy_complete(struct usb_ep *ep, struct usb_request *req)
@@ -297,8 +306,7 @@ static int handle_test_mode(struct mtu3 *mtu, struct usb_ctrlrequest *setup)
                ep0_load_test_packet(mtu);
 
        /* send status before entering test mode. */
-       value = mtu3_readl(mbase, U3D_EP0CSR) & EP0_W1C_BITS;
-       mtu3_writel(mbase, U3D_EP0CSR, value | EP0_SETUPPKTRDY | EP0_DATAEND);
+       ep0_do_status_stage(mtu);
 
        /* wait for ACK status sent by host */
        readl_poll_timeout_atomic(mbase + U3D_EP0CSR, value,
@@ -632,7 +640,6 @@ __acquires(mtu->lock)
 {
        struct usb_ctrlrequest setup;
        struct mtu3_request *mreq;
-       void __iomem *mbase = mtu->mac_base;
        int handled = 0;
 
        ep0_read_setup(mtu, &setup);
@@ -668,10 +675,7 @@ finish:
                mtu->delayed_status = true;
        } else if (le16_to_cpu(setup.wLength) == 0) { /* no data stage */
 
-               mtu3_writel(mbase, U3D_EP0CSR,
-                       (mtu3_readl(mbase, U3D_EP0CSR) & EP0_W1C_BITS)
-                       | EP0_SETUPPKTRDY | EP0_DATAEND);
-
+               ep0_do_status_stage(mtu);
                /* complete zlp request directly */
                mreq = next_ep0_request(mtu);
                if (mreq && !mreq->request.length)
@@ -802,12 +806,9 @@ static int ep0_queue(struct mtu3_ep *mep, struct mtu3_request *mreq)
        }
 
        if (mtu->delayed_status) {
-               u32 csr;
 
                mtu->delayed_status = false;
-               csr = mtu3_readl(mtu->mac_base, U3D_EP0CSR) & EP0_W1C_BITS;
-               csr |= EP0_SETUPPKTRDY | EP0_DATAEND;
-               mtu3_writel(mtu->mac_base, U3D_EP0CSR, csr);
+               ep0_do_status_stage(mtu);
                /* needn't giveback the request for handling delay STATUS */
                return 0;
        }