From: Lorenzo Bianconi Date: Thu, 24 Feb 2022 12:54:58 +0100 Subject: [PATCH] mac80211: MBSSID beacon handling in AP mode Add new fields in struct beacon_data to store all MBSSID elements. Generate a beacon template which includes all MBSSID elements. Move CSA offset to reflect the MBSSID element length. Co-developed-by: Aloka Dixit Signed-off-by: Aloka Dixit Co-developed-by: John Crispin Signed-off-by: John Crispin Signed-off-by: Lorenzo Bianconi Tested-by: Money Wang Link: https://lore.kernel.org/r/5322db3c303f431adaf191ab31c45e151dde5465.1645702516.git.lorenzo@kernel.org [small cleanups] Signed-off-by: Johannes Berg --- --- a/include/net/mac80211.h +++ b/include/net/mac80211.h @@ -4938,12 +4938,14 @@ void ieee80211_report_low_ack(struct iee * @cntdwn_counter_offs: array of IEEE80211_MAX_CNTDWN_COUNTERS_NUM offsets * to countdown counters. This array can contain zero values which * should be ignored. + * @mbssid_off: position of the multiple bssid element */ struct ieee80211_mutable_offsets { u16 tim_offset; u16 tim_length; u16 cntdwn_counter_offs[IEEE80211_MAX_CNTDWN_COUNTERS_NUM]; + u16 mbssid_off; }; /** --- a/net/mac80211/cfg.c +++ b/net/mac80211/cfg.c @@ -989,11 +989,29 @@ static int ieee80211_set_ftm_responder_p return 0; } +static int +ieee80211_copy_mbssid_beacon(u8 *pos, struct cfg80211_mbssid_elems *dst, + struct cfg80211_mbssid_elems *src) +{ + int i, offset = 0; + + for (i = 0; i < src->cnt; i++) { + memcpy(pos + offset, src->elem[i].data, src->elem[i].len); + dst->elem[i].len = src->elem[i].len; + dst->elem[i].data = pos + offset; + offset += dst->elem[i].len; + } + dst->cnt = src->cnt; + + return offset; +} + static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata, struct cfg80211_beacon_data *params, const struct ieee80211_csa_settings *csa, const struct ieee80211_color_change_settings *cca) { + struct cfg80211_mbssid_elems *mbssid = NULL; struct beacon_data *new, *old; int new_head_len, new_tail_len; int size, err; @@ -1021,6 +1039,17 @@ static int ieee80211_assign_beacon(struc size = sizeof(*new) + new_head_len + new_tail_len; + /* new or old multiple BSSID elements? */ + if (params->mbssid_ies) { + mbssid = params->mbssid_ies; + size += struct_size(new->mbssid_ies, elem, mbssid->cnt); + size += ieee80211_get_mbssid_beacon_len(mbssid); + } else if (old && old->mbssid_ies) { + mbssid = old->mbssid_ies; + size += struct_size(new->mbssid_ies, elem, mbssid->cnt); + size += ieee80211_get_mbssid_beacon_len(mbssid); + } + new = kzalloc(size, GFP_KERNEL); if (!new) return -ENOMEM; @@ -1029,12 +1058,20 @@ static int ieee80211_assign_beacon(struc /* * pointers go into the block we allocated, - * memory is | beacon_data | head | tail | + * memory is | beacon_data | head | tail | mbssid_ies */ new->head = ((u8 *) new) + sizeof(*new); new->tail = new->head + new_head_len; new->head_len = new_head_len; new->tail_len = new_tail_len; + /* copy in optional mbssid_ies */ + if (mbssid) { + u8 *pos = new->tail + new->tail_len; + + new->mbssid_ies = (void *)pos; + pos += struct_size(new->mbssid_ies, elem, mbssid->cnt); + ieee80211_copy_mbssid_beacon(pos, new->mbssid_ies, mbssid); + } if (csa) { new->cntdwn_current_counter = csa->count; @@ -1332,8 +1369,11 @@ static int ieee80211_stop_ap(struct wiph mutex_unlock(&local->mtx); - kfree(sdata->u.ap.next_beacon); - sdata->u.ap.next_beacon = NULL; + if (sdata->u.ap.next_beacon) { + kfree(sdata->u.ap.next_beacon->mbssid_ies); + kfree(sdata->u.ap.next_beacon); + sdata->u.ap.next_beacon = NULL; + } /* turn off carrier for this interface and dependent VLANs */ list_for_each_entry(vlan, &sdata->u.ap.vlans, u.vlan.list) @@ -3126,12 +3166,24 @@ cfg80211_beacon_dup(struct cfg80211_beac len = beacon->head_len + beacon->tail_len + beacon->beacon_ies_len + beacon->proberesp_ies_len + beacon->assocresp_ies_len + - beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len; + beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len + + ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies); new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL); if (!new_beacon) return NULL; + if (beacon->mbssid_ies && beacon->mbssid_ies->cnt) { + new_beacon->mbssid_ies = + kzalloc(struct_size(new_beacon->mbssid_ies, + elem, beacon->mbssid_ies->cnt), + GFP_KERNEL); + if (!new_beacon->mbssid_ies) { + kfree(new_beacon); + return NULL; + } + } + pos = (u8 *)(new_beacon + 1); if (beacon->head_len) { new_beacon->head_len = beacon->head_len; @@ -3169,6 +3221,10 @@ cfg80211_beacon_dup(struct cfg80211_beac memcpy(pos, beacon->probe_resp, beacon->probe_resp_len); pos += beacon->probe_resp_len; } + if (beacon->mbssid_ies && beacon->mbssid_ies->cnt) + pos += ieee80211_copy_mbssid_beacon(pos, + new_beacon->mbssid_ies, + beacon->mbssid_ies); /* might copy -1, meaning no changes requested */ new_beacon->ftm_responder = beacon->ftm_responder; @@ -3206,8 +3262,11 @@ static int ieee80211_set_after_csa_beaco case NL80211_IFTYPE_AP: err = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon, NULL, NULL); - kfree(sdata->u.ap.next_beacon); - sdata->u.ap.next_beacon = NULL; + if (sdata->u.ap.next_beacon) { + kfree(sdata->u.ap.next_beacon->mbssid_ies); + kfree(sdata->u.ap.next_beacon); + sdata->u.ap.next_beacon = NULL; + } if (err < 0) return err; @@ -3362,8 +3421,12 @@ static int ieee80211_set_csa_beacon(stru if ((params->n_counter_offsets_beacon > IEEE80211_MAX_CNTDWN_COUNTERS_NUM) || (params->n_counter_offsets_presp > - IEEE80211_MAX_CNTDWN_COUNTERS_NUM)) + IEEE80211_MAX_CNTDWN_COUNTERS_NUM)) { + kfree(sdata->u.ap.next_beacon->mbssid_ies); + kfree(sdata->u.ap.next_beacon); + sdata->u.ap.next_beacon = NULL; return -EINVAL; + } csa.counter_offsets_beacon = params->counter_offsets_beacon; csa.counter_offsets_presp = params->counter_offsets_presp; @@ -3373,7 +3436,9 @@ static int ieee80211_set_csa_beacon(stru err = ieee80211_assign_beacon(sdata, ¶ms->beacon_csa, &csa, NULL); if (err < 0) { + kfree(sdata->u.ap.next_beacon->mbssid_ies); kfree(sdata->u.ap.next_beacon); + sdata->u.ap.next_beacon = NULL; return err; } *changed |= err; @@ -3463,8 +3528,11 @@ static int ieee80211_set_csa_beacon(stru static void ieee80211_color_change_abort(struct ieee80211_sub_if_data *sdata) { sdata->vif.color_change_active = false; - kfree(sdata->u.ap.next_beacon); - sdata->u.ap.next_beacon = NULL; + if (sdata->u.ap.next_beacon) { + kfree(sdata->u.ap.next_beacon->mbssid_ies); + kfree(sdata->u.ap.next_beacon); + sdata->u.ap.next_beacon = NULL; + } cfg80211_color_change_aborted_notify(sdata->dev); } @@ -4202,8 +4270,11 @@ ieee80211_set_after_color_change_beacon( ret = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon, NULL, NULL); - kfree(sdata->u.ap.next_beacon); - sdata->u.ap.next_beacon = NULL; + if (sdata->u.ap.next_beacon) { + kfree(sdata->u.ap.next_beacon->mbssid_ies); + kfree(sdata->u.ap.next_beacon); + sdata->u.ap.next_beacon = NULL; + } if (ret < 0) return ret; @@ -4246,7 +4317,11 @@ ieee80211_set_color_change_beacon(struct err = ieee80211_assign_beacon(sdata, ¶ms->beacon_color_change, NULL, &color_change); if (err < 0) { - kfree(sdata->u.ap.next_beacon); + if (sdata->u.ap.next_beacon) { + kfree(sdata->u.ap.next_beacon->mbssid_ies); + kfree(sdata->u.ap.next_beacon); + sdata->u.ap.next_beacon = NULL; + } return err; } *changed |= err; --- a/net/mac80211/ieee80211_i.h +++ b/net/mac80211/ieee80211_i.h @@ -261,6 +261,7 @@ struct beacon_data { struct ieee80211_meshconf_ie *meshconf; u16 cntdwn_counter_offsets[IEEE80211_MAX_CNTDWN_COUNTERS_NUM]; u8 cntdwn_current_counter; + struct cfg80211_mbssid_elems *mbssid_ies; struct rcu_head rcu_head; }; @@ -1083,6 +1084,20 @@ ieee80211_vif_get_shift(struct ieee80211 return shift; } +static inline int +ieee80211_get_mbssid_beacon_len(struct cfg80211_mbssid_elems *elems) +{ + int i, len = 0; + + if (!elems) + return 0; + + for (i = 0; i < elems->cnt; i++) + len += elems->elem[i].len; + + return len; +} + enum { IEEE80211_RX_MSG = 1, IEEE80211_TX_STATUS_MSG = 2, --- a/net/mac80211/tx.c +++ b/net/mac80211/tx.c @@ -5041,6 +5041,19 @@ ieee80211_beacon_get_finish(struct ieee8 IEEE80211_TX_CTL_FIRST_FRAGMENT; } +static void +ieee80211_beacon_add_mbssid(struct sk_buff *skb, struct beacon_data *beacon) +{ + int i; + + if (!beacon->mbssid_ies) + return; + + for (i = 0; i < beacon->mbssid_ies->cnt; i++) + skb_put_data(skb, beacon->mbssid_ies->elem[i].data, + beacon->mbssid_ies->elem[i].len); +} + static struct sk_buff * ieee80211_beacon_get_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif, @@ -5054,6 +5067,7 @@ ieee80211_beacon_get_ap(struct ieee80211 struct ieee80211_if_ap *ap = &sdata->u.ap; struct sk_buff *skb = NULL; u16 csa_off_base = 0; + int mbssid_len; if (beacon->cntdwn_counter_offsets[0]) { if (!is_template) @@ -5063,11 +5077,12 @@ ieee80211_beacon_get_ap(struct ieee80211 } /* headroom, head length, - * tail length and maximum TIM length + * tail length, maximum TIM length and multiple BSSID length */ + mbssid_len = ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies); skb = dev_alloc_skb(local->tx_headroom + beacon->head_len + beacon->tail_len + 256 + - local->hw.extra_beacon_tailroom); + local->hw.extra_beacon_tailroom + mbssid_len); if (!skb) return NULL; @@ -5081,6 +5096,11 @@ ieee80211_beacon_get_ap(struct ieee80211 offs->tim_length = skb->len - beacon->head_len; offs->cntdwn_counter_offs[0] = beacon->cntdwn_counter_offsets[0]; + if (mbssid_len) { + ieee80211_beacon_add_mbssid(skb, beacon); + offs->mbssid_off = skb->len - mbssid_len; + } + /* for AP the csa offsets are from tail */ csa_off_base = skb->len; }