1 files changed, 38 insertions, 14 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-scan.c b/drivers/net/wireless/iwlwifi/iwl-scan.c
index a8437a6bc18e..031d8e21f82f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-scan.c
+++ b/drivers/net/wireless/iwlwifi/iwl-scan.c
@@ -52,6 +52,7 @@
 #define IWL_PASSIVE_DWELL_TIME_52   (10)
 #define IWL_PASSIVE_DWELL_BASE      (100)
 #define IWL_CHANNEL_TUNE_TIME       5
+#define MAX_SCAN_CHANNEL            50
 
 static int iwl_send_scan_abort(struct iwl_priv *priv)
 {
@@ -616,7 +617,8 @@ static int iwl_get_channels_for_scan(struct iwl_priv *priv,
  */
 
 static u16 iwl_fill_probe_req(struct ieee80211_mgmt *frame, const u8 *ta,
-                              const u8 *ies, int ie_len, int left)
+                              const u8 *ies, int ie_len, const u8 *ssid,
+                              u8 ssid_len, int left)
 {
         int len = 0;
         u8 *pos = NULL;
@@ -638,14 +640,18 @@ static u16 iwl_fill_probe_req(struct ieee80211_mgmt *frame, const u8 *ta,
         /* ...next IE... */
         pos = &frame->u.probe_req.variable[0];
 
-        /* fill in our indirect SSID IE */
-        left -= 2;
+        /* fill in our SSID IE */
+        left -= ssid_len + 2;
         if (left < 0)
                 return 0;
         *pos++ = WLAN_EID_SSID;
-        *pos++ = 0;
+        *pos++ = ssid_len;
+        if (ssid && ssid_len) {
+                memcpy(pos, ssid, ssid_len);
+                pos += ssid_len;
+        }
 
-        len += 2;
+        len += ssid_len + 2;
 
         if (WARN_ON(left < ie_len))
                 return len;
@@ -679,6 +685,15 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
         u8 active_chains;
         u8 scan_tx_antennas = priv->hw_params.valid_tx_ant;
         int ret;
+        int scan_cmd_size = sizeof(struct iwl_scan_cmd) +
+                            MAX_SCAN_CHANNEL * sizeof(struct iwl_scan_channel) +
+                            priv->fw->ucode_capa.max_probe_length;
+        const u8 *ssid = NULL;
+        u8 ssid_len = 0;
+
+        if (WARN_ON_ONCE(priv->scan_request &&
+                         priv->scan_request->n_channels > MAX_SCAN_CHANNEL))
+                return -EINVAL;
 
         lockdep_assert_held(&priv->mutex);
 
@@ -686,8 +701,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
                 ctx = iwl_rxon_ctx_from_vif(vif);
 
         if (!priv->scan_cmd) {
-                priv->scan_cmd = kmalloc(sizeof(struct iwl_scan_cmd) +
-                                         IWL_MAX_SCAN_SIZE, GFP_KERNEL);
+                priv->scan_cmd = kmalloc(scan_cmd_size, GFP_KERNEL);
                 if (!priv->scan_cmd) {
                         IWL_DEBUG_SCAN(priv,
                                        "fail to allocate memory for scan\n");
@@ -695,7 +709,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
                 }
         }
         scan = priv->scan_cmd;
-        memset(scan, 0, sizeof(struct iwl_scan_cmd) + IWL_MAX_SCAN_SIZE);
+        memset(scan, 0, scan_cmd_size);
 
         scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
         scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
@@ -746,10 +760,18 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
                 if (priv->scan_request->n_ssids) {
                         int i, p = 0;
                         IWL_DEBUG_SCAN(priv, "Kicking off active scan\n");
-                        for (i = 0; i < priv->scan_request->n_ssids; i++) {
-                                /* always does wildcard anyway */
-                                if (!priv->scan_request->ssids[i].ssid_len)
-                                        continue;
+                        /*
+                         * The highest priority SSID is inserted to the
+                         * probe request template.
+                         */
+                        ssid_len = priv->scan_request->ssids[0].ssid_len;
+                        ssid = priv->scan_request->ssids[0].ssid;
+
+                        /*
+                         * Invert the order of ssids, the firmware will invert
+                         * it back.
+                         */
+                        for (i = priv->scan_request->n_ssids - 1; i >= 1; i--) {
                                 scan->direct_scan[p].id = WLAN_EID_SSID;
                                 scan->direct_scan[p].len =
                                         priv->scan_request->ssids[i].ssid_len;
@@ -883,7 +905,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
                                         vif->addr,
                                         priv->scan_request->ie,
                                         priv->scan_request->ie_len,
-                                        IWL_MAX_SCAN_SIZE - sizeof(*scan));
+                                        ssid, ssid_len,
+                                        scan_cmd_size - sizeof(*scan));
                 break;
         case IWL_SCAN_RADIO_RESET:
         case IWL_SCAN_ROC:
@@ -891,7 +914,8 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
                 cmd_len = iwl_fill_probe_req(
                                         (struct ieee80211_mgmt *)scan->data,
                                         iwl_bcast_addr, NULL, 0,
-                                        IWL_MAX_SCAN_SIZE - sizeof(*scan));
+                                        NULL, 0,
+                                        scan_cmd_size - sizeof(*scan));
                 break;
         default:
                 BUG();