2 files changed, 68 insertions, 34 deletions

diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index e9400e62f171..d9aad6894b3e 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -1138,7 +1138,7 @@ static int qe_ep_tx(struct qe_ep *ep, struct qe_frame *frame)
         }
 }
 
-/* when an bd was transmitted, the function can *
+/* when a bd was transmitted, the function can
  * handle the tx_req, not include ep0           */
 static int txcomplete(struct qe_ep *ep, unsigned char restart)
 {
@@ -1174,7 +1174,7 @@ static int txcomplete(struct qe_ep *ep, unsigned char restart)
         return 0;
 }
 
-/* give a frame and a tx_req,send some data */
+/* give a frame and a tx_req, send some data */
 static int qe_usb_senddata(struct qe_ep *ep, struct qe_frame *frame)
 {
         unsigned int size;
@@ -1797,11 +1797,6 @@ static int qe_ep_set_halt(struct usb_ep *_ep, int value)
                 goto out;
         }
 
-        if (ep->epnum != 0) {
-                status = 0;
-                goto out;
-        }
-
         udc = ep->udc;
         /* Attempt to halt IN ep will fail if any transfer requests
          * are still queue */
@@ -1821,7 +1816,7 @@ static int qe_ep_set_halt(struct usb_ep *_ep, int value)
                 udc->ep0_dir = 0;
         }
 out:
-        dev_vdbg(udc->dev, " %s %s halt stat %d\n", ep->ep.name,
+        dev_vdbg(udc->dev, "%s %s halt stat %d\n", ep->ep.name,
                         value ?  "set" : "clear", status);
 
         return status;
@@ -1953,22 +1948,51 @@ static void ownercomplete(struct usb_ep *_ep, struct usb_request *_req)
         kfree(req);
 }
 
