usb: dwc3: ep0: implement Set SEL support

This patch implements Set SEL Standard Request
support for dwc3 driver. It needs to issue a command
to the controller passing the timing we received on
the data phase of the Set SEL request.

Signed-off-by: Felipe Balbi <balbi@ti.com>

2 files changed, 93 insertions, 0 deletions

diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 42641ada9e2d..39fbd154dc11 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -575,6 +575,10 @@ struct dwc3_request {
  * @setup_packet_pending: true when there's a Setup Packet in FIFO. Workaround
  * @needs_fifo_resize: not all users might want fifo resizing, flag it
  * @resize_fifos: tells us it's ok to reconfigure our TxFIFO sizes.
+ * @u2sel: parameter from Set SEL request.
+ * @u2pel: parameter from Set SEL request.
+ * @u1sel: parameter from Set SEL request.
+ * @u1pel: parameter from Set SEL request.
  * @ep0_next_event: hold the next expected event
  * @ep0state: state of endpoint zero
  * @link_state: link state
@@ -641,7 +645,13 @@ struct dwc3 {
         enum dwc3_link_state    link_state;
         enum dwc3_device_state  dev_state;
 
+        u16                     u2sel;
+        u16                     u2pel;
+        u8                      u1sel;
+        u8                      u1pel;
+
         u8                      speed;
+
         void                    *mem;
 
         struct dwc3_hwparams    hwparams;
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 18494b0d7d6e..9683d98bbb5d 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -490,6 +490,85 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
         return ret;
 }
 
+static void dwc3_ep0_set_sel_cmpl(struct usb_ep *ep, struct usb_request *req)
+{
+        struct dwc3_ep  *dep = to_dwc3_ep(ep);
+        struct dwc3     *dwc = dep->dwc;
+
+        u32             param = 0;
+        u32             reg;
+
+        struct timing {
+                u8      u1sel;
+                u8      u1pel;
+                u16     u2sel;
+                u16     u2pel;
+        } __packed timing;
+
+        int             ret;
+
+        memcpy(&timing, req->buf, sizeof(timing));
+
+        dwc->u1sel = timing.u1sel;
+        dwc->u1pel = timing.u1pel;
+        dwc->u2sel = timing.u2sel;
+        dwc->u2pel = timing.u2pel;
+
+        reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+        if (reg & DWC3_DCTL_INITU2ENA)
+                param = dwc->u2pel;
+        if (reg & DWC3_DCTL_INITU1ENA)
+                param = dwc->u1pel;
+
+        /*
+         * According to Synopsys Databook, if parameter is
+         * greater than 125, a value of zero should be
+         * programmed in the register.
+         */
+        if (param > 125)
+                param = 0;
+
+        /* now that we have the time, issue DGCMD Set Sel */
+        ret = dwc3_send_gadget_generic_command(dwc,
+                        DWC3_DGCMD_SET_PERIODIC_PAR, param);
+        WARN_ON(ret < 0);
+}
+
+static int dwc3_ep0_set_sel(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
+{
+        struct dwc3_ep  *dep;
+        u16             wLength;
+        u16             wValue;
+
+        if (dwc->dev_state == DWC3_DEFAULT_STATE)
+                return -EINVAL;
+
+        wValue = le16_to_cpu(ctrl->wValue);
+        wLength = le16_to_cpu(ctrl->wLength);
+
+        if (wLength != 6) {
+                dev_err(dwc->dev, "Set SEL should be 6 bytes, got %d\n",
+                                wLength);
+                return -EINVAL;
+        }
+
+        /*
+         * To handle Set SEL we need to receive 6 bytes from Host. So let's
+         * queue a usb_request for 6 bytes.
+         *
+         * Remember, though, this controller can't handle non-wMaxPacketSize
+         * aligned transfers on the OUT direction, so we queue a request for
+         * wMaxPacketSize instead.
+         */
+        dep = dwc->eps[0];
+        dwc->ep0_usb_req.dep = dep;
+        dwc->ep0_usb_req.request.length = dep->endpoint.maxpacket;
+        dwc->ep0_usb_req.request.buf = dwc->setup_buf;
+        dwc->ep0_usb_req.request.complete = dwc3_ep0_set_sel_cmpl;
+
+        return __dwc3_gadget_ep0_queue(dep, &dwc->ep0_usb_req);
+}
+
 static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 {
         int ret;
@@ -515,6 +594,10 @@ static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
                 dev_vdbg(dwc->dev, "USB_REQ_SET_CONFIGURATION\n");
                 ret = dwc3_ep0_set_config(dwc, ctrl);
                 break;
+        case USB_REQ_SET_SEL:
+                dev_vdbg(dwc->dev, "USB_REQ_SET_SEL\n");
+                ret = dwc3_ep0_set_sel(dwc, ctrl);
+                break;
         default:
                 dev_vdbg(dwc->dev, "Forwarding to gadget driver\n");
                 ret = dwc3_ep0_delegate_req(dwc, ctrl);