[PATCH 24/31] usb: usbssp: added detecting command timeout.

[Pawel Laszczak]

Patch add functionality responsible for detecting command timeout.
Because command can fail without any command completion event, driver
must handles such case in proper way. For this reason, driver implements
mechanism measuring the command time. If time expired driver aborts
last performed command.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-ring.c | 157 ++++++++++++++++++++++++++++++-
 drivers/usb/usbssp/gadget.h      |   1 +
 2 files changed, 154 insertions(+), 4 deletions(-)

diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index 8cc6e4247eef..e7afd185d4ad 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -289,6 +289,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;
+
+		dev_dbg(usbssp_data->dev, "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;
+
+	dev_dbg(usbssp_data->dev, "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) {
+		dev_err(usbssp_data->dev,
+			"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) {
+		dev_dbg(usbssp_data->dev,
+			"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)
@@ -1060,10 +1157,6 @@ static void usbssp_handle_cmd_reset_dev(struct usbssp_udc *usbssp_data,
 		dev_warn(usbssp_data->dev, "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)
@@ -1086,6 +1179,62 @@ 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;
+		dev_dbg(usbssp_data->dev, "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) {
+		dev_dbg(usbssp_data->dev, "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 */
+	dev_dbg(usbssp_data->dev, "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 c4e440db6b23..6737cb05cd27 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1803,6 +1803,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);
-- 
2.17.1