diff --git a/drivers/usb/usbssp/gadget-if.c b/drivers/usb/usbssp/gadget-if.c
index 6f42ad33979d..ef466aa47f56 100644
--- a/drivers/usb/usbssp/gadget-if.c
+++ b/drivers/usb/usbssp/gadget-if.c
@@ -288,6 +288,16 @@ void usbssp_gadget_free_endpoint(struct usbssp_udc *usbssp_data)
 	}
 }
 
+static void usbssp_disconnect_gadget(struct usbssp_udc *usbssp_data)
+{
+	if (usbssp_data->gadget_driver &&
+	    usbssp_data->gadget_driver->disconnect) {
+		spin_unlock(&usbssp_data->irq_thread_lock);
+		usbssp_data->gadget_driver->disconnect(&usbssp_data->gadget);
+		spin_lock(&usbssp_data->irq_thread_lock);
+	}
+}
+
 void usbssp_suspend_gadget(struct usbssp_udc *usbssp_data)
 {
 	if (usbssp_data->gadget_driver && usbssp_data->gadget_driver->suspend) {
@@ -318,6 +328,12 @@ static void usbssp_reset_gadget(struct usbssp_udc *usbssp_data)
 		spin_lock(&usbssp_data->lock);
 	}
 }
+
+void usbssp_gadget_disconnect_interrupt(struct usbssp_udc *usbssp_data)
+{
+	usbssp_disconnect_gadget(usbssp_data);
+}
+
 void usbssp_gadget_reset_interrupt(struct usbssp_udc *usbssp_data)
 {
 	usbssp_reset_gadget(usbssp_data);
diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index 5708a0090ead..2c220582a5f8 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -643,6 +643,70 @@ void usbssp_free_priv_device(struct usbssp_udc *usbssp_data)
 	usbssp_data->slot_id = 0;
 }
 
+int usbssp_alloc_priv_device(struct usbssp_udc *usbssp_data, gfp_t flags)
+{
+	struct usbssp_device *priv_dev;
+
+	/* Slot ID 0 is reserved */
+	if (usbssp_data->slot_id == 0) {
+		dev_warn(usbssp_data->dev, "Bad Slot ID %d\n",
+			usbssp_data->slot_id);
+		return 0;
+	}
+
+	priv_dev = &usbssp_data->devs;
+
+	/* Allocate the (output) device context that will be
+	 * used in the USBSSP.
+	 */
+	priv_dev->out_ctx = usbssp_alloc_container_ctx(usbssp_data,
+						USBSSP_CTX_TYPE_DEVICE, flags);
+
+	if (!priv_dev->out_ctx)
+		goto fail;
+
+	dev_dbg(usbssp_data->dev, "Slot %d output ctx = 0x%llx (dma)\n",
+		usbssp_data->slot_id,
+		(unsigned long long)priv_dev->out_ctx->dma);
+
+	/* Allocate the (input) device context for address device command */
+	priv_dev->in_ctx = usbssp_alloc_container_ctx(usbssp_data,
+						USBSSP_CTX_TYPE_INPUT, flags);
+
+	if (!priv_dev->in_ctx)
+		goto fail;
+
+	dev_dbg(usbssp_data->dev, "Slot %d input ctx = 0x%llx (dma)\n",
+		usbssp_data->slot_id,
+		(unsigned long long)priv_dev->in_ctx->dma);
+
+	/* Allocate endpoint 0 ring */
+	priv_dev->eps[0].ring = usbssp_ring_alloc(usbssp_data, 2, 1,
+						TYPE_CTRL, 0, flags);
+	if (!priv_dev->eps[0].ring)
+		goto fail;
+
+	priv_dev->gadget = &usbssp_data->gadget;
+
+	/* Point to output device context in dcbaa. */
+	usbssp_data->dcbaa->dev_context_ptrs[usbssp_data->slot_id] =
+		cpu_to_le64(priv_dev->out_ctx->dma);
+	dev_dbg(usbssp_data->dev, "Set slot id %d dcbaa entry %p to 0x%llx\n",
+		usbssp_data->slot_id,
+		&usbssp_data->dcbaa->dev_context_ptrs[usbssp_data->slot_id],
+		le64_to_cpu(usbssp_data->dcbaa->dev_context_ptrs[usbssp_data->slot_id]));
+
+	trace_usbssp_alloc_priv_device(priv_dev);
+	return 1;
+fail:
+	if (priv_dev->in_ctx)
+		usbssp_free_container_ctx(usbssp_data, priv_dev->in_ctx);
+	if (priv_dev->out_ctx)
+		usbssp_free_container_ctx(usbssp_data, priv_dev->out_ctx);
+
+	return 0;
+}
+
 struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
 					    bool allocate_completion,
 					    gfp_t mem_flags)
