package/kernel/mac80211/patches/subsys/324-v5.18-mac80211-MBSSID-beacon-handling-in-AP-mode.patch

   1 From: Lorenzo Bianconi <lorenzo@kernel.org>
   2 Date: Thu, 24 Feb 2022 12:54:58 +0100
   3 Subject: [PATCH] mac80211: MBSSID beacon handling in AP mode
   4
   5 Add new fields in struct beacon_data to store all MBSSID elements.
   6 Generate a beacon template which includes all MBSSID elements.
   7 Move CSA offset to reflect the MBSSID element length.
   8
   9 Co-developed-by: Aloka Dixit <alokad@codeaurora.org>
  10 Signed-off-by: Aloka Dixit <alokad@codeaurora.org>
  11 Co-developed-by: John Crispin <john@phrozen.org>
  12 Signed-off-by: John Crispin <john@phrozen.org>
  13 Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
  14 Tested-by: Money Wang <money.wang@mediatek.com>
  15 Link: https://lore.kernel.org/r/5322db3c303f431adaf191ab31c45e151dde5465.1645702516.git.lorenzo@kernel.org
  16 [small cleanups]
  17 Signed-off-by: Johannes Berg <johannes.berg@intel.com>
  18 ---
  19
  20 --- a/include/net/mac80211.h
  21 +++ b/include/net/mac80211.h
  22 @@ -4938,12 +4938,14 @@ void ieee80211_report_low_ack(struct iee
  23   * @cntdwn_counter_offs: array of IEEE80211_MAX_CNTDWN_COUNTERS_NUM offsets
  24   *     to countdown counters.  This array can contain zero values which
  25   *     should be ignored.
  26 + * @mbssid_off: position of the multiple bssid element
  27   */
  28  struct ieee80211_mutable_offsets {
  29         u16 tim_offset;
  30         u16 tim_length;
  31
  32         u16 cntdwn_counter_offs[IEEE80211_MAX_CNTDWN_COUNTERS_NUM];
  33 +       u16 mbssid_off;
  34  };
  35
  36  /**
  37 --- a/net/mac80211/cfg.c
  38 +++ b/net/mac80211/cfg.c
  39 @@ -989,11 +989,29 @@ static int ieee80211_set_ftm_responder_p
  40         return 0;
  41  }
  42
  43 +static int
  44 +ieee80211_copy_mbssid_beacon(u8 *pos, struct cfg80211_mbssid_elems *dst,
  45 +                            struct cfg80211_mbssid_elems *src)
  46 +{
  47 +       int i, offset = 0;
  48 +
  49 +       for (i = 0; i < src->cnt; i++) {
  50 +               memcpy(pos + offset, src->elem[i].data, src->elem[i].len);
  51 +               dst->elem[i].len = src->elem[i].len;
  52 +               dst->elem[i].data = pos + offset;
  53 +               offset += dst->elem[i].len;
  54 +       }
  55 +       dst->cnt = src->cnt;
  56 +
  57 +       return offset;
  58 +}
  59 +
  60  static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
  61                                    struct cfg80211_beacon_data *params,
  62                                    const struct ieee80211_csa_settings *csa,
  63                                    const struct ieee80211_color_change_settings *cca)
  64  {
  65 +       struct cfg80211_mbssid_elems *mbssid = NULL;
  66         struct beacon_data *new, *old;
  67         int new_head_len, new_tail_len;
  68         int size, err;
  69 @@ -1021,6 +1039,17 @@ static int ieee80211_assign_beacon(struc
  70
  71         size = sizeof(*new) + new_head_len + new_tail_len;
  72
  73 +       /* new or old multiple BSSID elements? */
  74 +       if (params->mbssid_ies) {
  75 +               mbssid = params->mbssid_ies;
  76 +               size += struct_size(new->mbssid_ies, elem, mbssid->cnt);
  77 +               size += ieee80211_get_mbssid_beacon_len(mbssid);
  78 +       } else if (old && old->mbssid_ies) {
  79 +               mbssid = old->mbssid_ies;
  80 +               size += struct_size(new->mbssid_ies, elem, mbssid->cnt);
  81 +               size += ieee80211_get_mbssid_beacon_len(mbssid);
  82 +       }
  83 +
  84         new = kzalloc(size, GFP_KERNEL);
  85         if (!new)
  86                 return -ENOMEM;
  87 @@ -1029,12 +1058,20 @@ static int ieee80211_assign_beacon(struc
  88
  89         /*
  90          * pointers go into the block we allocated,
  91 -        * memory is | beacon_data | head | tail |
  92 +        * memory is | beacon_data | head | tail | mbssid_ies
  93          */
  94         new->head = ((u8 *) new) + sizeof(*new);
  95         new->tail = new->head + new_head_len;
  96         new->head_len = new_head_len;
  97         new->tail_len = new_tail_len;
  98 +       /* copy in optional mbssid_ies */
  99 +       if (mbssid) {
 100 +               u8 *pos = new->tail + new->tail_len;
 101 +
 102 +               new->mbssid_ies = (void *)pos;
 103 +               pos += struct_size(new->mbssid_ies, elem, mbssid->cnt);
 104 +               ieee80211_copy_mbssid_beacon(pos, new->mbssid_ies, mbssid);
 105 +       }
 106
 107         if (csa) {
 108                 new->cntdwn_current_counter = csa->count;
 109 @@ -1332,8 +1369,11 @@ static int ieee80211_stop_ap(struct wiph
 110
 111         mutex_unlock(&local->mtx);
 112
 113 -       kfree(sdata->u.ap.next_beacon);
 114 -       sdata->u.ap.next_beacon = NULL;
 115 +       if (sdata->u.ap.next_beacon) {
 116 +               kfree(sdata->u.ap.next_beacon->mbssid_ies);
 117 +               kfree(sdata->u.ap.next_beacon);
 118 +               sdata->u.ap.next_beacon = NULL;
 119 +       }
 120
 121         /* turn off carrier for this interface and dependent VLANs */
 122         list_for_each_entry(vlan, &sdata->u.ap.vlans, u.vlan.list)
 123 @@ -3126,12 +3166,24 @@ cfg80211_beacon_dup(struct cfg80211_beac
 124
 125         len = beacon->head_len + beacon->tail_len + beacon->beacon_ies_len +
 126               beacon->proberesp_ies_len + beacon->assocresp_ies_len +
 127 -             beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len;
 128 +             beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len +
 129 +             ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies);
 130
 131         new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL);
 132         if (!new_beacon)
 133                 return NULL;
 134
 135 +       if (beacon->mbssid_ies && beacon->mbssid_ies->cnt) {
 136 +               new_beacon->mbssid_ies =
 137 +                       kzalloc(struct_size(new_beacon->mbssid_ies,
 138 +                                           elem, beacon->mbssid_ies->cnt),
 139 +                               GFP_KERNEL);
 140 +               if (!new_beacon->mbssid_ies) {
 141 +                       kfree(new_beacon);
 142 +                       return NULL;
 143 +               }
 144 +       }
 145 +
 146         pos = (u8 *)(new_beacon + 1);
 147         if (beacon->head_len) {
 148                 new_beacon->head_len = beacon->head_len;
 149 @@ -3169,6 +3221,10 @@ cfg80211_beacon_dup(struct cfg80211_beac
 150                 memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
 151                 pos += beacon->probe_resp_len;
 152         }
 153 +       if (beacon->mbssid_ies && beacon->mbssid_ies->cnt)
 154 +               pos += ieee80211_copy_mbssid_beacon(pos,
 155 +                                                   new_beacon->mbssid_ies,
 156 +                                                   beacon->mbssid_ies);
 157
 158         /* might copy -1, meaning no changes requested */
 159         new_beacon->ftm_responder = beacon->ftm_responder;
 160 @@ -3206,8 +3262,11 @@ static int ieee80211_set_after_csa_beaco
 161         case NL80211_IFTYPE_AP:
 162                 err = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon,
 163                                               NULL, NULL);
 164 -               kfree(sdata->u.ap.next_beacon);
 165 -               sdata->u.ap.next_beacon = NULL;
 166 +               if (sdata->u.ap.next_beacon) {
 167 +                       kfree(sdata->u.ap.next_beacon->mbssid_ies);
 168 +                       kfree(sdata->u.ap.next_beacon);
 169 +                       sdata->u.ap.next_beacon = NULL;
 170 +               }
 171
 172                 if (err < 0)
 173                         return err;
 174 @@ -3362,8 +3421,12 @@ static int ieee80211_set_csa_beacon(stru
 175                 if ((params->n_counter_offsets_beacon >
 176                      IEEE80211_MAX_CNTDWN_COUNTERS_NUM) ||
 177                     (params->n_counter_offsets_presp >
 178 -                    IEEE80211_MAX_CNTDWN_COUNTERS_NUM))
 179 +                    IEEE80211_MAX_CNTDWN_COUNTERS_NUM)) {
 180 +                       kfree(sdata->u.ap.next_beacon->mbssid_ies);
 181 +                       kfree(sdata->u.ap.next_beacon);
 182 +                       sdata->u.ap.next_beacon = NULL;
 183                         return -EINVAL;
 184 +               }
 185
 186                 csa.counter_offsets_beacon = params->counter_offsets_beacon;
 187                 csa.counter_offsets_presp = params->counter_offsets_presp;
 188 @@ -3373,7 +3436,9 @@ static int ieee80211_set_csa_beacon(stru
 189
 190                 err = ieee80211_assign_beacon(sdata, &params->beacon_csa, &csa, NULL);
 191                 if (err < 0) {
 192 +                       kfree(sdata->u.ap.next_beacon->mbssid_ies);
 193                         kfree(sdata->u.ap.next_beacon);
 194 +                       sdata->u.ap.next_beacon = NULL;
 195                         return err;
 196                 }
 197                 *changed |= err;
 198 @@ -3460,8 +3525,11 @@ static int ieee80211_set_csa_beacon(stru
 199  static void ieee80211_color_change_abort(struct ieee80211_sub_if_data  *sdata)
 200  {
 201         sdata->vif.color_change_active = false;
 202 -       kfree(sdata->u.ap.next_beacon);
 203 -       sdata->u.ap.next_beacon = NULL;
 204 +       if (sdata->u.ap.next_beacon) {
 205 +               kfree(sdata->u.ap.next_beacon->mbssid_ies);
 206 +               kfree(sdata->u.ap.next_beacon);
 207 +               sdata->u.ap.next_beacon = NULL;
 208 +       }
 209
 210         cfg80211_color_change_aborted_notify(sdata->dev);
 211  }
 212 @@ -4199,8 +4267,11 @@ ieee80211_set_after_color_change_beacon(
 213
 214                 ret = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon,
 215                                               NULL, NULL);
 216 -               kfree(sdata->u.ap.next_beacon);
 217 -               sdata->u.ap.next_beacon = NULL;
 218 +               if (sdata->u.ap.next_beacon) {
 219 +                       kfree(sdata->u.ap.next_beacon->mbssid_ies);
 220 +                       kfree(sdata->u.ap.next_beacon);
 221 +                       sdata->u.ap.next_beacon = NULL;
 222 +               }
 223
 224                 if (ret < 0)
 225                         return ret;
 226 @@ -4243,7 +4314,11 @@ ieee80211_set_color_change_beacon(struct
 227                 err = ieee80211_assign_beacon(sdata, &params->beacon_color_change,
 228                                               NULL, &color_change);
 229                 if (err < 0) {
 230 -                       kfree(sdata->u.ap.next_beacon);
 231 +                       if (sdata->u.ap.next_beacon) {
 232 +                               kfree(sdata->u.ap.next_beacon->mbssid_ies);
 233 +                               kfree(sdata->u.ap.next_beacon);
 234 +                               sdata->u.ap.next_beacon = NULL;
 235 +                       }
 236                         return err;
 237                 }
 238                 *changed |= err;
 239 --- a/net/mac80211/ieee80211_i.h
 240 +++ b/net/mac80211/ieee80211_i.h
 241 @@ -261,6 +261,7 @@ struct beacon_data {
 242         struct ieee80211_meshconf_ie *meshconf;
 243         u16 cntdwn_counter_offsets[IEEE80211_MAX_CNTDWN_COUNTERS_NUM];
 244         u8 cntdwn_current_counter;
 245 +       struct cfg80211_mbssid_elems *mbssid_ies;
 246         struct rcu_head rcu_head;
 247  };
 248
 249 @@ -1082,6 +1083,20 @@ ieee80211_vif_get_shift(struct ieee80211
 250         return shift;
 251  }
 252
 253 +static inline int
 254 +ieee80211_get_mbssid_beacon_len(struct cfg80211_mbssid_elems *elems)
 255 +{
 256 +       int i, len = 0;
 257 +
 258 +       if (!elems)
 259 +               return 0;
 260 +
 261 +       for (i = 0; i < elems->cnt; i++)
 262 +               len += elems->elem[i].len;
 263 +
 264 +       return len;
 265 +}
 266 +
 267  enum {
 268         IEEE80211_RX_MSG        = 1,
 269         IEEE80211_TX_STATUS_MSG = 2,
 270 --- a/net/mac80211/tx.c
 271 +++ b/net/mac80211/tx.c
 272 @@ -5041,6 +5041,19 @@ ieee80211_beacon_get_finish(struct ieee8
 273                        IEEE80211_TX_CTL_FIRST_FRAGMENT;
 274  }
 275
 276 +static void
 277 +ieee80211_beacon_add_mbssid(struct sk_buff *skb, struct beacon_data *beacon)
 278 +{
 279 +       int i;
 280 +
 281 +       if (!beacon->mbssid_ies)
 282 +               return;
 283 +
 284 +       for (i = 0; i < beacon->mbssid_ies->cnt; i++)
 285 +               skb_put_data(skb, beacon->mbssid_ies->elem[i].data,
 286 +                            beacon->mbssid_ies->elem[i].len);
 287 +}
 288 +
 289  static struct sk_buff *
 290  ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
 291                         struct ieee80211_vif *vif,
 292 @@ -5054,6 +5067,7 @@ ieee80211_beacon_get_ap(struct ieee80211
 293         struct ieee80211_if_ap *ap = &sdata->u.ap;
 294         struct sk_buff *skb = NULL;
 295         u16 csa_off_base = 0;
 296 +       int mbssid_len;
 297
 298         if (beacon->cntdwn_counter_offsets[0]) {
 299                 if (!is_template)
 300 @@ -5063,11 +5077,12 @@ ieee80211_beacon_get_ap(struct ieee80211
 301         }
 302
 303         /* headroom, head length,
 304 -        * tail length and maximum TIM length
 305 +        * tail length, maximum TIM length and multiple BSSID length
 306          */
 307 +       mbssid_len = ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies);
 308         skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
 309                             beacon->tail_len + 256 +
 310 -                           local->hw.extra_beacon_tailroom);
 311 +                           local->hw.extra_beacon_tailroom + mbssid_len);
 312         if (!skb)
 313                 return NULL;
 314
 315 @@ -5081,6 +5096,11 @@ ieee80211_beacon_get_ap(struct ieee80211
 316                 offs->tim_length = skb->len - beacon->head_len;
 317                 offs->cntdwn_counter_offs[0] = beacon->cntdwn_counter_offsets[0];
 318
 319 +               if (mbssid_len) {
 320 +                       ieee80211_beacon_add_mbssid(skb, beacon);
 321 +                       offs->mbssid_off = skb->len - mbssid_len;
 322 +               }
 323 +
 324                 /* for AP the csa offsets are from tail */
 325                 csa_off_base = skb->len;
 326         }