2 files changed, 23 insertions, 1 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index 0725603dbf1d..59d1968e6ae9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/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;
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index db50b650756c..080c35543881 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/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);