diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 0a61c66..0dc56ae 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -178,19 +178,6 @@
 }
 
 /**
- * hw_test_and_clear_setup_status: test & clear setup status (execute without
- *                                 interruption)
- * @n: endpoint number
- *
- * This function returns setup status
- */
-static int hw_test_and_clear_setup_status(struct ci_hdrc *ci, int n)
-{
-	n = ep_to_bit(ci, n);
-	return hw_test_and_clear(ci, OP_ENDPTSETUPSTAT, BIT(n));
-}
-
-/**
  * hw_ep_prime: primes endpoint (execute without interruption)
  * @num:     endpoint number
  * @dir:     endpoint direction
@@ -997,15 +984,11 @@
 			}
 		}
 
-		if (hwep->type != USB_ENDPOINT_XFER_CONTROL ||
-		    !hw_test_and_clear_setup_status(ci, i))
+		/* Only handle setup packet below */
+		if (i != 0 ||
+			!hw_test_and_clear(ci, OP_ENDPTSETUPSTAT, BIT(0)))
 			continue;
 
-		if (i != 0) {
-			dev_warn(ci->dev, "ctrl traffic at endpoint %d\n", i);
-			continue;
-		}
-
 		/*
 		 * Flush data and handshake transactions of previous
 		 * setup packet.
@@ -1193,6 +1176,11 @@
 
 	hwep->qh.ptr->td.next |= cpu_to_le32(TD_TERMINATE);   /* needed? */
 
+	if (hwep->num != 0 && hwep->type == USB_ENDPOINT_XFER_CONTROL) {
+		dev_err(hwep->ci->dev, "Set control xfer at non-ep0\n");
+		retval = -EINVAL;
+	}
+
 	/*
 	 * Enable endpoints in the HW other than ep0 as ep0
 	 * is always enabled