[PATCH 2/2 v2] driver_nl80211: handle P2P scans correctly

[Johannes Berg]

From: Johannes Berg <johannes.berg at intel.com>

To handle P2P scans, remove 11b rates from the list
of supported rates if present. To do that, we need
to build a list of supported rates first.

Signed-off-by: Johannes Berg <johannes.berg at intel.com>
---
Note: depends on new kernel patches and nl80211 API update

v2: failure/free paths -- thanks Eliad

 src/drivers/driver_nl80211.c |   79 +++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 76 insertions(+), 3 deletions(-)

--- a/src/drivers/driver_nl80211.c	2011-07-19 14:04:15.000000000 +0200
+++ b/src/drivers/driver_nl80211.c	2011-07-19 14:13:18.000000000 +0200
@@ -119,6 +119,11 @@ struct i802_bss {
 	unsigned int added_bridge:1;
 };
 
+struct nl80211_rates {
+	int nlband, n_rates;
+	u8 rates[NL80211_MAX_SUPP_RATES];
+};
+
 struct wpa_driver_nl80211_data {
 	struct nl80211_global *global;
 	struct dl_list list;
@@ -174,6 +179,9 @@ struct wpa_driver_nl80211_data {
 
 	struct i802_bss first_bss;
 
+	struct nl80211_rates *rates;
+	int num_bands;
+
 #ifdef HOSTAPD
 	int eapol_sock; /* socket for EAPOL frames */
 
@@ -2301,6 +2309,7 @@ static void wpa_driver_nl80211_deinit(vo
 	if (drv->global)
 		dl_list_del(&drv->list);
 
+	os_free(drv->rates);
 	os_free(drv);
 }
 
@@ -2378,6 +2387,36 @@ static int wpa_driver_nl80211_scan(void
 			params->extra_ies);
 	}
 
+	/*
+	 * filter out 11b rates
+	 * This relies on HW capabilities having been
+	 * queried before, which should always happen.
+	 */
+	if (params->p2p && drv->rates) {
+		void *rate_nest = nla_nest_start(msg, NL80211_ATTR_SCAN_SUPP_RATES);
+		u8 rates[NL80211_MAX_SUPP_RATES];
+		int band;
+
+		if (!rate_nest)
+			goto nla_put_failure;
+
+		for (band = 0; band < drv->num_bands; band++) {
+			int i, n_rates = 0;
+
+			for (i = 0; i < drv->rates[band].n_rates; i++) {
+				u8 rate = drv->rates[band].rates[i];
+				if (rate == 0x02 || rate == 0x04 ||
+				    rate == 0x0b || rate == 0x16)
+					continue;
+				rates[n_rates] = rate;
+				n_rates++;
+			}
+			NLA_PUT(msg, drv->rates[band].nlband, n_rates, rates);
+		}
+
+		nla_nest_end(msg, rate_nest);
+	}
+
 	if (params->freqs) {
 		for (i = 0; params->freqs[i]; i++) {
 			wpa_printf(MSG_MSGDUMP, "nl80211: Scan frequency %u "
@@ -3270,6 +3309,7 @@ nla_put_failure:
 
 
 struct phy_info_arg {
+	struct wpa_driver_nl80211_data *drv;
 	u16 *num_modes;
 	struct hostapd_hw_modes *modes;
 };
@@ -3304,6 +3344,8 @@ static int phy_info_handler(struct nl_ms
 	int rem_band, rem_freq, rem_rate;
 	struct hostapd_hw_modes *mode;
 	int idx, mode_is_set;
+	int rates_known = phy_info->drv->rates != NULL;
+	int num_bands = 0, band_idx = 0;
 
 	nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 		  genlmsg_attrlen(gnlh, 0), NULL);
@@ -3311,10 +3353,22 @@ static int phy_info_handler(struct nl_ms
 	if (!tb_msg[NL80211_ATTR_WIPHY_BANDS])
 		return NL_SKIP;
 
+	nla_for_each_nested(nl_band, tb_msg[NL80211_ATTR_WIPHY_BANDS], rem_band)
+		num_bands++;
+
+	if (!rates_known) {
+		phy_info->drv->num_bands = num_bands;
+		phy_info->drv->rates = os_malloc(num_bands * sizeof(phy_info->drv->rates[0]));
+		if (!phy_info->drv->rates)
+			return NL_SKIP;
+	}
+
 	nla_for_each_nested(nl_band, tb_msg[NL80211_ATTR_WIPHY_BANDS], rem_band) {
+		int band = nla_type(nl_band);
+
 		mode = os_realloc(phy_info->modes, (*phy_info->num_modes + 1) * sizeof(*mode));
 		if (!mode)
-			return NL_SKIP;
+			goto failure;
 		phy_info->modes = mode;
 
 		mode_is_set = 0;
@@ -3360,7 +3414,7 @@ static int phy_info_handler(struct nl_ms
 
 		mode->channels = os_zalloc(mode->num_channels * sizeof(struct hostapd_channel_data));
 		if (!mode->channels)
-			return NL_SKIP;
+			goto failure;
 
 		idx = 0;
 
@@ -3420,9 +3474,14 @@ static int phy_info_handler(struct nl_ms
 			mode->num_rates++;
 		}
 
+		if (!rates_known) {
+			phy_info->drv->rates[band_idx].n_rates = mode->num_rates;
+			phy_info->drv->rates[band_idx].nlband = band;
+		}
+
 		mode->rates = os_zalloc(mode->num_rates * sizeof(int));
 		if (!mode->rates)
-			return NL_SKIP;
+			goto failure;
 
 		idx = 0;
 
@@ -3433,6 +3492,10 @@ static int phy_info_handler(struct nl_ms
 				continue;
 			mode->rates[idx] = nla_get_u32(tb_rate[NL80211_BITRATE_ATTR_RATE]);
 
+			if (!rates_known)
+				phy_info->drv->rates[band_idx].rates[idx] =
+					mode->rates[idx] / 5;
+
 			/* crude heuristic */
 			if (mode->mode == HOSTAPD_MODE_IEEE80211B &&
 			    mode->rates[idx] > 200)
@@ -3440,9 +3503,18 @@ static int phy_info_handler(struct nl_ms
 
 			idx++;
 		}
+
+		band_idx++;
 	}
 
 	return NL_SKIP;
+ failure:
+	if (!rates_known) {
+		os_free(phy_info->drv->rates);
+		phy_info->drv->rates = NULL;
+		phy_info->drv->num_bands = 0;
+	}
+	return NL_SKIP;
 }
 
 static struct hostapd_hw_modes *
@@ -3674,6 +3746,7 @@ wpa_driver_nl80211_get_hw_feature_data(v
 	struct wpa_driver_nl80211_data *drv = bss->drv;
 	struct nl_msg *msg;
 	struct phy_info_arg result = {
+		.drv = drv,
 		.num_modes = num_modes,
 		.modes = NULL,
 	};