[PATCH 27/31] usb: usbssp: implements usbssp_gadget_ep_enable function

[Pawel Laszczak]

Patch implements function responsible for enabling
and configuring USB endpoint. This function is called
from USB core gadget during handling SET_CONFIGURATION
and SET_INTERFACE request.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-if.c  |  41 ++++-
 drivers/usb/usbssp/gadget-mem.c | 286 ++++++++++++++++++++++++++++++++
 drivers/usb/usbssp/gadget.c     | 271 +++++++++++++++++++++++++++++-
 drivers/usb/usbssp/gadget.h     |  10 ++
 4 files changed, 604 insertions(+), 4 deletions(-)

diff --git a/drivers/usb/usbssp/gadget-if.c b/drivers/usb/usbssp/gadget-if.c
index 376e03b7ef1f..afc4f32ec8af 100644
--- a/drivers/usb/usbssp/gadget-if.c
+++ b/drivers/usb/usbssp/gadget-if.c
@@ -39,13 +39,48 @@
 static int usbssp_gadget_ep_enable(struct usb_ep *ep,
 				   const struct usb_endpoint_descriptor *desc)
 {
-	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
+	struct usbssp_ep *ep_priv;
+	struct usbssp_udc *usbssp_data;
 	int ret = 0;
+	int irq_disabled_locally = 0;
+	unsigned long flags = 0;
 
-	if (!ep_priv)
+	if (!ep || !desc || desc->bDescriptorType != USB_DT_ENDPOINT) {
+		pr_err("invalid parameters\n");
 		return -EINVAL;
+	}
 
-	/*TODO: implements this function*/
+	ep_priv = to_usbssp_ep(ep);
+	usbssp_data = ep_priv->usbssp_data;
+
+	if (!desc->wMaxPacketSize) {
+		dev_dbg(usbssp_data->dev, "missing wMaxPacketSize\n");
+		return -EINVAL;
+	}
+
+	if (ep_priv->ep_state & USBSSP_EP_ENABLED) {
+		dev_dbg(usbssp_data->dev, "%s is already enabled\n",
+			ep_priv->name);
+		return -EINVAL;
+	}
+
+	usbssp_g_lock(irq_disabled_locally, flags);
+	ret = usbssp_add_endpoint(usbssp_data, ep_priv);
+	if (ret < 0)
+		goto finish;
+
+	ep_priv->ep_state |= USBSSP_EP_ENABLED;
+
+	/*Update bandwidth information*/
+	ret = usbssp_check_bandwidth(usbssp_data, &usbssp_data->gadget);
+
+	if (ret < 0)
+		ep_priv->ep_state &= ~USBSSP_EP_ENABLED;
+
+finish:
+	dev_dbg(usbssp_data->dev, "%s enable 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 d5d732b94454..8438596ecf48 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -817,6 +817,292 @@ int usbssp_setup_addressable_priv_dev(struct usbssp_udc *usbssp_data)
 	return 0;
 }
 
+/*
+ * Convert interval expressed as 2^(bInterval - 1) == interval into
+ * straight exponent value 2^n == interval.
+ *
+ */
+static unsigned int usbssp_parse_exponent_interval(struct usb_gadget *g,
+						   struct usbssp_ep *dep)
+{
+	unsigned int interval;
+
+	interval = clamp_val(dep->endpoint.desc->bInterval, 1, 16) - 1;
+	if (interval != dep->endpoint.desc->bInterval - 1)
+		dev_warn(&g->dev,
+			"ep %#x - rounding interval to %d %sframes\n",
+			dep->endpoint.desc->bEndpointAddress,
+			1 << interval,
+			g->speed == USB_SPEED_FULL ? "" : "micro");
+
+	if (g->speed == USB_SPEED_FULL) {
+		/*
+		 * Full speed isoc endpoints specify interval in frames,
+		 * not microframes. We are using microframes everywhere,
+		 * so adjust accordingly.
+		 */
+		interval += 3;	/* 1 frame = 2^3 uframes */
+	}
+
+	return interval;
+}
+
+/*
+ * Convert bInterval expressed in microframes (in 1-255 range) to exponent of
+ * microframes, rounded down to nearest power of 2.
+ */
+static unsigned int usbssp_microframes_to_exponent(struct usb_gadget *g,
+						   struct usbssp_ep *dep,
+						   unsigned int desc_interval,
+						   unsigned int min_exponent,
+						   unsigned int max_exponent)
+{
+	unsigned int interval;
+
+	interval = fls(desc_interval) - 1;
+	interval = clamp_val(interval, min_exponent, max_exponent);
+	if ((1 << interval) != desc_interval)
+		dev_dbg(&g->dev,
+			"ep %#x - rounding interval to %d microframes,"
+			"ep desc says %d microframes\n",
+			dep->endpoint.desc->bEndpointAddress,
+			1 << interval,
+			desc_interval);
+
+	return interval;
+}
+
+static unsigned int usbssp_parse_microframe_interval(struct usb_gadget *g,
+		struct usbssp_ep *dep)
+{
+	if (dep->endpoint.desc->bInterval == 0)
+		return 0;
+	return usbssp_microframes_to_exponent(g, dep,
+					dep->endpoint.desc->bInterval, 0, 15);
+}
+
+
+static unsigned int usbssp_parse_frame_interval(struct usb_gadget *g,
+						struct usbssp_ep *dep)
+{
+
+	return usbssp_microframes_to_exponent(g, dep,
+					dep->endpoint.desc->bInterval * 8,
+					3, 10);
+}
+
+/*
+ * Return the polling or NAK interval.
+ *
+ * The polling interval is expressed in "microframes". If DC's Interval field
+ * is set to N, it will service the endpoint every 2^(Interval)*125us.
+ *
+ * The NAK interval is one NAK per 1 to 255 microframes, or no NAKs if interval
+ * is set to 0.
+ */
+static unsigned int usbssp_get_endpoint_interval(struct usb_gadget *g,
+						 struct usbssp_ep *dep)
+{
+	unsigned int interval = 0;
+
+	switch (g->speed) {
+	case USB_SPEED_HIGH:
+		/* Max NAK rate */
+		if (usb_endpoint_xfer_control(dep->endpoint.desc) ||
+		    usb_endpoint_xfer_bulk(dep->endpoint.desc)) {
+			interval = usbssp_parse_microframe_interval(g, dep);
+			break;
+		}
+		/* Fall through - SS and HS isoc/int have same decoding */
+	case USB_SPEED_SUPER_PLUS:
+	case USB_SPEED_SUPER:
+		if (usb_endpoint_xfer_int(dep->endpoint.desc) ||
+		    usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
+			interval = usbssp_parse_exponent_interval(g, dep);
+		}
+		break;
+
+	case USB_SPEED_FULL:
+		if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
+			interval = usbssp_parse_exponent_interval(g, dep);
+			break;
+		}
+		/*
+		 * Fall through for interrupt endpoint interval decoding
+		 * since it uses the same rules as low speed interrupt
+		 * endpoints.
+		 */
+	case USB_SPEED_LOW:
+		if (usb_endpoint_xfer_int(dep->endpoint.desc) ||
+		    usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
+			interval = usbssp_parse_frame_interval(g, dep);
+		}
+		break;
+
+	default:
+		BUG();
+	}
+	return interval;
+}
+
+/*
+ * The "Mult" field in the endpoint context is only set for SuperSpeed isoc eps.
+ * High speed endpoint descriptors can define "the number of additional
+ * transaction opportunities per microframe", but that goes in the Max Burst
+ * endpoint context field.
+ */
+static u32 usbssp_get_endpoint_mult(struct usb_gadget *g,
+				    struct usbssp_ep *dep)
+{
+	if (g->speed < USB_SPEED_SUPER ||
+	    !usb_endpoint_xfer_isoc(dep->endpoint.desc))
+		return 0;
+
+	return dep->endpoint.comp_desc->bmAttributes;
+}
+
+static u32 usbssp_get_endpoint_max_burst(struct usb_gadget *g,
+					 struct usbssp_ep *dep)
+{
+	/* Super speed and Plus have max burst in ep companion desc */
+	if (g->speed >= USB_SPEED_SUPER)
+		return dep->endpoint.comp_desc->bMaxBurst;
+
+	if (g->speed == USB_SPEED_HIGH &&
+	   (usb_endpoint_xfer_isoc(dep->endpoint.desc) ||
+	    usb_endpoint_xfer_int(dep->endpoint.desc)))
+		return (usb_endpoint_maxp(dep->endpoint.desc) & 0x1800) >> 11;
+
+	return 0;
+}
+
+static u32 usbssp_get_endpoint_type(const struct usb_endpoint_descriptor *desc)
+{
+	int in;
+
+	in = usb_endpoint_dir_in(desc);
+
+	switch (usb_endpoint_type(desc)) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		return CTRL_EP;
+	case USB_ENDPOINT_XFER_BULK:
+		return in ? BULK_IN_EP : BULK_OUT_EP;
+	case USB_ENDPOINT_XFER_ISOC:
+		return in ? ISOC_IN_EP : ISOC_OUT_EP;
+	case USB_ENDPOINT_XFER_INT:
+		return in ? INT_IN_EP : INT_OUT_EP;
+	}
+	return 0;
+}
+
+/*
+ * Return the maximum endpoint service interval time (ESIT) payload.
+ * Basically, this is the maxpacket size, multiplied by the burst size
+ * and mult size.
+ */
+static u32 usbssp_get_max_esit_payload(struct usb_gadget *g,
+				       struct usbssp_ep *dep)
+{
+	int max_burst;
+	int max_packet;
+
+	/* Only applies for interrupt or isochronous endpoints*/
+	if (usb_endpoint_xfer_control(dep->endpoint.desc) ||
+	    usb_endpoint_xfer_bulk(dep->endpoint.desc))
+		return 0;
+
+	/* SuperSpeedPlus Isoc ep sending over 48k per esit*/
+
+	if ((g->speed >= USB_SPEED_SUPER_PLUS) &&
+	    USB_SS_SSP_ISOC_COMP(dep->endpoint.desc->bmAttributes))
+		return le32_to_cpu(dep->endpoint.comp_desc->wBytesPerInterval);
+	/* SuperSpeed or SuperSpeedPlus Isoc ep with less than 48k per esit */
+	else if (g->speed >= USB_SPEED_SUPER)
+		return le16_to_cpu(dep->endpoint.comp_desc->wBytesPerInterval);
+
+	max_packet = usb_endpoint_maxp(dep->endpoint.desc);
+	max_burst = usb_endpoint_maxp_mult(dep->endpoint.desc);
+	/* A 0 in max burst means 1 transfer per ESIT */
+	return max_packet * max_burst;
+}
+
+/*
+ * Set up an endpoint with one ring segment. Do not allocate stream rings.
+ * Drivers will have to call usb_alloc_streams() to do that.
+ */
+int usbssp_endpoint_init(struct usbssp_udc *usbssp_data,
+			 struct usbssp_device *dev_priv,
+			 struct usbssp_ep *dep,
+			 gfp_t mem_flags)
+{
+	unsigned int ep_index;
+	struct usbssp_ep_ctx *ep_ctx;
+	struct usbssp_ring *ep_ring;
+	unsigned int max_packet;
+	enum usbssp_ring_type ring_type;
+	u32 max_esit_payload;
+	u32 endpoint_type;
+	unsigned int max_burst;
+	unsigned int interval;
+	unsigned int mult;
+	unsigned int avg_trb_len;
+	unsigned int err_count = 0;
+
+	ep_index = usbssp_get_endpoint_index(dep->endpoint.desc);
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, dev_priv->in_ctx, ep_index);
+
+	endpoint_type = usbssp_get_endpoint_type(dep->endpoint.desc);
+	if (!endpoint_type)
+		return -EINVAL;
+
+	ring_type = usb_endpoint_type(dep->endpoint.desc);
+
+	/*
+	 * Get values to fill the endpoint context, mostly from ep descriptor.
+	 * The average TRB buffer lengt for bulk endpoints is unclear as we
+	 * have no clue on scatter gather list entry size. For Isoc and Int,
+	 * set it to max available.
+	 */
+	max_esit_payload = usbssp_get_max_esit_payload(&usbssp_data->gadget, dep);
+	interval = usbssp_get_endpoint_interval(&usbssp_data->gadget, dep);
+	mult = usbssp_get_endpoint_mult(&usbssp_data->gadget, dep);
+	max_packet = GET_MAX_PACKET(usb_endpoint_maxp(dep->endpoint.desc));
+	max_burst = usbssp_get_endpoint_max_burst(&usbssp_data->gadget, dep);
+	avg_trb_len = max_esit_payload;
+
+	/* Allow 3 retries for everything but isoc, set CErr = 3 */
+	if (!usb_endpoint_xfer_isoc(dep->endpoint.desc))
+		err_count = 3;
+	if (usb_endpoint_xfer_bulk(dep->endpoint.desc) &&
+	    usbssp_data->gadget.speed == USB_SPEED_HIGH)
+		max_packet = 512;
+	/* DC spec indicates that ctrl ep avg TRB Length should be 8 */
+	if (usb_endpoint_xfer_control(dep->endpoint.desc))
+		avg_trb_len = 8;
+
+	/* Set up the endpoint ring */
+	dev_priv->eps[ep_index].new_ring = usbssp_ring_alloc(usbssp_data, 2, 1,
+							ring_type, max_packet,
+							mem_flags);
+
+	dev_priv->eps[ep_index].skip = false;
+	ep_ring = dev_priv->eps[ep_index].new_ring;
+
+	/* Fill the endpoint context */
+	ep_ctx->ep_info = cpu_to_le32(EP_MAX_ESIT_PAYLOAD_HI(max_esit_payload) |
+				EP_INTERVAL(interval) | EP_MULT(mult));
+	ep_ctx->ep_info2 = cpu_to_le32(EP_TYPE(endpoint_type) |
+				MAX_PACKET(max_packet) | MAX_BURST(max_burst) |
+				ERROR_COUNT(err_count));
+	ep_ctx->deq = cpu_to_le64(ep_ring->first_seg->dma |
+				ep_ring->cycle_state);
+
+	ep_ctx->tx_info = cpu_to_le32(EP_MAX_ESIT_PAYLOAD_LO(max_esit_payload) |
+				EP_AVG_TRB_LENGTH(avg_trb_len));
+
+	return 0;
+}
+
 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 d5ed2c48e3fa..f5b0659b6f2d 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -463,6 +463,29 @@ unsigned int usbssp_get_endpoint_index(
 	return index;
 }
 
