diff --git a/drivers/usb/usbssp/Makefile b/drivers/usb/usbssp/Makefile
index 22479a6378c4..de8bbcec277e 100644
--- a/drivers/usb/usbssp/Makefile
+++ b/drivers/usb/usbssp/Makefile
@@ -5,7 +5,7 @@ CFLAGS_gadget-trace.o := -I$(src)
 obj-$(CONFIG_USB_USBSSP_GADGET) += usbssp.o
 usbssp-y 			:=  usbssp-plat.o gadget-ring.o \
 				    gadget.o gadget-mem.o gadget-port.o \
-				    gadget-dbg.o
+				    gadget-dbg.o gadget-ep0.o
 
 ifneq ($(CONFIG_TRACING),)
 	usbssp-y		+= gadget-trace.o
diff --git a/drivers/usb/usbssp/gadget-ep0.c b/drivers/usb/usbssp/gadget-ep0.c
new file mode 100644
index 000000000000..c889a3102740
--- /dev/null
+++ b/drivers/usb/usbssp/gadget-ep0.c
@@ -0,0 +1,23 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * USBSSP device controller driver
+ *
+ * Copyright (C) 2018 Cadence.
+ *
+ * Author: Pawel Laszczak
+ *
+ * A lot of code based on Linux XHCI driver.
+ * Origin: Copyright (C) 2008 Intel Corp
+ */
+
+#include <linux/list.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/composite.h>
+#include "gadget-trace.h"
+
+int usbssp_status_stage(struct usbssp_udc *usbssp_data)
+{
+	/*TODO; function must to be implemented*/
+	return 0;
+}
+
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index 32bf7a4cae34..983705f6cda9 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -1081,6 +1081,72 @@ struct usbssp_segment *usbssp_trb_in_td(struct usbssp_udc *usbssp_data,
 	return NULL;
 }
 
+void usbssp_cleanup_halted_endpoint(struct usbssp_udc *usbssp_data,
+				    unsigned int ep_index,
+				    unsigned int stream_id,
+				    struct usbssp_td *td,
+				    enum usbssp_ep_reset_type reset_type)
+{
+	struct usbssp_command *command;
+	struct usbssp_ep_ctx *ep_ctx;
+	int interrupt_disabled_locally;
+
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, usbssp_data->devs.out_ctx,
+			ep_index);
+
+	if (GET_EP_CTX_STATE(ep_ctx) != EP_STATE_HALTED) {
+		usbssp_dbg(usbssp_data,
+			"Endpint index %d is not in  halted state.\n",
+			ep_index);
+		usbssp_status_stage(usbssp_data);
+		return;
+	}
+
+	command = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+	if (!command)
+		return;
+
+	usbssp_queue_reset_ep(usbssp_data, command, ep_index,
+			reset_type);
+
+	usbssp_ring_cmd_db(usbssp_data);
+
+	if (irqs_disabled()) {
+		spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+				usbssp_data->irq_thread_flag);
+		interrupt_disabled_locally = 1;
+	} else {
+		spin_unlock(&usbssp_data->irq_thread_lock);
+	}
+
+	wait_for_completion(command->completion);
+
+	if (interrupt_disabled_locally)
+		spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+				usbssp_data->irq_thread_flag);
+	else
+		spin_lock(&usbssp_data->irq_thread_lock);
+
+	usbssp_free_command(usbssp_data, command);
+	if (ep_index != 0)
+		usbssp_status_stage(usbssp_data);
+}
+
+int usbssp_is_vendor_info_code(struct usbssp_udc *usbssp_data, unsigned int trb_comp_code)
+{
+	if (trb_comp_code >= 224 && trb_comp_code <= 255) {
+		/* Vendor defined "informational" completion code,
+		 * treat as not-an-error.
+		 */
+		usbssp_dbg(usbssp_data,
+				"Vendor defined info completion code %u\n",
+				trb_comp_code);
+		usbssp_dbg(usbssp_data, "Treating code as success.\n");
+		return 1;
+	}
+	return 0;
+}
+
 /*
  * If this function returns an error condition, it means it got a Transfer
  * event with a corrupted Slot ID, Endpoint ID, or TRB DMA address.
@@ -1412,6 +1478,67 @@ static int prepare_ring(struct usbssp_udc *usbssp_data,
 	return 0;
 }
 
+/* Stop endpoint after disconnecting device.*/
+int usbssp_cmd_stop_ep(struct usbssp_udc *usbssp_data, struct usb_gadget *g,
+		       struct usbssp_ep *ep_priv)
+{
+	int ret = 0;
+	struct usbssp_command *command;
+	unsigned int ep_index;
+	struct usbssp_container_ctx *out_ctx;
+	struct usbssp_ep_ctx *ep_ctx;
+	int interrupt_disabled_locally = 0;
+
+	ep_index = usbssp_get_endpoint_index(ep_priv->endpoint.desc);
+
+	if ((ep_priv->ep_state & EP_STOP_CMD_PENDING)) {
+		usbssp_dbg(usbssp_data,
+			"Stop endpoint command on %s (index: %d) is pending\n",
+			ep_priv->name, ep_index);
+		return 0;
+	}
+
+	command = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+	if (!command)
+		return -ENOMEM;
+
+	ep_priv->ep_state |= EP_STOP_CMD_PENDING;
+
+	usbssp_queue_stop_endpoint(usbssp_data, command,
+			ep_index, 0);
+	usbssp_ring_cmd_db(usbssp_data);
+
+	out_ctx = usbssp_data->devs.out_ctx;
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, out_ctx, ep_index);
+
+	if (irqs_disabled()) {
+		spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+				usbssp_data->irq_thread_flag);
+		interrupt_disabled_locally = 1;
+	} else {
+		spin_unlock(&usbssp_data->irq_thread_lock);
+	}
+
+	/* Wait for last stop endpoint command to finish */
+	wait_for_completion(command->completion);
+
+	if (interrupt_disabled_locally)
+		spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+				usbssp_data->irq_thread_flag);
+	else
+		spin_lock(&usbssp_data->irq_thread_lock);
+
+	if (command->status == COMP_COMMAND_ABORTED ||
+			command->status == COMP_COMMAND_RING_STOPPED) {
+		usbssp_warn(usbssp_data,
+			"Timeout while waiting for stop endpoint command\n");
+		ret = -ETIME;
+	}
+
+	usbssp_free_command(usbssp_data, command);
+	return ret;
+}
+
 /****		Command Ring Operations		****/
 /* Generic function for queueing a command TRB on the command ring.
  * Check to make sure there's room on the command ring for one command TRB.
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 6637fa010b39..0ae4e1c07035 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -328,6 +328,29 @@ int usbssp_resume(struct usbssp_udc *usbssp_data, bool hibernated)
 
 #endif	/* CONFIG_PM */
 
