3 files changed, 38 insertions, 22 deletions

diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index 4c22e7849515..ef63c376c552 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -667,6 +667,12 @@ int lbs_process_rx_command(struct lbs_private *priv)
 
 	/* Now we got response from FW, cancel the command timer */
 	del_timer(&priv->command_timer);
+	priv->cmd_timed_out = 0;
+	if (priv->nr_retries) {
+		lbs_pr_info("Received result %x to command %x after %d retries\n",
+			    result, curcmd, priv->nr_retries);
+		priv->nr_retries = 0;
+	}
 
 	/* Store the response code to cur_cmd_retcode. */
 	priv->cur_cmd_retcode = result;
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index e6f553d5d2cf..465080fd0607 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -201,6 +201,8 @@ struct lbs_private {
 
 	/** Timers */
 	struct timer_list command_timer;
+	int nr_retries;
+	int cmd_timed_out;
 
 	u8 hisregcpy;
 
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 839ffe818c18..9677b0d77160 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -670,6 +670,8 @@ static int lbs_thread(void *data)
 			shouldsleep = 1;	/* Sleep mode. Nothing we can do till it wakes */
 		else if (priv->intcounter)
 			shouldsleep = 0;	/* Interrupt pending. Deal with it now */
+		else if (priv->cmd_timed_out)
+			shouldsleep = 0;	/* Command timed out. Recover */
 		else if (!priv->fw_ready)
 			shouldsleep = 1;	/* Firmware not ready. We're waiting for it */
 		else if (priv->dnld_sent)
@@ -740,6 +742,26 @@ static int lbs_thread(void *data)
 			spin_lock_irq(&priv->driver_lock);
 		}
 
+		if (priv->cmd_timed_out && priv->cur_cmd) {
+			struct cmd_ctrl_node *cmdnode = priv->cur_cmd;
+
+			if (++priv->nr_retries > 10) {
+				lbs_pr_info("Excessive timeouts submitting command %x\n",
+					    le16_to_cpu(cmdnode->cmdbuf->command));
+				lbs_complete_command(priv, cmdnode, -ETIMEDOUT);
+				priv->nr_retries = 0;
+			} else {
+				priv->cur_cmd = NULL;
+				lbs_pr_info("requeueing command %x due to timeout (#%d)\n",
+					    le16_to_cpu(cmdnode->cmdbuf->command), priv->nr_retries);
+
+				/* Stick it back at the _top_ of the pending queue
+				   for immediate resubmission */
+				list_add(&cmdnode->list, &priv->cmdpendingq);
+			}
+		}
+		priv->cmd_timed_out = 0;
+
 		/* Any Card Event */
 		if (priv->hisregcpy & MRVDRV_CARDEVENT) {
 			lbs_deb_thread("main-thread: Card Event Activity\n");
@@ -922,35 +944,21 @@ done:
 static void command_timer_fn(unsigned long data)
 {
 	struct lbs_private *priv = (struct lbs_private *)data;
-	struct cmd_ctrl_node *node;
 	unsigned long flags;
 
-	node = priv->cur_cmd;
-	if (node == NULL) {
-		lbs_deb_fw("ptempnode empty\n");
-		return;
-	}
+	spin_lock_irqsave(&priv->driver_lock, flags);
 
-	if (!node->cmdbuf) {
-		lbs_deb_fw("cmd is NULL\n");
-		return;
+	if (!priv->cur_cmd) {
+		lbs_pr_info("Command timer expired; no pending command\n");
+		goto out;
 	}
 
-	lbs_pr_info("command %x timed out\n", le16_to_cpu(node->cmdbuf->command));
-
-	if (!priv->fw_ready)
-		return;
-
-	spin_lock_irqsave(&priv->driver_lock, flags);
-	priv->cur_cmd = NULL;
-	spin_unlock_irqrestore(&priv->driver_lock, flags);
-
-	lbs_deb_fw("re-sending same command because of timeout\n");
-	lbs_queue_cmd(priv, node, 0);
+	lbs_pr_info("Command %x timed out\n", le16_to_cpu(priv->cur_cmd->cmdbuf->command));
 
+	priv->cmd_timed_out = 1;
 	wake_up_interruptible(&priv->waitq);
-
-	return;
+ out:
+	spin_unlock_irqrestore(&priv->driver_lock, flags);
 }
 
 static int lbs_init_adapter(struct lbs_private *priv)