nl80211: Suppport multiple CSA counters

Channel switch may be performed using both CSA and eCSA IEs together.
This may happen, for example with a P2P GO on band A with legacy
clients. Extend driver API to support up to 2 CSA counters.

This patch also includes the required implementation for nl80211.

Signed-off-by: Andrei Otcheretianski <andrei.otcheretianski@intel.com>

src/ap/hostapd.c

@@ -2848,8 +2848,8 @@ static int hostapd_fill_csa_settings(struct hostapd_data *hapd,
 		return ret;
 	}
 
-	settings->counter_offset_beacon = hapd->cs_c_off_beacon;
-	settings->counter_offset_presp = hapd->cs_c_off_proberesp;
+	settings->counter_offset_beacon[0] = hapd->cs_c_off_beacon;
+	settings->counter_offset_presp[0] = hapd->cs_c_off_proberesp;
 
 	return 0;
 }

src/drivers/driver.h

@@ -1306,6 +1306,9 @@ struct wpa_driver_capa {
 	unsigned int max_conc_chan_2_4;
 	/* Maximum number of concurrent channels on 5 GHz */
 	unsigned int max_conc_chan_5_0;
+
+	/* Maximum number of supported CSA counters */
+	u16 max_csa_counters;
 };
 
 
@@ -1556,8 +1559,8 @@ struct csa_settings {
 	struct beacon_data beacon_csa;
 	struct beacon_data beacon_after;
 
-	u16 counter_offset_beacon;
-	u16 counter_offset_presp;
+	u16 counter_offset_beacon[2];
+	u16 counter_offset_presp[2];
 };
 
 /* TDLS peer capabilities for send_tdls_mgmt() */

src/drivers/driver_nl80211.c

@@ -7576,6 +7576,8 @@ static int nl80211_switch_channel(void *priv, struct csa_settings *settings)
 	struct wpa_driver_nl80211_data *drv = bss->drv;
 	struct nlattr *beacon_csa;
 	int ret = -ENOBUFS;
+	int csa_off_len = 0;
+	int i;
 
 	wpa_printf(MSG_DEBUG, "nl80211: Channel switch request (cs_count=%u block_tx=%u freq=%d width=%d cf1=%d cf2=%d)",
 		   settings->cs_count, settings->block_tx,
@@ -7592,20 +7594,56 @@ static int nl80211_switch_channel(void *priv, struct csa_settings *settings)
 	    (drv->nlmode != NL80211_IFTYPE_P2P_GO))
 		return -EOPNOTSUPP;
 
-	/* check settings validity */
-	if (!settings->beacon_csa.tail ||
-	    ((settings->beacon_csa.tail_len <=
-	      settings->counter_offset_beacon) ||
-	     (settings->beacon_csa.tail[settings->counter_offset_beacon] !=
-	      settings->cs_count)))
+	/*
+	 * Remove empty counters, assuming Probe Response and Beacon frame
+	 * counters match. This implementation assumes that there are only two
+	 * counters.
+	 */
+	if (settings->counter_offset_beacon[0] &&
+	    !settings->counter_offset_beacon[1]) {
+		csa_off_len = 1;
+	} else if (settings->counter_offset_beacon[1] &&
+		   !settings->counter_offset_beacon[0]) {
+		csa_off_len = 1;
+		settings->counter_offset_beacon[0] =
+			settings->counter_offset_beacon[1];
+		settings->counter_offset_presp[0] =
+			settings->counter_offset_presp[1];
+	} else if (settings->counter_offset_beacon[1] &&
+		   settings->counter_offset_beacon[0]) {
+		csa_off_len = 2;
+	} else {
+		wpa_printf(MSG_ERROR, "nl80211: No CSA counters provided");
 		return -EINVAL;
+	}
 