+/**
+ * usbssp_get_endpoint_index - Find the index for an endpoint given its
+ * descriptor.Use the return value to right shift 1 for the bitmask.
+ *
+ * Index = (epnum * 2) + direction - 1,
+ * where direction = 0 for OUT, 1 for IN.
+ * For control endpoints, the IN index is used (OUT index is unused), so
+ * index = (epnum * 2) + direction - 1 = (epnum * 2) + 1 - 1 = (epnum * 2)
+ */
+unsigned int usbssp_get_endpoint_index(
+		const struct usb_endpoint_descriptor *desc)
+{
+	unsigned int index;
+
+	if (usb_endpoint_xfer_control(desc)) {
+		index = (unsigned int) (usb_endpoint_num(desc)*2);
+	} else {
+		index = (unsigned int) (usb_endpoint_num(desc)*2) +
+			(usb_endpoint_dir_in(desc) ? 1 : 0) - 1;
+	}
+	return index;
+}
+
 /* 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.
@@ -339,10 +362,136 @@ unsigned int usbssp_last_valid_endpoint(u32 added_ctxs)
 	return fls(added_ctxs) - 1;
 }
 
+/* Returns 1 if the arguments are OK;
+ * returns -EINVAL for NULL pointers.
+ */
+static int usbssp_check_args(struct usbssp_udc *usbssp_data,
+			     struct usbssp_ep *ep, int check_ep,
+			     bool check_dev_priv, const char *func)
+{
+	struct usbssp_device	*dev_priv;
+
+	if (!usbssp_data || (check_ep && !ep)) {
+		pr_debug("USBSSP %s called with invalid args\n", func);
+		return -EINVAL;
+	}
+
+	if (check_dev_priv)
+		dev_priv = &usbssp_data->devs;
+
+	if (usbssp_data->usbssp_state & USBSSP_STATE_HALTED)
+		return -ENODEV;
+
+	return 1;
+}
+
 int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep,
 			 int value)
 {
-	/*TODO: implement this function*/
+	int ret = 1;
+	struct usbssp_device *dev_priv;
+	struct usbssp_command *command;
+	unsigned int ep_index;
+	int interrupt_disabled_locally = 0;
+
+	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;
+	ep_index = usbssp_get_endpoint_index(dep->endpoint.desc);
+
+	command = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+
+	if (!command)
+		return -ENOMEM;
+
+	if (value) {
+		dep->ep_state |= EP_HALTED;
+
+		ret = usbssp_cmd_stop_ep(usbssp_data,
+				&usbssp_data->gadget, dep);
+		if (ret < 0) {
+			usbssp_err(usbssp_data,
+					"Command Stop Endpoint failed 1\n");
+			return ret;
+		}
+
+		ret = usbssp_queue_halt_endpoint(usbssp_data, command,
+				ep_index);
+
+		if (ret < 0) {
+			usbssp_err(usbssp_data,
+					"Command Halt Endpoint failed\n");
+			goto command_cleanup;
+		}
+
+		usbssp_ring_cmd_db(usbssp_data);
+		/*wait for ep*/
+		if (irqs_disabled()) {
+			spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+					usbssp_data->irq_thread_flag);
+			interrupt_disabled_locally = 1;
+		} else {
+			spin_unlock(&usbssp_data->irq_thread_lock);
+		}
+
+		/* Wait for last stop endpoint command to finish */
+		wait_for_completion(command->completion);
+
+		if (interrupt_disabled_locally)
+			spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+					usbssp_data->irq_thread_flag);
+		else
+			spin_lock(&usbssp_data->irq_thread_lock);
+
+
+	} else {
+		struct usbssp_td *td;
+
+		/* Issue a reset endpoint command to clear the device side
+		 * halt, followed by a set dequeue command to move the
+		 * dequeue pointer past the TD.
+		 */
+		td = list_first_entry(&dep->ring->td_list, struct usbssp_td,
+				td_list);
+
+		usbssp_cleanup_halted_endpoint(usbssp_data, ep_index,
+				dep->ring->stream_id, td, EP_HARD_RESET);
+
+		goto command_cleanup;
+	}
+
+	ret = command->status;
+
+	switch (ret) {
+	case COMP_COMMAND_ABORTED:
+	case COMP_COMMAND_RING_STOPPED:
+		usbssp_warn(usbssp_data,
+				"Timeout waiting for Halt Endpoint command\n");
+		ret = -ETIME;
+		goto command_cleanup;
+	case COMP_SUCCESS:
+		usbssp_dbg(usbssp_data, "Successful Halt Endpoint command.\n");
+		break;
+	default:
+		if (usbssp_is_vendor_info_code(usbssp_data, ret))
+			break;
+		usbssp_warn(usbssp_data, "Unknown completion code %u for "
+				"Halt Endpoint command.\n", ret);
+		ret = -EINVAL;
+		goto command_cleanup;
+	}
+
+command_cleanup:
+	kfree(command->completion);
+	kfree(command);
+	return ret;
+
 	return 0;
 }
 
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 38e9b80faf2a..45b568d33723 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1681,6 +1681,7 @@ void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data);
 int usbssp_mem_init(struct usbssp_udc *usbssp_data, gfp_t flags);
 void usbssp_free_priv_device(struct usbssp_udc *usbssp_data);
 int usbssp_alloc_priv_device(struct usbssp_udc *usbssp_data, gfp_t flags);
