[PATCH 5/6] usb: musb: gadget: implement send_response

[Paul Elder <paul.elder@xxxxxxxxxxxxxxxx>]

This patch implements a mechanism to signal the MUSB driver to reply to
a control OUT request with STALL or ACK.

Signed-off-by: Paul Elder <paul.elder@xxxxxxxxxxxxxxxx>
Reviewed-by: Laurent Pinchart <laurent.pinchart@xxxxxxxxxxxxxxxx>
---
 drivers/usb/musb/musb_gadget_ep0.c | 41 ++++++++++++++++++++++++++++++
 1 file changed, 41 insertions(+)

diff --git a/drivers/usb/musb/musb_gadget_ep0.c b/drivers/usb/musb/musb_gadget_ep0.c
index 91a5027b5c1f..f0ed1f7472a3 100644
--- a/drivers/usb/musb/musb_gadget_ep0.c
+++ b/drivers/usb/musb/musb_gadget_ep0.c
@@ -458,6 +458,25 @@ __acquires(musb->lock)
 	return handled;
 }
 
+static int ep0_send_response(struct musb *musb, bool stall)
+{
+	void __iomem *regs = musb->control_ep->regs;
+	u16 ackpend;
+
+	if (musb->ep0_state != MUSB_EP0_STAGE_RX &&
+	    musb->ep0_state != MUSB_EP0_STAGE_STATUSIN)
+		return -EINVAL;
+
+	ackpend = MUSB_CSR0_P_DATAEND
+		| MUSB_CSR0_P_SVDRXPKTRDY
+		| (stall ? MUSB_CSR0_P_SENDSTALL : 0);
+
+	musb_ep_select(musb->mregs, 0);
+	musb_writew(regs, MUSB_CSR0, ackpend);
+
+	return 0;
+}
+
 /* we have an ep0out data packet
  * Context:  caller holds controller lock
  */
@@ -466,10 +485,13 @@ static void ep0_rxstate(struct musb *musb)
 	void __iomem		*regs = musb->control_ep->regs;
 	struct musb_request	*request;
 	struct usb_request	*req;
+	struct usb_ep		*ep;
 	u16			count, csr;
+	bool			last_packet = false;
 
 	request = next_ep0_request(musb);
 	req = &request->request;
+	ep = &request->ep->end_point;
 
 	/* read packet and ack; or stall because of gadget driver bug:
 	 * should have provided the rx buffer before setup() returned.
@@ -492,6 +514,7 @@ static void ep0_rxstate(struct musb *musb)
 		if (count < 64 || req->actual == req->length) {
 			musb->ep0_state = MUSB_EP0_STAGE_STATUSIN;
 			csr |= MUSB_CSR0_P_DATAEND;
+			last_packet = true;
 		} else
 			req = NULL;
 	} else
@@ -508,6 +531,10 @@ static void ep0_rxstate(struct musb *musb)
 			return;
 		musb->ackpend = 0;
 	}
+
+	if (last_packet && ep->delayed_status)
+		return;
+
 	musb_ep_select(musb->mregs, 0);
 	musb_writew(regs, MUSB_CSR0, csr);
 }
@@ -991,6 +1018,19 @@ static int musb_g_ep0_dequeue(struct usb_ep *ep, struct usb_request *req)
 	return -EINVAL;
 }
 
+static int musb_g_ep0_send_response(struct usb_ep *e, bool stall)
+{
+	struct musb_ep *ep = to_musb_ep(e);
+	struct musb *musb = ep->musb;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&musb->lock, flags);
+	ret = ep0_send_response(musb, stall);
+	spin_unlock_irqrestore(&musb->lock, flags);
+	return ret;
+}
+
 static int musb_g_ep0_halt(struct usb_ep *e, int value)
 {
 	struct musb_ep		*ep;
@@ -1059,4 +1099,5 @@ const struct usb_ep_ops musb_g_ep0_ops = {
 	.queue		= musb_g_ep0_queue,
 	.dequeue	= musb_g_ep0_dequeue,
 	.set_halt	= musb_g_ep0_halt,
+	.send_response  = musb_g_ep0_send_response,
 };
-- 
2.18.0