[PATCH 28/31] usb: usbssp: implemented usbssp_gadget_ep_disable function.

[Pawel Laszczak <pawell@cadence.com>]

Patch implements function responsible for disabling
USB endpoint. This function is called
from USB core gadget during handling SET_CONFIGURATION or
SET_INTERFACE request, or after such events as USB_RESET or detaching
device from host.

Signed-off-by: Pawel Laszczak <pawell@cadence.com>
---
 drivers/usb/usbssp/gadget-if.c  | 44 +++++++++++++++++--
 drivers/usb/usbssp/gadget-mem.c | 21 ++++++++-
 drivers/usb/usbssp/gadget.c     | 75 +++++++++++++++++++++++++++++++++
 drivers/usb/usbssp/gadget.h     |  2 +
 4 files changed, 137 insertions(+), 5 deletions(-)

diff --git a/drivers/usb/usbssp/gadget-if.c b/drivers/usb/usbssp/gadget-if.c
index afc4f32ec8af..d5ad204612fa 100644
--- a/drivers/usb/usbssp/gadget-if.c
+++ b/drivers/usb/usbssp/gadget-if.c
@@ -86,13 +86,49 @@ static int usbssp_gadget_ep_enable(struct usb_ep *ep,
 
 int usbssp_gadget_ep_disable(struct usb_ep *ep)
 {
-	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
-	int ret = 0;
+	struct usbssp_ep *ep_priv;
+	struct usbssp_udc *usbssp_data;
+	int ep_index = 0;
+	int ret;
+	int irq_disabled_locally = 0;
+	unsigned long flags = 0;
+	struct usbssp_request *req_priv;
+
+	ep_priv = to_usbssp_ep(ep);
+	usbssp_data = ep_priv->usbssp_data;
+	ep_index = usbssp_get_endpoint_index(ep_priv->endpoint.desc);
 
-	if (!ep_priv)
+	if (!(ep_priv->ep_state & USBSSP_EP_ENABLED)) {
+		dev_dbg(usbssp_data->dev, "%s is already disabled\n",
+			ep_priv->name);
 		return -EINVAL;
+	}
+
+	usbssp_g_lock(irq_disabled_locally, flags);
+
+	ep_priv->ep_state |= USBSSP_EP_DISABLE_PENDING;
+
+	/*dequeue all USB request from endpoint*/
+	list_for_each_entry(req_priv, &ep_priv->pending_list, list) {
+		usbssp_dequeue(ep_priv, req_priv);
+	}
+
+	ret = usbssp_drop_endpoint(usbssp_data, &usbssp_data->gadget, ep_priv);
+	if (ret)
+		goto finish;
+
+	ret = usbssp_check_bandwidth(usbssp_data, &usbssp_data->gadget);
+	if (ret)
+		goto finish;
 
-	/*TODO: implements this function*/
+	ep_priv->ep_state &= ~USBSSP_EP_ENABLED;
+
+finish:
+	ep_priv->ep_state &= ~USBSSP_EP_DISABLE_PENDING;
+	dev_dbg(usbssp_data->dev, "%s disable endpoint %s\n", ep_priv->name,
+		(ret == 0) ? "success" : "failed");
+
+	usbssp_g_unlock(irq_disabled_locally, flags);
 	return ret;
 }
 
diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index 8438596ecf48..217ce46bff8b 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -873,7 +873,7 @@ static unsigned int usbssp_microframes_to_exponent(struct usb_gadget *g,
 }
 
 static unsigned int usbssp_parse_microframe_interval(struct usb_gadget *g,
-		struct usbssp_ep *dep)
+						     struct usbssp_ep *dep)
 {
 	if (dep->endpoint.desc->bInterval == 0)
 		return 0;
@@ -1103,6 +1103,25 @@ int usbssp_endpoint_init(struct usbssp_udc *usbssp_data,
 	return 0;
 }
 
+void usbssp_endpoint_zero(struct usbssp_udc *usbssp_data,
+			  struct usbssp_device *dev_priv,
+			  struct usbssp_ep *ep)
+{
+	unsigned int ep_index;
+	struct usbssp_ep_ctx *ep_ctx;
+
+	ep_index = usbssp_get_endpoint_index(ep->endpoint.desc);
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, dev_priv->in_ctx, ep_index);
+
+	ep_ctx->ep_info = 0;
+	ep_ctx->ep_info2 = 0;
+	ep_ctx->deq = 0;
+	ep_ctx->tx_info = 0;
+	/*
+	 * Don't free the endpoint ring until the set interface or configuration
+	 * request succeeds.
+	 */
+}
 struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
 					    bool allocate_completion,
 					    gfp_t mem_flags)
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index f5b0659b6f2d..378828d10a2e 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -715,6 +715,81 @@ int usbssp_dequeue(struct usbssp_ep *ep_priv, struct usbssp_request *req_priv)
 	return ret;
 }
 