+unsigned int usbssp_get_endpoint_index(const struct usb_endpoint_descriptor *desc);
 unsigned int usbssp_last_valid_endpoint(u32 added_ctxs);
 int usbssp_ring_expansion(struct usbssp_udc *usbssp_data,
 		struct usbssp_ring *ring, unsigned int num_trbs, gfp_t flags);
@@ -1723,12 +1724,26 @@ struct usbssp_segment *usbssp_trb_in_td(struct usbssp_udc *usbssp_data,
 		struct usbssp_segment *start_seg,
 		union usbssp_trb *start_trb, union usbssp_trb *end_trb,
 		dma_addr_t suspect_dma, bool debug);
+
+int usbssp_is_vendor_info_code(struct usbssp_udc *usbssp_data,
+		unsigned int trb_comp_code);
 void usbssp_ring_cmd_db(struct usbssp_udc *usbssp_data);
 int usbssp_queue_slot_control(struct usbssp_udc *usbssp_data,
 		struct usbssp_command *cmd, u32 trb_type);
 int usbssp_queue_stop_endpoint(struct usbssp_udc *usbssp_data,
 		struct usbssp_command *cmd,
 		unsigned int ep_index, int suspend);
+int usbssp_queue_reset_ep(struct usbssp_udc *usbssp_data,
+		struct usbssp_command *cmd,
+		unsigned int ep_index, enum usbssp_ep_reset_type reset_type);
+void usbssp_cleanup_halted_endpoint(struct usbssp_udc *usbssp_data,
+		unsigned int ep_index,
+		unsigned int stream_id,
+		struct usbssp_td *td,
+		enum usbssp_ep_reset_type reset_type);
+int usbssp_queue_halt_endpoint(struct usbssp_udc *usbssp_data,
+		struct usbssp_command *cmd,
+		unsigned int ep_index);
 void usbssp_handle_command_timeout(struct work_struct *work);
 
 void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data);
@@ -1761,6 +1776,7 @@ int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data,
 		struct usbssp_ep *dep, int value);
 int usbssp_cmd_stop_ep(struct usbssp_udc *usbssp_data, struct usb_gadget *g,
 		struct usbssp_ep *ep_priv);
+int usbssp_status_stage(struct usbssp_udc *usbssp_data);
 
 static inline char *usbssp_slot_state_string(u32 state)
 {
