From: Johannes Berg <johannes.berg@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@intel.com>
---
Note: depends on new kernel patches and nl80211 API update

 src/drivers/driver_nl80211.c |   61 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 61 insertions(+)

--- a/src/drivers/driver_nl80211.c	2011-07-18 19:03:20.000000000 +0200
+++ b/src/drivers/driver_nl80211.c	2011-07-18 19:03:49.000000000 +0200
@@ -118,6 +118,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;
@@ -172,6 +177,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 */
 
@@ -2346,6 +2354,32 @@ static int wpa_driver_nl80211_scan(void
 			params->extra_ies);
 	}
 
+	/* filter out 11b rates */
+	if (params->p2p) {
+		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 "
@@ -3236,6 +3270,7 @@ nla_put_failure:
 
 
 struct phy_info_arg {
+	struct wpa_driver_nl80211_data *drv;
 	u16 *num_modes;
 	struct hostapd_hw_modes *modes;
 };
@@ -3270,6 +3305,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);
@@ -3277,7 +3314,19 @@ 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;
@@ -3386,6 +3435,11 @@ 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;
@@ -3399,6 +3453,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)
@@ -3406,6 +3464,8 @@ static int phy_info_handler(struct nl_ms
 
 			idx++;
 		}
+
+		band_idx++;
 	}
 
 	return NL_SKIP;
@@ -3640,6 +3700,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,
 	};