+/**
+ * The reverse operation to usbssp_get_endpoint_index.
+ * Calculate the USB endpoint address from the USBSSP endpoint index.
+ */
+unsigned int usbssp_get_endpoint_address(unsigned int ep_index)
+{
+	unsigned int number = DIV_ROUND_UP(ep_index, 2);
+	unsigned int direction = ep_index % 2 ? USB_DIR_OUT : USB_DIR_IN;
+
+	return direction | number;
+}
+
+/**
+ * Find the flag for this endpoint (for use in the control context). Use the
+ * endpoint index to create a bitmask. The slot context is bit 0, endpoint 0 is
+ * bit 1, etc.
+ */
+unsigned int usbssp_get_endpoint_flag(
+			const struct usb_endpoint_descriptor *desc)
+{
+	return 1 << (usbssp_get_endpoint_index(desc) + 1);
+}
+
 /* Compute the last valid endpoint context index. Basically, this is the
  * endpoint index plus one. For slot contexts with more than valid endpoint,
  * we find the most significant bit set in the added contexts flags.
@@ -692,6 +715,132 @@ int usbssp_dequeue(struct usbssp_ep *ep_priv, struct usbssp_request *req_priv)
 	return ret;
 }
 
+
+/**
+ * Add an endpoint to a new possible 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 different endpoint descriptor in usbssp_ep.
+ * A call to usbssp_add_endpoint() followed by a call to usbssp_drop_endpoint()
+ * is not allowed.
+ *
+ */
+int usbssp_add_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep)
+{
+	const struct usb_endpoint_descriptor *desc = dep->endpoint.desc;
+	struct usbssp_container_ctx *in_ctx;
+	unsigned int ep_index;
+	struct usbssp_input_control_ctx *ctrl_ctx;
+	u32 added_ctxs;
+	u32 new_add_flags, new_drop_flags;
+	struct usbssp_device *dev_priv;
+	int ret = 0;
+
+	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;
+
+	added_ctxs = usbssp_get_endpoint_flag(desc);
+	if (added_ctxs == SLOT_FLAG || added_ctxs == EP0_FLAG) {
+		dev_dbg(usbssp_data->dev, "USBSSP %s - can't add slot or ep 0 %#x\n",
+			__func__, added_ctxs);
+		return 0;
+	}
+
+	dev_priv = &usbssp_data->devs;
+	in_ctx = dev_priv->in_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(desc);
+
+	/*
+	 * If this endpoint is already in use, and the upper layers are trying
+	 * to add it again without dropping it, reject the addition.
+	 */
+	if (dev_priv->eps[ep_index].ring &&
+	    !(le32_to_cpu(ctrl_ctx->drop_flags) & added_ctxs)) {
+		dev_warn(usbssp_data->dev,
+			"Trying to add endpoint 0x%x without dropping it.\n",
+			(unsigned int) desc->bEndpointAddress);
+		return -EINVAL;
+	}
+
+	/*
+	 * If already noted the endpoint is enabled,
+	 * ignore this request.
+	 */
+	if (le32_to_cpu(ctrl_ctx->add_flags) & added_ctxs) {
+		dev_warn(usbssp_data->dev, "USBSSP %s called with enabled ep %p\n",
+			__func__, dep);
+		return 0;
+	}
+
+	if (usbssp_endpoint_init(usbssp_data, dev_priv, dep, GFP_ATOMIC) < 0) {
+		dev_dbg(usbssp_data->dev, "%s - could not initialize ep %#x\n",
+			__func__, desc->bEndpointAddress);
+		return -ENOMEM;
+	}
+
+	ctrl_ctx->add_flags |= cpu_to_le32(added_ctxs);
+	new_add_flags = le32_to_cpu(ctrl_ctx->add_flags);
+	new_drop_flags = le32_to_cpu(ctrl_ctx->drop_flags);
+
+	dev_dbg(usbssp_data->dev,
+		"add ep 0x%x, new drop flags = %#x, new add flags = %#x\n",
+		(unsigned int) desc->bEndpointAddress,
+		(unsigned int) new_drop_flags,
+		(unsigned int) new_add_flags);
+	return 0;
+}
+
+static void usbssp_zero_in_ctx(struct usbssp_udc *usbssp_data,
+			       struct usbssp_device *dev_priv)
+{
+	struct usbssp_input_control_ctx *ctrl_ctx;
+	struct usbssp_ep_ctx *ep_ctx;
+	struct usbssp_slot_ctx *slot_ctx;
+	int i;
+
+	ctrl_ctx = usbssp_get_input_control_ctx(dev_priv->in_ctx);
+	if (!ctrl_ctx) {
+		dev_warn(usbssp_data->dev,
+			"%s: Could not get input context, bad type.\n",
+			__func__);
+		return;
+	}
+
+	/*
+	 * When a device's add flag and drop flag are zero, any subsequent
+	 * configure endpoint command will leave that endpoint's state
+	 * untouched. Make sure we don't leave any old state in the input
+	 * endpoint contexts.
+	 */
+	ctrl_ctx->drop_flags = 0;
+	ctrl_ctx->add_flags = 0;
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, dev_priv->in_ctx);
+	slot_ctx->dev_info &= cpu_to_le32(~LAST_CTX_MASK);
+
+	/* Endpoint 0 is always valid */
+	slot_ctx->dev_info |= cpu_to_le32(LAST_CTX(1));
+	for (i = 1; i < 31; ++i) {
+		ep_ctx = usbssp_get_ep_ctx(usbssp_data, dev_priv->in_ctx, i);
+		ep_ctx->ep_info = 0;
+		ep_ctx->ep_info2 = 0;
+		ep_ctx->deq = 0;
+		ep_ctx->tx_info = 0;
+	}
+}
+
 static int usbssp_configure_endpoint_result(struct usbssp_udc *usbssp_data,
 					    struct usb_gadget *g,
 					    u32 *cmd_status)
