mwifiex: separate out response buffer parsing code

This new function will be useful later for extended scan
feature.
Signed-off-by: Amitkumar Karwar <akarwar@marvell.com>
Signed-off-by: Bing Zhao <bzhao@marvell.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/mwifiex/scan.c

@@ -1576,112 +1576,19 @@ static int mwifiex_update_curr_bss_params(struct mwifiex_private *priv,
 	return 0;
 }
 
-/*
- * This function handles the command response of scan.
- *
- * The response buffer for the scan command has the following
- * memory layout:
- *
- *      .-------------------------------------------------------------.
- *      |  Header (4 * sizeof(t_u16)):  Standard command response hdr |
- *      .-------------------------------------------------------------.
- *      |  BufSize (t_u16) : sizeof the BSS Description data          |
- *      .-------------------------------------------------------------.
- *      |  NumOfSet (t_u8) : Number of BSS Descs returned             |
- *      .-------------------------------------------------------------.
- *      |  BSSDescription data (variable, size given in BufSize)      |
- *      .-------------------------------------------------------------.
- *      |  TLV data (variable, size calculated using Header->Size,    |
- *      |            BufSize and sizeof the fixed fields above)       |
- *      .-------------------------------------------------------------.
- */
-int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
-			    struct host_cmd_ds_command *resp)
+static int
+mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
+				  u32 *bytes_left, u64 fw_tsf, u8 *radio_type,
+				  bool ext_scan)
 {
-	int ret = 0;
 	struct mwifiex_adapter *adapter = priv->adapter;
-	struct cmd_ctrl_node *cmd_node;
-	struct host_cmd_ds_802_11_scan_rsp *scan_rsp;
-	struct mwifiex_ie_types_data *tlv_data;
-	struct mwifiex_ie_types_tsf_timestamp *tsf_tlv;
-	u8 *bss_info;
-	u32 scan_resp_size;
-	u32 bytes_left;
-	u32 idx;
-	u32 tlv_buf_size;
 	struct mwifiex_chan_freq_power *cfp;
-	struct mwifiex_ie_types_chan_band_list_param_set *chan_band_tlv;
-	struct chan_band_param_set *chan_band;
-	u8 is_bgscan_resp;
-	unsigned long flags;
 	struct cfg80211_bss *bss;
-
-	is_bgscan_resp = (le16_to_cpu(resp->command)
-			  == HostCmd_CMD_802_11_BG_SCAN_QUERY);
-	if (is_bgscan_resp)
-		scan_rsp = &resp->params.bg_scan_query_resp.scan_resp;
-	else
-		scan_rsp = &resp->params.scan_resp;
-
-
-	if (scan_rsp->number_of_sets > MWIFIEX_MAX_AP) {
-		dev_err(adapter->dev, "SCAN_RESP: too many AP returned (%d)\n",
-			scan_rsp->number_of_sets);
-		ret = -1;
-		goto check_next_scan;
-	}
-
-	/* Check csa channel expiry before parsing scan response */
-	mwifiex_11h_get_csa_closed_channel(priv);
-
-	bytes_left = le16_to_cpu(scan_rsp->bss_descript_size);
-	dev_dbg(adapter->dev, "info: SCAN_RESP: bss_descript_size %d\n",
-		bytes_left);
-
-	scan_resp_size = le16_to_cpu(resp->size);
-
-	dev_dbg(adapter->dev,
-		"info: SCAN_RESP: returned %d APs before parsing\n",
-		scan_rsp->number_of_sets);
-
-	bss_info = scan_rsp->bss_desc_and_tlv_buffer;
-
-	/*
-	 * The size of the TLV buffer is equal to the entire command response
-	 *   size (scan_resp_size) minus the fixed fields (sizeof()'s), the
-	 *   BSS Descriptions (bss_descript_size as bytesLef) and the command
-	 *   response header (S_DS_GEN)
-	 */
-	tlv_buf_size = scan_resp_size - (bytes_left
-					 + sizeof(scan_rsp->bss_descript_size)
-					 + sizeof(scan_rsp->number_of_sets)
-					 + S_DS_GEN);
-
-	tlv_data = (struct mwifiex_ie_types_data *) (scan_rsp->
-						 bss_desc_and_tlv_buffer +
-						 bytes_left);
-
-	/* Search the TLV buffer space in the scan response for any valid
-	   TLVs */
-	mwifiex_ret_802_11_scan_get_tlv_ptrs(adapter, tlv_data, tlv_buf_size,
-					     TLV_TYPE_TSFTIMESTAMP,
-					     (struct mwifiex_ie_types_data **)
-					     &tsf_tlv);
-
-	/* Search the TLV buffer space in the scan response for any valid
-	   TLVs */
-	mwifiex_ret_802_11_scan_get_tlv_ptrs(adapter, tlv_data, tlv_buf_size,
-					     TLV_TYPE_CHANNELBANDLIST,
-					     (struct mwifiex_ie_types_data **)
-					     &chan_band_tlv);
-
-	for (idx = 0; idx < scan_rsp->number_of_sets && bytes_left; idx++) {
 	u8 bssid[ETH_ALEN];
 	s32 rssi;
 	const u8 *ie_buf;
 	size_t ie_len;
 	u16 channel = 0;
-		__le64 fw_tsf = 0;
 	u16 beacon_size = 0;
 	u32 curr_bcn_bytes;
 	u32 freq;
@@ -1692,27 +1599,27 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 	struct mwifiex_fixed_bcn_param *bcn_param;
 	struct mwifiex_bss_priv *bss_priv;
 
-		if (bytes_left >= sizeof(beacon_size)) {
+	if (*bytes_left >= sizeof(beacon_size)) {
 		/* Extract & convert beacon size from command buffer */
-			memcpy(&beacon_size, bss_info, sizeof(beacon_size));
-			bytes_left -= sizeof(beacon_size);
-			bss_info += sizeof(beacon_size);
+		memcpy(&beacon_size, *bss_info, sizeof(beacon_size));
+		*bytes_left -= sizeof(beacon_size);
+		*bss_info += sizeof(beacon_size);
 	}
 
-		if (!beacon_size || beacon_size > bytes_left) {
-			bss_info += bytes_left;
-			bytes_left = 0;
-			ret = -1;
-			goto check_next_scan;
+	if (!beacon_size || beacon_size > *bytes_left) {
+		*bss_info += *bytes_left;
+		*bytes_left = 0;
+		return -EFAULT;
 	}
 
 	/* Initialize the current working beacon pointer for this BSS
-		 * iteration */
-		current_ptr = bss_info;
+	 * iteration
+	 */
+	current_ptr = *bss_info;
 
 	/* Advance the return beacon pointer past the current beacon */
-		bss_info += beacon_size;
-		bytes_left -= beacon_size;
+	*bss_info += beacon_size;
+	*bytes_left -= beacon_size;
 
 	curr_bcn_bytes = beacon_size;
 
@@ -1721,20 +1628,21 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 	 */
 	if (curr_bcn_bytes < ETH_ALEN + sizeof(u8) +
 	    sizeof(struct mwifiex_fixed_bcn_param)) {
-			dev_err(adapter->dev,
-				"InterpretIE: not enough bytes left\n");
-			continue;
+		dev_err(adapter->dev, "InterpretIE: not enough bytes left\n");
+		return -EFAULT;
 	}
 
 	memcpy(bssid, current_ptr, ETH_ALEN);
 	current_ptr += ETH_ALEN;
 	curr_bcn_bytes -= ETH_ALEN;
 
+	if (!ext_scan) {
 		rssi = (s32) *(u8 *)current_ptr;
 		rssi = (-rssi) * 100;		/* Convert dBm to mBm */
 		current_ptr += sizeof(u8);
 		curr_bcn_bytes -= sizeof(u8);
 		dev_dbg(adapter->dev, "info: InterpretIE: RSSI=%d\n", rssi);
+	}
 
 	bcn_param = (struct mwifiex_fixed_bcn_param *)current_ptr;
 	current_ptr += sizeof(*bcn_param);
@@ -1750,8 +1658,7 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 	/* Rest of the current buffer are IE's */
 	ie_buf = current_ptr;
 	ie_len = curr_bcn_bytes;
-		dev_dbg(adapter->dev,
-			"info: InterpretIE: IELength for this AP = %d\n",
+	dev_dbg(adapter->dev, "info: InterpretIE: IELength for this AP = %d\n",
 		curr_bcn_bytes);
 
 	while (curr_bcn_bytes >= sizeof(struct ieee_types_header)) {
@@ -1761,32 +1668,21 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 		element_len = *(current_ptr + 1);
 		if (curr_bcn_bytes < element_len +
 				sizeof(struct ieee_types_header)) {
-				dev_err(priv->adapter->dev,
-					"%s: bytes left < IE length\n",
-					__func__);
-				goto check_next_scan;
+			dev_err(adapter->dev,
+				"%s: bytes left < IE length\n", __func__);
+			return -EFAULT;
 		}
 		if (element_id == WLAN_EID_DS_PARAMS) {
-				channel = *(current_ptr + sizeof(struct ieee_types_header));
+			channel = *(current_ptr +
+				    sizeof(struct ieee_types_header));
 			break;
 		}
 
-			current_ptr += element_len +
-					sizeof(struct ieee_types_header);
+		current_ptr += element_len + sizeof(struct ieee_types_header);
 		curr_bcn_bytes -= element_len +
 					sizeof(struct ieee_types_header);
 	}
 
-		/*
-		 * If the TSF TLV was appended to the scan results, save this
-		 * entry's TSF value in the fw_tsf field. It is the firmware's
-		 * TSF value at the time the beacon or probe response was
-		 * received.
-		 */
-		if (tsf_tlv)
-			memcpy(&fw_tsf, &tsf_tlv->tsf_data[idx * TSF_DATA_SIZE],
-			       sizeof(fw_tsf));
-
 	if (channel) {
 		struct ieee80211_channel *chan;
 		u8 band;
@@ -1795,17 +1691,13 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 		if (channel == priv->csa_chan) {
 			dev_dbg(adapter->dev,
 				"Dropping entry on csa closed channel\n");
-				continue;
+			return 0;
 		}
 
 		band = BAND_G;
-			if (chan_band_tlv) {
-				chan_band =
-					&chan_band_tlv->chan_band_param[idx];
-				band = mwifiex_radio_type_to_band(
-						chan_band->radio_type
-						& (BIT(0) | BIT(1)));
-			}
+		if (radio_type)
+			band = mwifiex_radio_type_to_band(*radio_type &
+							  (BIT(0) | BIT(1)));
 
 		cfp = mwifiex_get_cfp(priv, band, channel, 0);
 
@@ -1820,18 +1712,143 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 					    ie_buf, ie_len, rssi, GFP_KERNEL);
 			bss_priv = (struct mwifiex_bss_priv *)bss->priv;
 			bss_priv->band = band;
-				bss_priv->fw_tsf = le64_to_cpu(fw_tsf);
+			bss_priv->fw_tsf = fw_tsf;
 			if (priv->media_connected &&
-				    !memcmp(bssid,
-					    priv->curr_bss_params.bss_descriptor
+			    !memcmp(bssid, priv->curr_bss_params.bss_descriptor
 				    .mac_address, ETH_ALEN))
-					mwifiex_update_curr_bss_params(priv,
-								       bss);
+				mwifiex_update_curr_bss_params(priv, bss);
 			cfg80211_put_bss(priv->wdev->wiphy, bss);
 		}
 	} else {
 		dev_dbg(adapter->dev, "missing BSS channel IE\n");
 	}
+
+	return 0;
+}
+
+/*
+ * This function handles the command response of scan.
+ *
+ * The response buffer for the scan command has the following
+ * memory layout:
+ *
+ *      .-------------------------------------------------------------.
+ *      |  Header (4 * sizeof(t_u16)):  Standard command response hdr |
+ *      .-------------------------------------------------------------.
+ *      |  BufSize (t_u16) : sizeof the BSS Description data          |
+ *      .-------------------------------------------------------------.
+ *      |  NumOfSet (t_u8) : Number of BSS Descs returned             |
+ *      .-------------------------------------------------------------.
+ *      |  BSSDescription data (variable, size given in BufSize)      |
+ *      .-------------------------------------------------------------.
+ *      |  TLV data (variable, size calculated using Header->Size,    |
+ *      |            BufSize and sizeof the fixed fields above)       |
+ *      .-------------------------------------------------------------.
+ */
+int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
+			    struct host_cmd_ds_command *resp)
+{
+	int ret = 0;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct cmd_ctrl_node *cmd_node;
+	struct host_cmd_ds_802_11_scan_rsp *scan_rsp;
+	struct mwifiex_ie_types_data *tlv_data;
+	struct mwifiex_ie_types_tsf_timestamp *tsf_tlv;
+	u8 *bss_info;
+	u32 scan_resp_size;
+	u32 bytes_left;
+	u32 idx;
+	u32 tlv_buf_size;
+	struct mwifiex_ie_types_chan_band_list_param_set *chan_band_tlv;
+	struct chan_band_param_set *chan_band;
+	u8 is_bgscan_resp;
+	unsigned long flags;
+	__le64 fw_tsf = 0;
+	u8 *radio_type;
+
+	is_bgscan_resp = (le16_to_cpu(resp->command)
+			  == HostCmd_CMD_802_11_BG_SCAN_QUERY);
+	if (is_bgscan_resp)
+		scan_rsp = &resp->params.bg_scan_query_resp.scan_resp;
+	else
+		scan_rsp = &resp->params.scan_resp;
+
+
+	if (scan_rsp->number_of_sets > MWIFIEX_MAX_AP) {
+		dev_err(adapter->dev, "SCAN_RESP: too many AP returned (%d)\n",
+			scan_rsp->number_of_sets);
+		ret = -1;
+		goto check_next_scan;
+	}
+
+	/* Check csa channel expiry before parsing scan response */
+	mwifiex_11h_get_csa_closed_channel(priv);
+
+	bytes_left = le16_to_cpu(scan_rsp->bss_descript_size);
+	dev_dbg(adapter->dev, "info: SCAN_RESP: bss_descript_size %d\n",
+		bytes_left);
+
+	scan_resp_size = le16_to_cpu(resp->size);
+
+	dev_dbg(adapter->dev,
+		"info: SCAN_RESP: returned %d APs before parsing\n",
+		scan_rsp->number_of_sets);
+
+	bss_info = scan_rsp->bss_desc_and_tlv_buffer;
+
+	/*
+	 * The size of the TLV buffer is equal to the entire command response
+	 *   size (scan_resp_size) minus the fixed fields (sizeof()'s), the
+	 *   BSS Descriptions (bss_descript_size as bytesLef) and the command
+	 *   response header (S_DS_GEN)
+	 */
+	tlv_buf_size = scan_resp_size - (bytes_left
+					 + sizeof(scan_rsp->bss_descript_size)
+					 + sizeof(scan_rsp->number_of_sets)
+					 + S_DS_GEN);
+
+	tlv_data = (struct mwifiex_ie_types_data *) (scan_rsp->
+						 bss_desc_and_tlv_buffer +
+						 bytes_left);
+
+	/* Search the TLV buffer space in the scan response for any valid
+	   TLVs */
+	mwifiex_ret_802_11_scan_get_tlv_ptrs(adapter, tlv_data, tlv_buf_size,
+					     TLV_TYPE_TSFTIMESTAMP,
+					     (struct mwifiex_ie_types_data **)
+					     &tsf_tlv);
+
+	/* Search the TLV buffer space in the scan response for any valid
+	   TLVs */
+	mwifiex_ret_802_11_scan_get_tlv_ptrs(adapter, tlv_data, tlv_buf_size,
+					     TLV_TYPE_CHANNELBANDLIST,
+					     (struct mwifiex_ie_types_data **)
+					     &chan_band_tlv);
+
+	for (idx = 0; idx < scan_rsp->number_of_sets && bytes_left; idx++) {
+		/*
+		 * If the TSF TLV was appended to the scan results, save this
+		 * entry's TSF value in the fw_tsf field. It is the firmware's
+		 * TSF value at the time the beacon or probe response was
+		 * received.
+		 */
+		if (tsf_tlv)
+			memcpy(&fw_tsf, &tsf_tlv->tsf_data[idx * TSF_DATA_SIZE],
+			       sizeof(fw_tsf));
+
+		if (chan_band_tlv) {
+			chan_band = &chan_band_tlv->chan_band_param[idx];
+			radio_type = &chan_band->radio_type;
+		} else {
+			radio_type = NULL;
+		}
+
+		ret = mwifiex_parse_single_response_buf(priv, &bss_info,
+							&bytes_left,
+							le64_to_cpu(fw_tsf),
+							radio_type, false);
+		if (ret)
+			goto check_next_scan;
 	}
 
 check_next_scan: