diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index 989d096f64ac..f942c19fdfc4 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -288,6 +288,103 @@ static bool usbssp_mod_cmd_timer(struct usbssp_udc *usbssp_data,
 	return 0;
 }
 
+static struct usbssp_command *usbssp_next_queued_cmd(
+		struct usbssp_udc *usbssp_data)
+{
+	return list_first_entry_or_null(&usbssp_data->cmd_list,
+					struct usbssp_command,
+					cmd_list);
+}
+
+/*
+ * Turn all commands on command ring with status set to "aborted" to no-op trbs.
+ * If there are other commands waiting then restart the ring and kick the timer.
+ * This must be called with command ring stopped and usbssp_data->lock held.
+ */
+static void usbssp_handle_stopped_cmd_ring(struct usbssp_udc *usbssp_data,
+					   struct usbssp_command *cur_cmd)
+{
+	struct usbssp_command *i_cmd;
+
+	/* Turn all aborted commands in list to no-ops, then restart */
+	list_for_each_entry(i_cmd, &usbssp_data->cmd_list, cmd_list) {
+
+		if (i_cmd->status != COMP_COMMAND_ABORTED)
+			continue;
+
+		i_cmd->status = COMP_COMMAND_RING_STOPPED;
+
+		usbssp_dbg(usbssp_data, "Turn aborted command %p to no-op\n",
+			 i_cmd->command_trb);
+
+		trb_to_noop(i_cmd->command_trb, TRB_CMD_NOOP);
+
+		/*
+		 * caller waiting for completion is called when command
+		 *  completion event is received for these no-op commands
+		 */
+	}
+
+	usbssp_data->cmd_ring_state = CMD_RING_STATE_RUNNING;
+
+	/* ring command ring doorbell to restart the command ring */
+	if ((usbssp_data->cmd_ring->dequeue != usbssp_data->cmd_ring->enqueue) &&
+	    !(usbssp_data->usbssp_state & USBSSP_STATE_DYING)) {
+		usbssp_data->current_cmd = cur_cmd;
+		usbssp_mod_cmd_timer(usbssp_data, USBSSP_CMD_DEFAULT_TIMEOUT);
+		usbssp_ring_cmd_db(usbssp_data);
+	}
+}
+
+/* Must be called with usbssp_data->lock held, releases and aquires lock back */
+static int usbssp_abort_cmd_ring(struct usbssp_udc *usbssp_data,
+				 unsigned long flags)
+{
+	u64 temp_64;
+	int ret;
+
+	usbssp_dbg(usbssp_data, "Abort command ring\n");
+	reinit_completion(&usbssp_data->cmd_ring_stop_completion);
+
+	temp_64 = usbssp_read_64(usbssp_data, &usbssp_data->op_regs->cmd_ring);
+	usbssp_write_64(usbssp_data, temp_64 | CMD_RING_ABORT,
+			&usbssp_data->op_regs->cmd_ring);
+
+	/* Spec says software should also time the
+	 * completion of the Command Abort operation. If CRR is not negated in 5
+	 * seconds then driver handles it as if device died (-ENODEV).
+	 */
+	ret = usbssp_handshake(&usbssp_data->op_regs->cmd_ring,
+			CMD_RING_RUNNING, 0, 5 * 1000 * 1000);
+
+	if (ret < 0) {
+		usbssp_err(usbssp_data,
+				"Abort failed to stop command ring: %d\n", ret);
+		usbssp_halt(usbssp_data);
+		usbssp_udc_died(usbssp_data);
+		return ret;
+	}
+
+	/*
+	 * Writing the CMD_RING_ABORT bit should cause a cmd completion event,
+	 * Wait 2 secs (arbitrary number).
+	 */
+	spin_unlock_irqrestore(&usbssp_data->lock, flags);
+	ret = wait_for_completion_timeout(
+			&usbssp_data->cmd_ring_stop_completion,
+			msecs_to_jiffies(2000));
+	spin_lock_irqsave(&usbssp_data->lock, flags);
+	if (!ret) {
+		usbssp_dbg(usbssp_data,
+				"No stop event for abort, ring start fail?\n");
+		usbssp_cleanup_command_queue(usbssp_data);
+	} else {
+		usbssp_handle_stopped_cmd_ring(usbssp_data,
+				usbssp_next_queued_cmd(usbssp_data));
+	}
+	return 0;
+}
+
 void usbssp_ring_ep_doorbell(struct usbssp_udc *usbssp_data,
 		unsigned int ep_index,
 		unsigned int stream_id)