-static void ch9getstatus(struct qe_udc *udc, u16 value, u16 index,
-                         u16 length)
+static void ch9getstatus(struct qe_udc *udc, u8 request_type, u16 value,
+                        u16 index, u16 length)
 {
-        u16 usb_status = 0;     /* fix me to give correct status */
-
+        u16 usb_status = 0;
         struct qe_req *req;
         struct qe_ep *ep;
         int status = 0;
 
         ep = &udc->eps[0];
+        if ((request_type & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
+                /* Get device status */
+                usb_status = 1 << USB_DEVICE_SELF_POWERED;
+        } else if ((request_type & USB_RECIP_MASK) == USB_RECIP_INTERFACE) {
+                /* Get interface status */
+                /* We don't have interface information in udc driver */
+                usb_status = 0;
+        } else if ((request_type & USB_RECIP_MASK) == USB_RECIP_ENDPOINT) {
+                /* Get endpoint status */
+                int pipe = index & USB_ENDPOINT_NUMBER_MASK;
+                struct qe_ep *target_ep = &udc->eps[pipe];
+                u16 usep;
+
+                /* stall if endpoint doesn't exist */
+                if (!target_ep->desc)
+                        goto stall;
+
+                usep = in_be16(&udc->usb_regs->usb_usep[pipe]);
+                if (index & USB_DIR_IN) {
+                        if (target_ep->dir != USB_DIR_IN)
+                                goto stall;
+                        if ((usep & USB_THS_MASK) == USB_THS_STALL)
+                                usb_status = 1 << USB_ENDPOINT_HALT;
+                } else {
+                        if (target_ep->dir != USB_DIR_OUT)
+                                goto stall;
+                        if ((usep & USB_RHS_MASK) == USB_RHS_STALL)
+                                usb_status = 1 << USB_ENDPOINT_HALT;
+                }
+        }
 
         req = container_of(qe_alloc_request(&ep->ep, GFP_KERNEL),
                                         struct qe_req, req);
         req->req.length = 2;
-        req->req.buf = udc->nullbuf;
-        memcpy(req->req.buf, (u8 *)&usb_status, 2);
+        req->req.buf = udc->statusbuf;
+        *(u16 *)req->req.buf = cpu_to_le16(usb_status);
         req->req.status = -EINPROGRESS;
         req->req.actual = 0;
         req->req.complete = ownercomplete;
@@ -1978,10 +2002,11 @@ static void ch9getstatus(struct qe_udc *udc, u16 value, u16 index,
         /* data phase */
         status = qe_ep_queue(&ep->ep, &req->req, GFP_ATOMIC);
 
-        if (status) {
-                dev_err(udc->dev, "Can't respond to getstatus request \n");
-                qe_ep0_stall(udc);
-        }
+        if (status == 0)
+                return;
+stall:
+        dev_err(udc->dev, "Can't respond to getstatus request \n");
+        qe_ep0_stall(udc);
 }
 
 /* only handle the setup request, suppose the device in normal status */
@@ -2007,7 +2032,8 @@ static void setup_received_handle(struct qe_udc *udc,
                 if ((setup->bRequestType & (USB_DIR_IN | USB_TYPE_MASK))
                                         != (USB_DIR_IN | USB_TYPE_STANDARD))
                         break;
-                ch9getstatus(udc, wValue, wIndex, wLength);
+                ch9getstatus(udc, setup->bRequestType, wValue, wIndex,
+                                        wLength);
                 return;
 
         case USB_REQ_SET_ADDRESS:
@@ -2021,7 +2047,7 @@ static void setup_received_handle(struct qe_udc *udc,
         case USB_REQ_CLEAR_FEATURE:
         case USB_REQ_SET_FEATURE:
                 /* Requests with no data phase, status phase from udc */
-                if ((setup->bRequestType &  USB_TYPE_MASK)
+                if ((setup->bRequestType & USB_TYPE_MASK)
                                         != USB_TYPE_STANDARD)
                         break;
 
@@ -2055,7 +2081,7 @@ static void setup_received_handle(struct qe_udc *udc,
                 if (setup->bRequestType & USB_DIR_IN) {
                         udc->ep0_state = DATA_STATE_XMIT;
                         udc->ep0_dir = USB_DIR_IN;
-                } else{
+                } else {
                         udc->ep0_state = DATA_STATE_RECV;
                         udc->ep0_dir = USB_DIR_OUT;
                 }
@@ -2160,13 +2186,11 @@ static int tx_irq(struct qe_udc *udc)
                         bd = ep->c_txbd;
                         if (!(in_be32((u32 __iomem *)bd) & T_R)
                                                 && (in_be32(&bd->buf))) {
-                                /* Disable the TX Interrupt */
-                                /*confirm the transmitted bd*/
+                                /* confirm the transmitted bd */
                                 if (ep->epnum == 0)
                                         res = qe_ep0_txconf(ep);
                                 else
                                         res = qe_ep_txconf(ep);
-                                /* Enable the TX Interrupt */
                         }
                 }
         }
@@ -2205,7 +2229,6 @@ static irqreturn_t qe_udc_irq(int irq, void *_udc)
         irqreturn_t status = IRQ_NONE;
         unsigned long flags;
 
-
         spin_lock_irqsave(&udc->lock, flags);
 
         irq_src = in_be16(&udc->usb_regs->usb_usber) &
@@ -2520,10 +2543,9 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
         udc_controller->gadget.dev.release = qe_udc_release;
         udc_controller->gadget.dev.parent = &ofdev->dev;
 
-
-        /* EP:intialization qe_ep struct */
+        /* initialize qe_ep struct */
         for (i = 0; i < USB_MAX_ENDPOINTS ; i++) {
-                /*because the ep type isn't decide here so
+                /* because the ep type isn't decide here so
                  * qe_ep_init() should be called in ep_enable() */
 
                 /* setup the qe_ep struct and link ep.ep.list
@@ -2536,7 +2558,7 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
         if (ret)
                 goto err2;
 
-        /* create a buf for ZLP send */
+        /* create a buf for ZLP send, need to remain zeroed */
         udc_controller->nullbuf = kzalloc(256, GFP_KERNEL);
         if (udc_controller->nullbuf == NULL) {
                 dev_dbg(udc_controller->dev, "cannot alloc nullbuf\n");
@@ -2544,6 +2566,13 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
                 goto err3;
         }
 
+        /* buffer for data of get_status request */
+        udc_controller->statusbuf = kzalloc(2, GFP_KERNEL);
+        if (udc_controller->statusbuf == NULL) {
+                ret = -ENOMEM;
+                goto err4;
+        }
+
         udc_controller->nullp = virt_to_phys((void *)udc_controller->nullbuf);
         if (udc_controller->nullp == DMA_ADDR_INVALID) {
                 udc_controller->nullp = dma_map_single(
@@ -2568,20 +2597,21 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
         if (ret) {
                 dev_err(udc_controller->dev, "cannot request irq %d err %d \n",
                         udc_controller->usb_irq, ret);
-                goto err4;
+                goto err5;
         }
 
         ret = device_add(&udc_controller->gadget.dev);
         if (ret)
-                goto err5;
+                goto err6;
 
         dev_info(udc_controller->dev,
-                        "QE/CPM USB controller initialized as device\n");
+                        "%s USB controller initialized as device\n",
+                        (udc_controller->soc_type == PORT_QE) ? "QE" : "CPM");
         return 0;
 
-err5:
+err6:
         free_irq(udc_controller->usb_irq, udc_controller);
-err4:
+err5:
         if (udc_controller->nullmap) {
                 dma_unmap_single(udc_controller->gadget.dev.parent,
                         udc_controller->nullp, 256,
@@ -2592,6 +2622,8 @@ err4:
                         udc_controller->nullp, 256,
                                 DMA_TO_DEVICE);
         }
+        kfree(udc_controller->statusbuf);
+err4:
         kfree(udc_controller->nullbuf);
 err3:
         ep = &udc_controller->eps[0];
@@ -2642,6 +2674,7 @@ static int __devexit qe_udc_remove(struct of_device *ofdev)
                         udc_controller->nullp, 256,
                                 DMA_TO_DEVICE);
         }
+        kfree(udc_controller->statusbuf);
         kfree(udc_controller->nullbuf);
 
         ep = &udc_controller->eps[0];
diff --git a/drivers/usb/gadget/fsl_qe_udc.h b/drivers/usb/gadget/fsl_qe_udc.h
index b4c07a22e8e7..31b2710882e4 100644
--- a/drivers/usb/gadget/fsl_qe_udc.h
+++ b/drivers/usb/gadget/fsl_qe_udc.h
@@ -349,6 +349,7 @@ struct qe_udc {
         u32 c_end;
 
         u8 *nullbuf;
+        u8 *statusbuf;
         dma_addr_t nullp;
         u8 nullmap;
         u8 device_address;      /* Device USB address */