[PATCH 04/14] ap/nl80211: Suppport multiple CS counters

[Ilan Peer]

From: Andrei Otcheretianski <andrei.otcheretianski at intel.com>

Channel switch may be performed using both CSA and eCSA IEs together. This
may happen, for example with 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 cfg80211/nl80211.

Signed-off-by: Andrei Otcheretianski <andrei.otcheretianski at intel.com>
---
 src/ap/hostapd.c             |    4 +--
 src/drivers/driver.h         |    7 ++--
 src/drivers/driver_nl80211.c |   74 +++++++++++++++++++++++++++++++++---------
 3 files changed, 66 insertions(+), 19 deletions(-)

diff --git a/src/ap/hostapd.c b/src/ap/hostapd.c
index e5a75ab..72f2b24 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -2355,8 +2355,8 @@ static int hostapd_fill_csa_settings(struct hostapd_iface *iface,
 		return ret;
 	}
 
-	settings->counter_offset_beacon = iface->cs_c_off_beacon;
-	settings->counter_offset_presp = iface->cs_c_off_proberesp;
+	settings->counter_offset_beacon[0] = iface->cs_c_off_beacon;
+	settings->counter_offset_presp[0] = iface->cs_c_off_proberesp;
 
 	return 0;
 }
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index bb9dec5..6f42e26 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -1046,6 +1046,9 @@ struct wpa_driver_capa {
 	unsigned int extended_capa_len;
 
 	struct wowlan_triggers wowlan_triggers;
+
+	/* maximum number of supported csa counters */
+	u16 max_csa_counters;
 };
 
 
@@ -1296,8 +1299,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() */
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 67332d9..d397ccd 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -3855,6 +3855,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 		capa->max_stations =
 			nla_get_u32(tb[NL80211_ATTR_MAX_AP_ASSOC_STA]);
 
+	/* TODO: get the number of counters from the kernel */
+	capa->max_csa_counters = 1;
+
 	return NL_SKIP;
 }
 
@@ -11950,6 +11953,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,
@@ -11966,20 +11971,58 @@ 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 beacons counters match.
+	 * This implementation assumes that there are only 2 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;
+	}
 
 	msg = nlmsg_alloc();
 	if (!msg)
@@ -12009,12 +12052,13 @@ static int nl80211_switch_channel(void *priv, struct csa_settings *settings)
 	if (ret)
 		goto error;
 
-	NLA_PUT_U16(msg, NL80211_ATTR_CSA_C_OFF_BEACON,
-		    settings->counter_offset_beacon);
+	NLA_PUT(msg, NL80211_ATTR_CSA_C_OFF_BEACON,
+		csa_off_len * sizeof(u16), settings->counter_offset_beacon);
 
 	if (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);
 
 	nla_nest_end(msg, beacon_csa);
 	ret = send_and_recv_msgs(drv, msg, NULL, NULL);
-- 
1.7.10.4