+/*
+ * Drop an endpoint from a new bandwidth configuration for this device.
+ * Only one call to this function is allowed per endpoint before
+ * check_bandwidth() or reset_bandwidth() must be called.
+ * A call to usbssp_drop_endpoint() followed by a call to usbssp_add_endpoint()
+ * will add the endpoint to the schedule with possibly new parameters
+ * denoted by a different endpoint descriptor in usbssp_ep.
+ * A call to usbssp_add_endpoint() followed by a call to
+ * usbsssp_drop_endpoint() is not allowed.
+ */
+int usbssp_drop_endpoint(struct usbssp_udc *usbssp_data, struct usb_gadget *g,
+			 struct usbssp_ep *dep)
+{
+	struct usbssp_container_ctx *in_ctx, *out_ctx;
+	struct usbssp_input_control_ctx *ctrl_ctx;
+	unsigned int ep_index;
+	struct usbssp_ep_ctx *ep_ctx;
+	u32 drop_flag;
+	u32 new_add_flags, new_drop_flags;
+	int ret;
+
+	ret = usbssp_check_args(usbssp_data, dep, 1, true, __func__);
+	if (ret <= 0)
+		return ret;
+
+	if (usbssp_data->usbssp_state & USBSSP_STATE_DYING)
+		return -ENODEV;
+
+	drop_flag = usbssp_get_endpoint_flag(dep->endpoint.desc);
+	if (drop_flag == SLOT_FLAG || drop_flag == EP0_FLAG) {
+		dev_dbg(usbssp_data->dev, "USBSSP %s - can't drop slot or ep 0 %#x\n",
+			__func__, drop_flag);
+		return 0;
+	}
+
+	in_ctx = usbssp_data->devs.in_ctx;
+	out_ctx = usbssp_data->devs.out_ctx;
+	ctrl_ctx = usbssp_get_input_control_ctx(in_ctx);
+	if (!ctrl_ctx) {
+		dev_warn(usbssp_data->dev, "%s: Could not get input context, bad type.\n",
+			__func__);
+		return 0;
+	}
+
+	ep_index = usbssp_get_endpoint_index(dep->endpoint.desc);
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, out_ctx, ep_index);
+
+	/*
+	 *If the controller already knows the endpoint is disabled,
+	 * or the USBSSP driver has noted it is disabled, ignore this request
+	 */
+	if ((GET_EP_CTX_STATE(ep_ctx) == EP_STATE_DISABLED) ||
+	    le32_to_cpu(ctrl_ctx->drop_flags) &
+	    usbssp_get_endpoint_flag(dep->endpoint.desc)) {
+		/* Do not warn when called after a usb_device_reset */
+		if (usbssp_data->devs.eps[ep_index].ring != NULL)
+			dev_warn(usbssp_data->dev, "USBSSP %s called with disabled ep %p\n",
+				__func__, dep);
+		return 0;
+	}
+
+	ctrl_ctx->drop_flags |= cpu_to_le32(drop_flag);
+	new_drop_flags = le32_to_cpu(ctrl_ctx->drop_flags);
+
+	ctrl_ctx->add_flags &= cpu_to_le32(~drop_flag);
+	new_add_flags = le32_to_cpu(ctrl_ctx->add_flags);
+
+	usbssp_endpoint_zero(usbssp_data, &usbssp_data->devs, dep);
+
+	dev_dbg(usbssp_data->dev, "drop ep 0x%x, new drop flags = %#x, new add flags = %#x\n",
+		(unsigned int) dep->endpoint.desc->bEndpointAddress,
+		(unsigned int) new_drop_flags,
+		(unsigned int) new_add_flags);
+	return 0;
+}
 
 /**
  * Add an endpoint to a new possible bandwidth configuration for this device.
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index c4075e765dcc..3a223b89efe6 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1694,6 +1694,8 @@ void usbssp_copy_ep0_dequeue_into_input_ctx(struct usbssp_udc *usbssp_data);
 unsigned int usbssp_get_endpoint_index(const struct usb_endpoint_descriptor *desc);
 unsigned int usbssp_get_endpoint_address(unsigned int ep_index);
 unsigned int usbssp_last_valid_endpoint(u32 added_ctxs);
+void usbssp_endpoint_zero(struct usbssp_udc *usbssp_data,
+			struct usbssp_device *dev_priv, struct usbssp_ep *ep);
 int usbssp_endpoint_init(struct usbssp_udc *usbssp_data,
 			struct usbssp_device *dev_priv,
 			struct usbssp_ep *dep,
-- 
2.17.1