-	if (settings->beacon_csa.probe_resp &&
-	    ((settings->beacon_csa.probe_resp_len <=
-	      settings->counter_offset_presp) ||
-	     (settings->beacon_csa.probe_resp[settings->counter_offset_presp] !=
-	      settings->cs_count)))
+	/* Check CSA counters validity */
+	if (drv->capa.max_csa_counters &&
+	    csa_off_len > drv->capa.max_csa_counters) {
+		wpa_printf(MSG_ERROR,
+			   "nl80211: Too many CSA counters provided");
 		return -EINVAL;
+	}
+
+	if (!settings->beacon_csa.tail)
+		return -EINVAL;
+
+	for (i = 0; i < csa_off_len; i++) {
+		u16 csa_c_off_bcn = settings->counter_offset_beacon[i];
+		u16 csa_c_off_presp = settings->counter_offset_presp[i];
+
+		if ((settings->beacon_csa.tail_len <= csa_c_off_bcn) ||
+		    (settings->beacon_csa.tail[csa_c_off_bcn] !=
+		     settings->cs_count))
+			return -EINVAL;
+
+		if (settings->beacon_csa.probe_resp &&
+		    ((settings->beacon_csa.probe_resp_len <=
+		      csa_c_off_presp) ||
+		     (settings->beacon_csa.probe_resp[csa_c_off_presp] !=
+		      settings->cs_count)))
+			return -EINVAL;
+	}
 
 	if (!(msg = nl80211_bss_msg(bss, 0, NL80211_CMD_CHANNEL_SWITCH)) ||
 	    nla_put_u32(msg, NL80211_ATTR_CH_SWITCH_COUNT,
@@ -7629,11 +7667,13 @@ static int nl80211_switch_channel(void *priv, struct csa_settings *settings)
 	if (ret)
 		goto error;
 
-	if (nla_put_u16(msg, NL80211_ATTR_CSA_C_OFF_BEACON,
-			settings->counter_offset_beacon) ||
+	if (nla_put(msg, NL80211_ATTR_CSA_C_OFF_BEACON,
+		    csa_off_len * sizeof(u16),
+		    settings->counter_offset_beacon) ||
 	    (settings->beacon_csa.probe_resp &&
-	     nla_put_u16(msg, NL80211_ATTR_CSA_C_OFF_PRESP,
-			 settings->counter_offset_presp)))
+	     nla_put(msg, NL80211_ATTR_CSA_C_OFF_PRESP,
+		     csa_off_len * sizeof(u16),
+		     settings->counter_offset_presp)))
 		goto fail;
 
 	nla_nest_end(msg, beacon_csa);

src/drivers/driver_nl80211_capa.c

@@ -638,6 +638,10 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 		capa->max_stations =
 			nla_get_u32(tb[NL80211_ATTR_MAX_AP_ASSOC_STA]);
 
+	if (tb[NL80211_ATTR_MAX_CSA_COUNTERS])
+		capa->max_csa_counters =
+			nla_get_u8(tb[NL80211_ATTR_MAX_CSA_COUNTERS]);
+
 	return NL_SKIP;
 }
 
@@ -694,8 +698,6 @@ static int wpa_driver_nl80211_get_info(struct wpa_driver_nl80211_data *drv,
 	if (!drv->capa.max_remain_on_chan)
 		drv->capa.max_remain_on_chan = 5000;
 
-	if (info->channel_switch_supported)
-		drv->capa.flags |= WPA_DRIVER_FLAGS_AP_CSA;
 	drv->capa.wmm_ac_supported = info->wmm_ac_supported;
 
 	drv->capa.mac_addr_rand_sched_scan_supported =
@@ -703,6 +705,12 @@ static int wpa_driver_nl80211_get_info(struct wpa_driver_nl80211_data *drv,
 	drv->capa.mac_addr_rand_scan_supported =
 		info->mac_addr_rand_scan_supported;
 
+	if (info->channel_switch_supported) {
+		drv->capa.flags |= WPA_DRIVER_FLAGS_AP_CSA;
+		if (!drv->capa.max_csa_counters)
+			drv->capa.max_csa_counters = 1;
+	}
+
 	return 0;
 }