@@ -1056,10 +1153,6 @@ static void usbssp_handle_cmd_reset_dev(struct usbssp_udc *usbssp_data,
 		usbssp_warn(usbssp_data, "Reset device command completion\n");
 }
 
-void usbssp_handle_command_timeout(struct work_struct *work)
-{
-	/*TODO: implements function*/
-}
 
 static void usbssp_complete_del_and_free_cmd(struct usbssp_command *cmd,
 					     u32 status)
@@ -1082,6 +1175,61 @@ void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data)
 		usbssp_complete_del_and_free_cmd(cur_cmd, COMP_COMMAND_ABORTED);
 }
 
+void usbssp_handle_command_timeout(struct work_struct *work)
+{
+	struct usbssp_udc *usbssp_data;
+	unsigned long flags;
+	u64 hw_ring_state;
+
+	usbssp_data = container_of(to_delayed_work(work), struct usbssp_udc,
+			cmd_timer);
+
+	spin_lock_irqsave(&usbssp_data->lock, flags);
+
+	/*
+	 * If timeout work is pending, or current_cmd is NULL, it means we
+	 * raced with command completion. Command is handled so just return.
+	 */
+	if (!usbssp_data->current_cmd ||
+	     delayed_work_pending(&usbssp_data->cmd_timer)) {
+		spin_unlock_irqrestore(&usbssp_data->lock, flags);
+		return;
+	}
+	/* mark this command to be cancelled */
+	usbssp_data->current_cmd->status = COMP_COMMAND_ABORTED;
+
+	/* Make sure command ring is running before aborting it */
+	hw_ring_state = usbssp_read_64(usbssp_data,
+			&usbssp_data->op_regs->cmd_ring);
+	if (hw_ring_state == ~(u64)0) {
+		usbssp_udc_died(usbssp_data);
+		goto time_out_completed;
+	}
+
+	if ((usbssp_data->cmd_ring_state & CMD_RING_STATE_RUNNING) &&
+	    (hw_ring_state & CMD_RING_RUNNING))  {
+		/* Prevent new doorbell, and start command abort */
+		usbssp_data->cmd_ring_state = CMD_RING_STATE_ABORTED;
+		usbssp_dbg(usbssp_data, "Command timeout\n");
+		usbssp_abort_cmd_ring(usbssp_data, flags);
+		goto time_out_completed;
+	}
+
+	/* device disconnected. Bail out */
+	if (usbssp_data->usbssp_state & USBSSP_STATE_REMOVING) {
+		usbssp_dbg(usbssp_data, "device removed, ring start fail?\n");
+		usbssp_cleanup_command_queue(usbssp_data);
+		goto time_out_completed;
+	}
+
+	/* command timeout on stopped ring, ring can't be aborted */
+	usbssp_dbg(usbssp_data, "Command timeout on stopped ring\n");
+	usbssp_handle_stopped_cmd_ring(usbssp_data, usbssp_data->current_cmd);
+
+time_out_completed:
+	spin_unlock_irqrestore(&usbssp_data->lock, flags);
+}
+
 static void handle_cmd_completion(struct usbssp_udc *usbssp_data,
 		struct usbssp_event_cmd *event)
 {
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 8512ef727b98..38ae684d71e7 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1786,6 +1786,7 @@ void usbssp_set_link_state(struct usbssp_udc *usbssp_data,
 void usbssp_test_and_clear_bit(struct usbssp_udc *usbssp_data,
 		__le32 __iomem *port_regs, u32 port_bit);
 
+void usbssp_udc_died(struct usbssp_udc *usbssp_data);
 /* USBSSP DC contexts */
 struct usbssp_input_control_ctx *usbssp_get_input_control_ctx(
 		struct usbssp_container_ctx *ctx);
