iwlagn: add "echo" test when command queue stuck

When detect command queue stuck, instead of reload the firmware
do the "echo" test to make sure it is really stuck before reload

Signed-off-by: Wey-Yi Guy <wey-yi.w.guy@intel.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/iwlwifi/iwl-core.c

@@ -1732,10 +1732,31 @@ int iwl_mac_change_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	return err;
 }
 
+int iwl_cmd_echo_test(struct iwl_priv *priv)
+{
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_ECHO,
+		.flags = CMD_SYNC,
+	};
+
+	return iwl_trans_send_cmd(trans(priv), &cmd);
+}
+
 static inline int iwl_check_stuck_queue(struct iwl_priv *priv, int txq)
 {
 	if (iwl_trans_check_stuck_queue(trans(priv), txq)) {
-		int ret = iwl_force_reset(priv, IWL_FW_RESET, false);
+		int ret;
+		if (txq == priv->shrd->cmd_queue) {
+			/*
+			 * validate command queue still working
+			 * by sending "ECHO" command
+			 */
+			if (!iwl_cmd_echo_test(priv))
+				return 0;
+			else
+				IWL_DEBUG_HC(priv, "echo testing fail\n");
+		}
+		ret = iwl_force_reset(priv, IWL_FW_RESET, false);
 		return (ret == -EAGAIN) ? 0 : 1;
 	}
 	return 0;

drivers/net/wireless/iwlwifi/iwl-core.h

@@ -266,6 +266,7 @@ void iwl_mac_remove_interface(struct ieee80211_hw *hw,
 int iwl_mac_change_interface(struct ieee80211_hw *hw,
 			     struct ieee80211_vif *vif,
 			     enum nl80211_iftype newtype, bool newp2p);
+int iwl_cmd_echo_test(struct iwl_priv *priv);
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 int iwl_alloc_traffic_mem(struct iwl_priv *priv);
 void iwl_free_traffic_mem(struct iwl_priv *priv);