@@ -854,6 +1003,21 @@ static int usbssp_configure_endpoint(struct usbssp_udc *usbssp_data,
 	return ret;
 }
 
+static void usbssp_check_bw_drop_ep_streams(struct usbssp_udc *usbssp_data,
+					    struct usbssp_device *vdev, int i)
+{
+	struct usbssp_ep *ep = &vdev->eps[i];
+
+	if (ep->ep_state & EP_HAS_STREAMS) {
+		dev_warn(usbssp_data->dev,
+			"WARN: endpoint 0x%02x has streams on set_interface, freeing streams.\n",
+			 usbssp_get_endpoint_address(i));
+		usbssp_free_stream_info(usbssp_data, ep->stream_info);
+		ep->stream_info = NULL;
+		ep->ep_state &= ~EP_HAS_STREAMS;
+	}
+}
+
 int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep,
 			 int value)
 {
@@ -961,10 +1125,115 @@ int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep,
 	kfree(command->completion);
 	kfree(command);
 	return ret;
+}
 
-	return 0;
+/**
+ *  Called after one or more calls to usbssp_add_endpoint() or
+ * usbssp_drop_endpoint(). If this call fails, the driver is expected
+ * to call usbssp_reset_bandwidth().
+ */
+int usbssp_check_bandwidth(struct usbssp_udc *usbssp_data, struct usb_gadget *g)
+{
+	int i;
+	int ret = 0;
+	struct usbssp_device *dev_priv;
+	struct usbssp_input_control_ctx *ctrl_ctx;
+	struct usbssp_slot_ctx *slot_ctx;
+	struct usbssp_command *command;
+
+	ret = usbssp_check_args(usbssp_data, NULL, 0, true, __func__);
+	if (ret <= 0)
+		return ret;
+
+	if ((usbssp_data->usbssp_state & USBSSP_STATE_DYING) ||
+	    (usbssp_data->usbssp_state & USBSSP_STATE_REMOVING))
+		return -ENODEV;
+
+	dev_priv = &usbssp_data->devs;
+
+	command = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+	if (!command)
+		return -ENOMEM;
+
+	command->in_ctx = dev_priv->in_ctx;
+
+	ctrl_ctx = usbssp_get_input_control_ctx(command->in_ctx);
+	if (!ctrl_ctx) {
+		dev_warn(usbssp_data->dev,
+			"%s: Could not get input context, bad type.\n",
+			__func__);
+		ret = -ENOMEM;
+		goto command_cleanup;
+	}
+	ctrl_ctx->add_flags |= cpu_to_le32(SLOT_FLAG);
+	ctrl_ctx->add_flags &= cpu_to_le32(~EP0_FLAG);
+	ctrl_ctx->drop_flags &= cpu_to_le32(~(SLOT_FLAG | EP0_FLAG));
+
+	/* Don't issue the command if there's no endpoints to update.*/
+	if (ctrl_ctx->add_flags == cpu_to_le32(SLOT_FLAG) &&
+	    ctrl_ctx->drop_flags == 0) {
+		ret = 0;
+		goto command_cleanup;
+	}
+
+	/* Fix up Context Entries field. Minimum value is EP0 == BIT(1). */
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, dev_priv->in_ctx);
+	for (i = 31; i >= 1; i--) {
+		__le32 le32 = cpu_to_le32(BIT(i));
+
+		if ((dev_priv->eps[i-1].ring && !(ctrl_ctx->drop_flags & le32))
+		    || (ctrl_ctx->add_flags & le32) || i == 1) {
+			slot_ctx->dev_info &= cpu_to_le32(~LAST_CTX_MASK);
+			slot_ctx->dev_info |= cpu_to_le32(LAST_CTX(i));
+			break;
+		}
+	}
+
+	ret = usbssp_configure_endpoint(usbssp_data, g, command,
+			false, false);
+
+	if (ret)
+		/* Caller should call reset_bandwidth() */
+		goto command_cleanup;
+
+	/* Free any rings that were dropped, but not changed. */
+	for (i = 1; i < 31; ++i) {
+		if ((le32_to_cpu(ctrl_ctx->drop_flags) & (1 << (i + 1))) &&
+		    !(le32_to_cpu(ctrl_ctx->add_flags) & (1 << (i + 1)))) {
+			usbssp_free_endpoint_ring(usbssp_data, dev_priv, i);
+			usbssp_check_bw_drop_ep_streams(usbssp_data,
+							dev_priv, i);
+		}
+	}
+
+	usbssp_zero_in_ctx(usbssp_data, dev_priv);
+
+	/*
+	 * Install any rings for completely new endpoints or changed endpoints,
+	 * and free any old rings from changed endpoints.
+	 */
+	for (i = 1; i < 31; ++i) {
+		if (!dev_priv->eps[i].new_ring)
+			continue;
+
+		/*
+		 * Only free the old ring if it exists.
+		 * It may not if this is the first add of an endpoint.
+		 */
+		if (dev_priv->eps[i].ring)
+			usbssp_free_endpoint_ring(usbssp_data, dev_priv, i);
+
+		usbssp_check_bw_drop_ep_streams(usbssp_data, dev_priv, i);
+		dev_priv->eps[i].ring = dev_priv->eps[i].new_ring;
+		dev_priv->eps[i].new_ring = NULL;
+	}
+command_cleanup:
+	kfree(command->completion);
+	kfree(command);
+	return ret;
 }
 
