diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 0180ecfdaf9c..d5ed2c48e3fa 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -692,6 +692,168 @@ int usbssp_dequeue(struct usbssp_ep *ep_priv, struct usbssp_request *req_priv)
 	return ret;
 }
 
+static int usbssp_configure_endpoint_result(struct usbssp_udc *usbssp_data,
+					    struct usb_gadget *g,
+					    u32 *cmd_status)
+{
+	int ret;
+
+	switch (*cmd_status) {
+	case COMP_COMMAND_ABORTED:
+	case COMP_COMMAND_RING_STOPPED:
+		dev_warn(usbssp_data->dev,
+			"Timeout while waiting for configure endpoint command\n");
+		ret = -ETIME;
+		break;
+	case COMP_RESOURCE_ERROR:
+		dev_warn(&g->dev,
+			"Not enough device controller resources for new device state.\n");
+		ret = -ENOMEM;
+		break;
+	case COMP_TRB_ERROR:
+		/* the gadget driver set up something wrong */
+		dev_warn(&g->dev, "ERROR: Endpoint drop flag = 0, "
+			"add flag = 1, and endpoint is not disabled.\n");
+		ret = -EINVAL;
+		break;
+	case COMP_INCOMPATIBLE_DEVICE_ERROR:
+		dev_warn(&g->dev,
+			"ERROR: Incompatible device for endpoint configure command.\n");
+		ret = -ENODEV;
+		break;
+	case COMP_SUCCESS:
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_context_change,
+			"Successful Endpoint Configure command");
+		ret = 0;
+		break;
+	default:
+		dev_err(usbssp_data->dev,
+			"ERROR: unexpected command completion code 0x%x.\n",
+			*cmd_status);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static int usbssp_evaluate_context_result(struct usbssp_udc *usbssp_data,
+					   struct usb_gadget *g, u32 *cmd_status)
+{
+	int ret;
+
+	switch (*cmd_status) {
+	case COMP_COMMAND_ABORTED:
+	case COMP_COMMAND_RING_STOPPED:
+		dev_warn(usbssp_data->dev,
+			"Timeout while waiting for evaluate context command\n");
+		ret = -ETIME;
+		break;
+	case COMP_PARAMETER_ERROR:
+		dev_warn(&g->dev,
+			"WARN: USBSSP driver setup invalid evaluate context command.\n");
+		ret = -EINVAL;
+		break;
+	case COMP_SLOT_NOT_ENABLED_ERROR:
+		dev_warn(&g->dev,
+			"WARN: slot not enabled for evaluate context command.\n");
+		ret = -EINVAL;
+		break;
+	case COMP_CONTEXT_STATE_ERROR:
+		dev_warn(&g->dev,
+			"WARN: invalid context state for evaluate context command.\n");
+		ret = -EINVAL;
+		break;
+	case COMP_INCOMPATIBLE_DEVICE_ERROR:
+		dev_warn(&g->dev,
+			"ERROR: Incompatible device for evaluate context command.\n");
+		ret = -ENODEV;
+		break;
+	case COMP_MAX_EXIT_LATENCY_TOO_LARGE_ERROR:
+		/* Max Exit Latency too large error */
+		dev_warn(&g->dev, "WARN: Max Exit Latency too large\n");
+		ret = -EINVAL;
+		break;
+	case COMP_SUCCESS:
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_context_change,
+			"Successful evaluate context command");
+		ret = 0;
+		break;
+	default:
+		dev_err(usbssp_data->dev,
+			"ERROR: unexpected command completion code 0x%x.\n",
+			*cmd_status);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+/* Issue a configure endpoint command or evaluate context command
+ * and wait for it to finish.
+ */
+static int usbssp_configure_endpoint(struct usbssp_udc *usbssp_data,
+				     struct usb_gadget *g,
+				     struct usbssp_command *command,
+				     bool ctx_change, bool must_succeed)
+{
+	int ret;
+	struct usbssp_input_control_ctx *ctrl_ctx;
+	struct usbssp_device *dev_priv;
+	struct usbssp_slot_ctx *slot_ctx;
+
+	if (!command)
+		return -EINVAL;
+
+	if (usbssp_data->usbssp_state & USBSSP_STATE_DYING)
+		return -ESHUTDOWN;
+
+	dev_priv = &usbssp_data->devs;
+	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__);
+		return -ENOMEM;
+	}
+
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, command->in_ctx);
+	trace_usbssp_configure_endpoint(slot_ctx);
+
+	if (!ctx_change) {
+		ret = usbssp_queue_configure_endpoint(usbssp_data, command,
+						command->in_ctx->dma, must_succeed);
+	} else {
+		ret = usbssp_queue_evaluate_context(usbssp_data, command,
+						command->in_ctx->dma, must_succeed);
+	}
+
+	if (ret < 0) {
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_context_change,
+			"FIXME allocate a new ring segment");
+		return -ENOMEM;
+	}
+
+	usbssp_ring_cmd_db(usbssp_data);
+
+	spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+
+	/*Waiting for handling Endpoint Configure command */
+	while (!command->status)
+		udelay(100);
+
+	spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+
+	if (!ctx_change)
+		ret = usbssp_configure_endpoint_result(usbssp_data, g,
+						&command->status);
+	else
+		ret = usbssp_evaluate_context_result(usbssp_data, g,
+						&command->status);
+	return ret;
+}
+
 int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep,
 			 int value)
 {
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 6737cb05cd27..81d7fe44519a 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1692,6 +1692,7 @@ int usbssp_alloc_priv_device(struct usbssp_udc *usbssp_data, gfp_t flags);
 int usbssp_setup_addressable_priv_dev(struct usbssp_udc *usbssp_data);
 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_ring_expansion(struct usbssp_udc *usbssp_data,
 			struct usbssp_ring *ring,
@@ -1704,6 +1705,15 @@ 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_stream_info(struct usbssp_udc *usbssp_data,
+			struct usbssp_stream_info *stream_info);
+struct usbssp_ring *usbssp_dma_to_transfer_ring(
+					struct usbssp_ep *ep,
+					u64 address);
+struct usbssp_ring *usbssp_stream_id_to_ring(
+					struct usbssp_device *dev,
+					unsigned int ep_index,
+					unsigned int stream_id);
 
 struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
 					bool allocate_completion,
@@ -1771,6 +1781,14 @@ int usbssp_queue_isoc_tx_prepare(
 			struct usbssp_udc *usbssp_data, gfp_t mem_flags,
 			struct usbssp_request *req_priv,
 			unsigned int ep_index);
+int usbssp_queue_configure_endpoint(struct usbssp_udc *usbssp_data,
+				struct usbssp_command *cmd,
+				dma_addr_t in_ctx_ptr,
+				bool command_must_succeed);
+int usbssp_queue_evaluate_context(struct usbssp_udc *usbssp_data,
+				struct usbssp_command *cmd,
+				dma_addr_t in_ctx_ptr,
+				bool command_must_succeed);
 int usbssp_queue_reset_ep(struct usbssp_udc *usbssp_data,
 			struct usbssp_command *cmd,
 			unsigned int ep_index,