@@ -754,6 +818,7 @@ void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data)
 
 	cancel_delayed_work_sync(&usbssp_data->cmd_timer);
 	cancel_work_sync(&usbssp_data->bottom_irq);
+	destroy_workqueue(usbssp_data->bottom_irq_wq);
 
 	/* Free the Event Ring Segment Table and the actual Event Ring */
 	usbssp_free_erst(usbssp_data, &usbssp_data->erst);
@@ -1265,6 +1330,14 @@ int usbssp_mem_init(struct usbssp_udc *usbssp_data, gfp_t flags)
 			usbssp_handle_command_timeout);
 	init_completion(&usbssp_data->cmd_ring_stop_completion);
 
+	usbssp_data->bottom_irq_wq =
+		create_singlethread_workqueue(dev_name(usbssp_data->dev));
+
+	if (!usbssp_data->bottom_irq_wq)
+		goto fail;
+
+	INIT_WORK(&usbssp_data->bottom_irq, usbssp_bottom_irq);
+
 	page_size = readl(&usbssp_data->op_regs->page_size);
 	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
 			"Supported page size register = 0x%x", page_size);
diff --git a/drivers/usb/usbssp/gadget-port.c b/drivers/usb/usbssp/gadget-port.c
index 09ea5b574ae0..2c4d28070cab 100644
--- a/drivers/usb/usbssp/gadget-port.c
+++ b/drivers/usb/usbssp/gadget-port.c
@@ -65,6 +65,98 @@ u32 usbssp_port_state_to_neutral(u32 state)
 	return (state & USBSSP_PORT_RO) | (state & USBSSP_PORT_RWS);
 }
 