+
 /*
  * This submits a Reset Device Command, which will set the device state to 0,
  * set the device address to 0, and disable all the endpoints except the default
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 81d7fe44519a..c4075e765dcc 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1694,6 +1694,11 @@ 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);
+int usbssp_endpoint_init(struct usbssp_udc *usbssp_data,
+			struct usbssp_device *dev_priv,
+			struct usbssp_ep *dep,
+			gfp_t mem_flags);
+
 int usbssp_ring_expansion(struct usbssp_udc *usbssp_data,
 			struct usbssp_ring *ring,
 			unsigned int num_trbs, gfp_t flags);
@@ -1705,6 +1710,9 @@ struct usbssp_ring *usbssp_dma_to_transfer_ring(struct usbssp_ep *ep,
 struct usbssp_ring *usbssp_stream_id_to_ring(struct usbssp_device *dev,
 					unsigned int ep_index,
 					unsigned int stream_id);
+void usbssp_free_endpoint_ring(struct usbssp_udc *usbssp_data,
+			struct usbssp_device *dev_priv,
+			unsigned int ep_index);
 void usbssp_free_stream_info(struct usbssp_udc *usbssp_data,
 			struct usbssp_stream_info *stream_info);
 struct usbssp_ring *usbssp_dma_to_transfer_ring(
@@ -1857,6 +1865,8 @@ int usbssp_setup_analyze(struct usbssp_udc *usbssp_data);
 int usbssp_status_stage(struct usbssp_udc *usbssp_data);
 
 int usbssp_reset_device(struct usbssp_udc *usbssp_data);
+int usbssp_check_bandwidth(struct usbssp_udc *usbssp_data,
+			struct usb_gadget *g);
 
 static inline struct usbssp_ring *usbssp_request_to_transfer_ring(
 					struct usbssp_udc *usbssp_data,
-- 
2.17.1