mac80211: add support for CSA in IBSS mode

This function adds the channel switch announcement implementation for the
IBSS code. It is triggered by userspace (mac80211/cfg) or by external
channel switch announcement, which have to be adopted. Both CSAs in
beacons and action frames are supported. As for AP mode, the channel
switch is applied after some time. However in IBSS mode, the channel
switch IEs are generated in the kernel.

Signed-off-by: Simon Wunderlich <siwu@hrz.tu-chemnitz.de>
Signed-off-by: Mathias Kretschmer <mathias.kretschmer@fokus.fraunhofer.de>
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

1 files changed, 348 insertions, 6 deletions

diff --git a/net/mac80211/ibss.c b/net/mac80211/ibss.c
index 0c3ec08..c004213 100644
--- a/net/mac80211/ibss.c
+++ b/net/mac80211/ibss.c
@@ -39,7 +39,8 @@ ieee80211_ibss_build_presp(struct ieee80211_sub_if_data *sdata,
 			   const int beacon_int, const u32 basic_rates,
 			   const u16 capability, u64 tsf,
 			   struct cfg80211_chan_def *chandef,
-			   bool *have_higher_than_11mbit)
+			   bool *have_higher_than_11mbit,
+			   struct cfg80211_csa_settings *csa_settings)
 {
 	struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
 	struct ieee80211_local *local = sdata->local;
@@ -59,6 +60,7 @@ ieee80211_ibss_build_presp(struct ieee80211_sub_if_data *sdata,
 		    2 + 8 /* max Supported Rates */ +
 		    3 /* max DS params */ +
 		    4 /* IBSS params */ +
+		    5 /* Channel Switch Announcement */ +
 		    2 + (IEEE80211_MAX_SUPP_RATES - 8) +
 		    2 + sizeof(struct ieee80211_ht_cap) +
 		    2 + sizeof(struct ieee80211_ht_operation) +
@@ -135,6 +137,16 @@ ieee80211_ibss_build_presp(struct ieee80211_sub_if_data *sdata,
 	*pos++ = 0;
 	*pos++ = 0;
 
+	if (csa_settings) {
+		*pos++ = WLAN_EID_CHANNEL_SWITCH;
+		*pos++ = 3;
+		*pos++ = csa_settings->block_tx ? 1 : 0;
+		*pos++ = ieee80211_frequency_to_channel(
+				csa_settings->chandef.chan->center_freq);
+		sdata->csa_counter_offset_beacon = (pos - presp->head);
+		*pos++ = csa_settings->count;
+	}
+
 	/* put the remaining rates in WLAN_EID_EXT_SUPP_RATES */
 	if (rates_n > 8) {
 		*pos++ = WLAN_EID_EXT_SUPP_RATES;
@@ -276,7 +288,7 @@ static void __ieee80211_sta_join_ibss(struct ieee80211_sub_if_data *sdata,
 
 	presp = ieee80211_ibss_build_presp(sdata, beacon_int, basic_rates,
 					   capability, tsf, &chandef,
-					   &have_higher_than_11mbit);
+					   &have_higher_than_11mbit, NULL);
 	if (!presp)
 		return;
 
@@ -416,6 +428,109 @@ static void ieee80211_sta_join_ibss(struct ieee80211_sub_if_data *sdata,
 				  tsf, false);
 }
 
+int ieee80211_ibss_csa_beacon(struct ieee80211_sub_if_data *sdata,
+			      struct cfg80211_csa_settings *csa_settings)
+{
+	struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
+	struct beacon_data *presp, *old_presp;
+	struct cfg80211_bss *cbss;
+	const struct cfg80211_bss_ies *ies;
+	u16 capability;
+	u64 tsf;
+	int ret = 0;
+
+	sdata_assert_lock(sdata);
+
+	capability = WLAN_CAPABILITY_IBSS;
+
+	if (ifibss->privacy)
+		capability |= WLAN_CAPABILITY_PRIVACY;
+
+	cbss = cfg80211_get_bss(sdata->local->hw.wiphy, ifibss->chandef.chan,
+				ifibss->bssid, ifibss->ssid,
+				ifibss->ssid_len, WLAN_CAPABILITY_IBSS |
+				WLAN_CAPABILITY_PRIVACY,
+				capability);
+
+	if (WARN_ON(!cbss)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	rcu_read_lock();
+	ies = rcu_dereference(cbss->ies);
+	tsf = ies->tsf;
+	rcu_read_unlock();
+	cfg80211_put_bss(sdata->local->hw.wiphy, cbss);
+
+	old_presp = rcu_dereference_protected(ifibss->presp,
+					  lockdep_is_held(&sdata->wdev.mtx));
+
+	presp = ieee80211_ibss_build_presp(sdata,
+					   sdata->vif.bss_conf.beacon_int,
+					   sdata->vif.bss_conf.basic_rates,
+					   capability, tsf, &ifibss->chandef,
+					   NULL, csa_settings);
+	if (!presp) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	rcu_assign_pointer(ifibss->presp, presp);
+	if (old_presp)
+		kfree_rcu(old_presp, rcu_head);
+
+	ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_BEACON);
+ out:
+	return ret;
+}
+
+int ieee80211_ibss_finish_csa(struct ieee80211_sub_if_data *sdata)
+{
+	struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
+	struct cfg80211_bss *cbss;
+	int err;
+	u16 capability;
+
+	sdata_lock(sdata);
+	/* update cfg80211 bss information with the new channel */
+	if (!is_zero_ether_addr(ifibss->bssid)) {
+		capability = WLAN_CAPABILITY_IBSS;
+
+		if (ifibss->privacy)
+			capability |= WLAN_CAPABILITY_PRIVACY;
+
+		cbss = cfg80211_get_bss(sdata->local->hw.wiphy,
+					ifibss->chandef.chan,
+					ifibss->bssid, ifibss->ssid,
+					ifibss->ssid_len, WLAN_CAPABILITY_IBSS |
+					WLAN_CAPABILITY_PRIVACY,
+					capability);
+		/* XXX: should not really modify cfg80211 data */
+		if (cbss) {
+			cbss->channel = sdata->local->csa_chandef.chan;
+			cfg80211_put_bss(sdata->local->hw.wiphy, cbss);
+		}
+	}
+
+	ifibss->chandef = sdata->local->csa_chandef;
+
+	/* generate the beacon */
+	err = ieee80211_ibss_csa_beacon(sdata, NULL);
+	sdata_unlock(sdata);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+void ieee80211_ibss_stop(struct ieee80211_sub_if_data *sdata)
+{
+	struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
+
+	cancel_work_sync(&ifibss->csa_connection_drop_work);
+}
+
 static struct sta_info *ieee80211_ibss_finish_sta(struct sta_info *sta)
 	__acquires(RCU)
 {
@@ -589,6 +704,204 @@ static void ieee80211_ibss_disconnect(struct ieee80211_sub_if_data *sdata)
 	ieee80211_vif_release_channel(sdata);
 }
 
+static void ieee80211_csa_connection_drop_work(struct work_struct *work)
+{
+	struct ieee80211_sub_if_data *sdata =
+		container_of(work, struct ieee80211_sub_if_data,
+			     u.ibss.csa_connection_drop_work);
+
+	ieee80211_ibss_disconnect(sdata);
+	synchronize_rcu();
+	skb_queue_purge(&sdata->skb_queue);
+
+	/* trigger a scan to find another IBSS network to join */
+	ieee80211_queue_work(&sdata->local->hw, &sdata->work);
+}
+
+static bool
+ieee80211_ibss_process_chanswitch(struct ieee80211_sub_if_data *sdata,
+				  struct ieee802_11_elems *elems,
+				  bool beacon)
+{
+	struct cfg80211_csa_settings params;
+	struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
+	struct ieee80211_chanctx_conf *chanctx_conf;
+	struct ieee80211_chanctx *chanctx;
+	enum nl80211_channel_type ch_type;
+	int err, num_chanctx;
+	u32 sta_flags;
+	u8 mode;
+
+	if (sdata->vif.csa_active)
+		return true;
+
+	if (!sdata->vif.bss_conf.ibss_joined)
+		return false;
+
+	sta_flags = IEEE80211_STA_DISABLE_VHT;
+	switch (ifibss->chandef.width) {
+	case NL80211_CHAN_WIDTH_5:
+	case NL80211_CHAN_WIDTH_10:
+	case NL80211_CHAN_WIDTH_20_NOHT:
+		sta_flags |= IEEE80211_STA_DISABLE_HT;
+		/* fall through */
+	case NL80211_CHAN_WIDTH_20:
+		sta_flags |= IEEE80211_STA_DISABLE_40MHZ;
+		break;
+	default:
+		break;
+	}
+
+	memset(&params, 0, sizeof(params));
+	err = ieee80211_parse_ch_switch_ie(sdata, elems, beacon,
+					   ifibss->chandef.chan->band,
+					   sta_flags, ifibss->bssid,
+					   &params.count, &mode,
+					   &params.chandef);
+
+	/* can't switch to destination channel, fail */
+	if (err < 0)
+		goto disconnect;
+
+	/* did not contain a CSA */
+	if (err)
+		return false;
+
+	if (ifibss->chandef.chan->band != params.chandef.chan->band)
+		goto disconnect;
+
+	switch (ifibss->chandef.width) {
+	case NL80211_CHAN_WIDTH_20_NOHT:
+	case NL80211_CHAN_WIDTH_20:
+	case NL80211_CHAN_WIDTH_40:
+		/* keep our current HT mode (HT20/HT40+/HT40-), even if
+		 * another mode  has been announced. The mode is not adopted
+		 * within the beacon while doing CSA and we should therefore
+		 * keep the mode which we announce.
+		 */
+		ch_type = cfg80211_get_chandef_type(&ifibss->chandef);
+		cfg80211_chandef_create(&params.chandef, params.chandef.chan,
+					ch_type);
+		break;
+	case NL80211_CHAN_WIDTH_5:
+	case NL80211_CHAN_WIDTH_10:
+		if (params.chandef.width != ifibss->chandef.width) {
+			sdata_info(sdata,
+				   "IBSS %pM received channel switch from incompatible channel width (%d MHz, width:%d, CF1/2: %d/%d MHz), disconnecting\n",
+				   ifibss->bssid,
+				   params.chandef.chan->center_freq,
+				   params.chandef.width,
+				   params.chandef.center_freq1,
+				   params.chandef.center_freq2);
+			goto disconnect;
+		}
+		break;
+	default:
+		/* should not happen, sta_flags should prevent VHT modes. */
+		WARN_ON(1);
+		goto disconnect;
+	}
+
+	if (!cfg80211_chandef_usable(sdata->local->hw.wiphy, &params.chandef,
+				     IEEE80211_CHAN_DISABLED)) {
+		sdata_info(sdata,
+			   "IBSS %pM switches to unsupported channel (%d MHz, width:%d, CF1/2: %d/%d MHz), disconnecting\n",
+			   ifibss->bssid,
+			   params.chandef.chan->center_freq,
+			   params.chandef.width,
+			   params.chandef.center_freq1,
+			   params.chandef.center_freq2);
+		goto disconnect;
+	}
+
+	err = cfg80211_chandef_dfs_required(sdata->local->hw.wiphy,
+					    &params.chandef);
+	if (err < 0)
+		goto disconnect;
+	if (err) {
+		params.radar_required = true;
+
+		/* TODO: IBSS-DFS not (yet) supported, disconnect. */
+		goto disconnect;
+	}
+
+	rcu_read_lock();
+	chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
+	if (!chanctx_conf) {
+		rcu_read_unlock();
+		goto disconnect;
+	}
+
+	/* don't handle for multi-VIF cases */
+	chanctx = container_of(chanctx_conf, struct ieee80211_chanctx, conf);
+	if (chanctx->refcount > 1) {
+		rcu_read_unlock();
+		goto disconnect;
+	}
+	num_chanctx = 0;
+	list_for_each_entry_rcu(chanctx, &sdata->local->chanctx_list, list)
+		num_chanctx++;
+
+	if (num_chanctx > 1) {
+		rcu_read_unlock();
+		goto disconnect;
+	}
+	rcu_read_unlock();
+
+	/* all checks done, now perform the channel switch. */
+	ibss_dbg(sdata,
+		 "received channel switch announcement to go to channel %d MHz\n",
+		 params.chandef.chan->center_freq);
+
+	params.block_tx = !!mode;
+
+	ieee80211_ibss_csa_beacon(sdata, &params);
+	sdata->csa_radar_required = params.radar_required;
+
+	if (params.block_tx)
+		ieee80211_stop_queues_by_reason(&sdata->local->hw,
+				IEEE80211_MAX_QUEUE_MAP,
+				IEEE80211_QUEUE_STOP_REASON_CSA);
+
+	sdata->local->csa_chandef = params.chandef;
+	sdata->vif.csa_active = true;
+
+	ieee80211_bss_info_change_notify(sdata, err);
+	drv_channel_switch_beacon(sdata, &params.chandef);
+
+	return true;
+disconnect:
+	ibss_dbg(sdata, "Can't handle channel switch, disconnect\n");
+	ieee80211_queue_work(&sdata->local->hw,
+			     &ifibss->csa_connection_drop_work);
+
+	return true;
+}
+
+static void
+ieee80211_rx_mgmt_spectrum_mgmt(struct ieee80211_sub_if_data *sdata,
+				struct ieee80211_mgmt *mgmt, size_t len,
+				struct ieee80211_rx_status *rx_status,
+				struct ieee802_11_elems *elems)
+{
+	int required_len;
+
+	if (len < IEEE80211_MIN_ACTION_SIZE + 1)
+		return;
+
+	/* CSA is the only action we handle for now */
+	if (mgmt->u.action.u.measurement.action_code !=
+	    WLAN_ACTION_SPCT_CHL_SWITCH)
+		return;
+
+	required_len = IEEE80211_MIN_ACTION_SIZE +
+		       sizeof(mgmt->u.action.u.chan_switch);
+	if (len < required_len)
+		return;
+
+	ieee80211_ibss_process_chanswitch(sdata, elems, false);
+}
+
 static void ieee80211_rx_mgmt_deauth_ibss(struct ieee80211_sub_if_data *sdata,
 					  struct ieee80211_mgmt *mgmt,
 					  size_t len)
@@ -751,10 +1064,6 @@ static void ieee80211_rx_bss_info(struct ieee80211_sub_if_data *sdata,
 
 	/* check if we need to merge IBSS */
 
-	/* we use a fixed BSSID */
-	if (sdata->u.ibss.fixed_bssid)
-		goto put_bss;
-
 	/* not an IBSS */
 	if (!(cbss->capability & WLAN_CAPABILITY_IBSS))
 		goto put_bss;
@@ -770,10 +1079,18 @@ static void ieee80211_rx_bss_info(struct ieee80211_sub_if_data *sdata,
 				sdata->u.ibss.ssid_len))
 		goto put_bss;
 
+	/* process channel switch */
+	if (ieee80211_ibss_process_chanswitch(sdata, elems, true))
+		goto put_bss;
+
 	/* same BSSID */
 	if (ether_addr_equal(cbss->bssid, sdata->u.ibss.bssid))
 		goto put_bss;
 
+	/* we use a fixed BSSID */
+	if (sdata->u.ibss.fixed_bssid)
+		goto put_bss;
+
 	if (ieee80211_have_rx_timestamp(rx_status)) {
 		/* time when timestamp field was received */
 		rx_timestamp =
@@ -1142,6 +1459,8 @@ void ieee80211_ibss_rx_queued_mgmt(struct ieee80211_sub_if_data *sdata,
 	struct ieee80211_rx_status *rx_status;
 	struct ieee80211_mgmt *mgmt;
 	u16 fc;
+	struct ieee802_11_elems elems;
+	int ies_len;
 
 	rx_status = IEEE80211_SKB_RXCB(skb);
 	mgmt = (struct ieee80211_mgmt *) skb->data;
@@ -1167,6 +1486,27 @@ void ieee80211_ibss_rx_queued_mgmt(struct ieee80211_sub_if_data *sdata,
 	case IEEE80211_STYPE_DEAUTH:
 		ieee80211_rx_mgmt_deauth_ibss(sdata, mgmt, skb->len);
 		break;
+	case IEEE80211_STYPE_ACTION:
+		switch (mgmt->u.action.category) {
+		case WLAN_CATEGORY_SPECTRUM_MGMT:
+			ies_len = skb->len -
+				  offsetof(struct ieee80211_mgmt,
+					   u.action.u.chan_switch.variable);
+
+			if (ies_len < 0)
+				break;
+
+			ieee802_11_parse_elems(
+				mgmt->u.action.u.chan_switch.variable,
+				ies_len, true, &elems);
+
+			if (elems.parse_error)
+				break;
+
+			ieee80211_rx_mgmt_spectrum_mgmt(sdata, mgmt, skb->len,
+							rx_status, &elems);
+			break;
+		}
 	}
 
  mgmt_out:
@@ -1233,6 +1573,8 @@ void ieee80211_ibss_setup_sdata(struct ieee80211_sub_if_data *sdata)
 		    (unsigned long) sdata);
 	INIT_LIST_HEAD(&ifibss->incomplete_stations);
 	spin_lock_init(&ifibss->incomplete_lock);
+	INIT_WORK(&ifibss->csa_connection_drop_work,
+		  ieee80211_csa_connection_drop_work);
 }
 
 /* scan finished notification */