+/*
+ * Stop device
+ * It issues stop endpoint command for EP 0 to 30. And wait the last command
+ * to complete.
+ */
+int usbssp_stop_device(struct usbssp_udc *usbssp_data, int suspend)
+{
+	struct usbssp_device *priv_dev;
+	struct usbssp_ep_ctx *ep_ctx;
+	int ret = 0;
+	int i;
+
+	ret = 0;
+	priv_dev = &usbssp_data->devs;
+
+	trace_usbssp_stop_device(priv_dev);
+
+	if (usbssp_data->gadget.state < USB_STATE_ADDRESS) {
+		dev_dbg(usbssp_data->dev,
+			"Device is not yet in USB_STATE_ADDRESS state\n");
+		goto stop_ep0;
+	}
+
+	for (i = LAST_EP_INDEX; i > 0; i--) {
+		if (priv_dev->eps[i].ring && priv_dev->eps[i].ring->dequeue) {
+			struct usbssp_command *command;
+
+			if (priv_dev->eps[i].ep_state & EP_HALTED) {
+				dev_dbg(usbssp_data->dev,
+					"ep_index %d is in halted state "
+					"- ep state: %x\n",
+					i, priv_dev->eps[i].ep_state);
+				usbssp_halt_endpoint(usbssp_data,
+						&priv_dev->eps[i], 0);
+			}
+
+			ep_ctx = usbssp_get_ep_ctx(usbssp_data,
+					priv_dev->out_ctx, i);
+
+			if (GET_EP_CTX_STATE(ep_ctx) != EP_STATE_RUNNING) {
+				dev_dbg(usbssp_data->dev,
+					"ep_index %d is already stopped.\n", i);
+				continue;
+			}
+
+			if (priv_dev->eps[i].ep_state & EP_STOP_CMD_PENDING) {
+				dev_dbg(usbssp_data->dev,
+					"Stop endpoint command is pending "
+					"for ep_index %d.\n", i);
+				continue;
+			}
+
+			/*
+			 * Device was disconnected so endpoint should be disabled
+			 * and transfer ring stopped.
+			 */
+			priv_dev->eps[i].ep_state |= EP_STOP_CMD_PENDING |
+						USBSSP_EP_DISABLE_PENDING;
+
+			command = usbssp_alloc_command(usbssp_data, false,
+						GFP_ATOMIC);
+			if (!command)
+				return -ENOMEM;
+
+			ret = usbssp_queue_stop_endpoint(usbssp_data,
+							command, i, suspend);
+			if (ret) {
+				usbssp_free_command(usbssp_data, command);
+				return ret;
+			}
+		}
+	}
+
+stop_ep0:
+	if (priv_dev->eps[0].ep_state & EP_HALTED) {
+		dev_dbg(usbssp_data->dev,
+			"ep_index 0 is in halted state - ep state: %x\n",
+			priv_dev->eps[i].ep_state);
+		ret = usbssp_halt_endpoint(usbssp_data, &priv_dev->eps[0], 0);
+	} else {
+		/*
+		 * Device was disconnected so endpoint should be disabled
+		 * and transfer ring stopped.
+		 */
+		priv_dev->eps[0].ep_state &= ~USBSSP_EP_ENABLED;
+		ret = usbssp_cmd_stop_ep(usbssp_data, &usbssp_data->gadget,
+					&priv_dev->eps[0]);
+	}
+
+	return ret;
+}
+
 __le32 __iomem *usbssp_get_port_io_addr(struct usbssp_udc *usbssp_data)
 {
 	if (usbssp_data->port_major_revision == 0x03)
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index eab7676fa744..f3ee1c4d82dc 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -218,6 +218,18 @@ static inline int room_on_ring(struct usbssp_udc *usbssp_data,
 	return 1;
 }
 
+/* Ring the device controller doorbell after placing a command on the ring */
+void usbssp_ring_cmd_db(struct usbssp_udc *usbssp_data)
+{
+	if (!(usbssp_data->cmd_ring_state & CMD_RING_STATE_RUNNING))
+		return;
+
+	dev_dbg(usbssp_data->dev, "// Ding dong command ring!\n");
+	writel(DB_VALUE_CMD, &usbssp_data->dba->doorbell[0]);
+	/* Flush PCI posted writes */
+	readl(&usbssp_data->dba->doorbell[0]);
+}
+
 static bool usbssp_mod_cmd_timer(struct usbssp_udc *usbssp_data,
 				  unsigned long delay)
 {
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 6b3dc973c0d9..32d095b32e9f 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -23,6 +23,64 @@
 #include "gadget-trace.h"
 #include "gadget.h"
 
+void usbssp_bottom_irq(struct work_struct *work)
+{
+	struct usbssp_udc *usbssp_data = container_of(work, struct usbssp_udc,
+						bottom_irq);
+
+	if (usbssp_data->usbssp_state & USBSSP_STATE_DYING) {
+		dev_err(usbssp_data->dev, "Device controller dying\n");
+		return;
+	}
+
+	mutex_lock(&usbssp_data->mutex);
+	spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+
+	if (usbssp_data->defered_event & EVENT_DEV_DISCONECTED) {
+		dev_dbg(usbssp_data->dev, "Disconnecting device sequence\n");
+		usbssp_data->defered_event &= ~EVENT_DEV_DISCONECTED;
+		usbssp_data->usbssp_state |= USBSSP_STATE_DISCONNECT_PENDING;
+		usbssp_stop_device(usbssp_data, 0);
+
+		usbssp_gadget_disconnect_interrupt(usbssp_data);
+		usbssp_data->gadget.speed = USB_SPEED_UNKNOWN;
+		usb_gadget_set_state(&usbssp_data->gadget, USB_STATE_NOTATTACHED);
+
+		dev_dbg(usbssp_data->dev, "Wait for disconnect\n");
+
+		spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+					usbssp_data->irq_thread_flag);
+		/*fixme: should be replaced by wait_for_completion*/
+		msleep(200);
+		spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+				usbssp_data->irq_thread_flag);
+	}
+
+	if (usbssp_data->defered_event & EVENT_DEV_CONNECTED) {
+		dev_dbg(usbssp_data->dev, "Connecting device sequence\n");
+		if (usbssp_data->usbssp_state & USBSSP_STATE_DISCONNECT_PENDING) {
+			usbssp_free_dev(usbssp_data);
+			usbssp_data->usbssp_state &= ~USBSSP_STATE_DISCONNECT_PENDING;
+		}
+
+		usbssp_data->defered_event &= ~EVENT_DEV_CONNECTED;
+		usbssp_alloc_dev(usbssp_data);
+	}
+
+	if (usbssp_data->defered_event & EVENT_USB_RESET) {
+		/*TODO: implement handling of USB_RESET*/
+	}
+
+	/*handle setup packet*/
+	if (usbssp_data->defered_event & EVENT_SETUP_PACKET) {
+		/*TODO: implement handling of SETUP packet*/
+	}
+
+	spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+	mutex_unlock(&usbssp_data->mutex);
+}
 
 /*
  * usbssp_handshake - spin reading dc until handshake completes or fails
@@ -273,6 +331,123 @@ unsigned int usbssp_last_valid_endpoint(u32 added_ctxs)
 	return fls(added_ctxs) - 1;
 }
 
+int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep,
+			 int value)
+{
+	/*TODO: implement this function*/
+	return 0;
+}
+
+/*
+ * At this point, the struct usb_device is about to go away, the device has
+ * disconnected, and all traffic has been stopped and the endpoints have been
+ * disabled. Free any DC data structures associated with that device.
+ */
+void usbssp_free_dev(struct usbssp_udc *usbssp_data)
+{
+	struct usbssp_device *priv_dev;
+	int i, ret;
+	struct usbssp_slot_ctx *slot_ctx;
+
+	priv_dev = &usbssp_data->devs;
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, priv_dev->out_ctx);
+	trace_usbssp_free_dev(slot_ctx);
+
+	for (i = 0; i < 31; ++i)
+		priv_dev->eps[i].ep_state &= ~EP_STOP_CMD_PENDING;
+
+	ret = usbssp_disable_slot(usbssp_data);
+	if (ret)
+		usbssp_free_priv_device(usbssp_data);
+}
+
+int usbssp_disable_slot(struct usbssp_udc *usbssp_data)
+{
+	struct usbssp_command *command;
+	u32 state;
+	int ret = 0;
+
+	command = usbssp_alloc_command(usbssp_data, false, GFP_ATOMIC);
+	if (!command)
+		return -ENOMEM;
+
+	/* Don't disable the slot if the device controller is dead. */
+	state = readl(&usbssp_data->op_regs->status);
+	if (state == 0xffffffff ||
+	    (usbssp_data->usbssp_state & USBSSP_STATE_DYING) ||
+	    (usbssp_data->usbssp_state & USBSSP_STATE_HALTED)) {
+		kfree(command);
+		return -ENODEV;
+	}
+
+	ret = usbssp_queue_slot_control(usbssp_data, command, TRB_DISABLE_SLOT);
+	if (ret) {
+		kfree(command);
+		return ret;
+	}
+	usbssp_ring_cmd_db(usbssp_data);
+	return ret;
+}
+
+/*
+ * Returns 0 if the DC n out of device slots, the Enable Slot command
+ * timed out, or allocating memory failed. Returns 1 on success.
+ */
+int usbssp_alloc_dev(struct usbssp_udc *usbssp_data)
+{
+	int ret, slot_id;
+	struct usbssp_command *command;
+	struct usbssp_slot_ctx *slot_ctx;
+
+	command = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+
+	if (!command)
+		return -ENOMEM;
+
+	ret = usbssp_queue_slot_control(usbssp_data, command, TRB_ENABLE_SLOT);
+
+	if (ret) {
+		usbssp_free_command(usbssp_data, command);
+		return ret;
+	}
+
+	usbssp_ring_cmd_db(usbssp_data);
+	spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+				usbssp_data->irq_thread_flag);
+	wait_for_completion(command->completion);
+	spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+
+	slot_id = usbssp_data->slot_id;
+
+	if (!slot_id || command->status != COMP_SUCCESS) {
+		dev_err(usbssp_data->dev,
+			"Error while assigning device slot ID\n");
+		usbssp_free_command(usbssp_data, command);
+		return 0;
+	}
+
+	usbssp_free_command(usbssp_data, command);
+
+	if (!usbssp_alloc_priv_device(usbssp_data, GFP_ATOMIC)) {
+		dev_warn(usbssp_data->dev,
+			"Could not allocate usbssp_device data structures\n");
+		goto disable_slot;
+	}
+
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, usbssp_data->devs.out_ctx);
+	trace_usbssp_alloc_dev(slot_ctx);
+
+	return 1;
+
+disable_slot:
+	ret = usbssp_disable_slot(usbssp_data);
+	if (ret)
+		usbssp_free_priv_device(usbssp_data);
+
+	return 0;
+}
+
 int usbssp_gen_setup(struct usbssp_udc *usbssp_data)
 {
 	int retval;
@@ -420,7 +595,6 @@ int usbssp_gadget_exit(struct usbssp_udc *usbssp_data)
 
 	usb_del_gadget_udc(&usbssp_data->gadget);
 	usbssp_gadget_free_endpoint(usbssp_data);
-	/*TODO: add usbssp_stop implementation*/
-	//usbssp_stop(usbssp_data);
+	usbssp_stop(usbssp_data);
 	return ret;
 }
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 145371eee47d..1827781125bd 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1685,6 +1685,8 @@ void usbssp_dbg_trace(struct usbssp_udc *usbssp_data,
 /* USBSSP memory management */
 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_last_valid_endpoint(u32 added_ctxs);
 int usbssp_ring_expansion(struct usbssp_udc *usbssp_data,
 			struct usbssp_ring *ring,
@@ -1712,12 +1714,15 @@ int usbssp_handshake(void __iomem *ptr, u32 mask, u32 done, int usec);
 void usbssp_quiesce(struct usbssp_udc *usbssp_data);
 int usbssp_halt(struct usbssp_udc *usbssp_data);
 extern int usbssp_reset(struct usbssp_udc *usbssp_data);
+int usbssp_disable_slot(struct usbssp_udc *usbssp_data);
 
 int usbssp_suspend(struct usbssp_udc *usbssp_data, bool do_wakeup);
 int usbssp_resume(struct usbssp_udc *usbssp_data, bool hibernated);
 
 irqreturn_t usbssp_irq(int irq, void *priv);
 
+int usbssp_alloc_dev(struct usbssp_udc *usbssp_data);
+void usbssp_free_dev(struct usbssp_udc *usbssp_data);
 /* USBSSP ring, segment, TRB, and TD functions */
 dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
 				union usbssp_trb *trb);
@@ -1726,6 +1731,12 @@ struct usbssp_segment *usbssp_trb_in_td(struct usbssp_udc *usbssp_data,
 				union usbssp_trb *start_trb,
 				union usbssp_trb *end_trb,
 				dma_addr_t suspect_dma, bool debug);
+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);
 void usbssp_handle_command_timeout(struct work_struct *work);
 
 void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data);
@@ -1753,6 +1764,12 @@ void usbssp_gadget_free_endpoint(struct usbssp_udc *usbssp_data);
 int usbssp_gadget_init_endpoint(struct usbssp_udc *usbssp_data);
 unsigned int usbssp_port_speed(unsigned int port_status);
 void usbssp_gadget_reset_interrupt(struct usbssp_udc *usbssp_data);
+void usbssp_gadget_disconnect_interrupt(struct usbssp_udc *usbssp_data);
+int usbssp_stop_device(struct usbssp_udc *usbssp_data, int suspend);
+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);
 
 static inline char *usbssp_slot_state_string(u32 state)
 {
