package/kernel/mac80211/patches/subsys/388-mac80211-add-support-for-BSS-color-change.patch

   1 From: John Crispin <john@phrozen.org>
   2 Date: Fri, 2 Jul 2021 19:44:08 +0200
   3 Subject: [PATCH] mac80211: add support for BSS color change
   4
   5 The color change announcement is very similar to how CSA works where
   6 we have an IE that includes a counter. When the counter hits 0, the new
   7 color is applied via an updated beacon.
   8
   9 This patch makes the CSA counter functionality reusable, rather than
  10 implementing it again. This also allows for future reuse incase support
  11 for other counter IEs gets added.
  12
  13 Co-developed-by: Lorenzo Bianconi <lorenzo@kernel.org>
  14 Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
  15 Signed-off-by: John Crispin <john@phrozen.org>
  16 Link: https://lore.kernel.org/r/057c1e67b82bee561ea44ce6a45a8462d3da6995.1625247619.git.lorenzo@kernel.org
  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 @@ -1713,6 +1713,10 @@ enum ieee80211_offload_flags {
  23   *     protected by fq->lock.
  24   * @offload_flags: 802.3 -> 802.11 enapsulation offload flags, see
  25   *     &enum ieee80211_offload_flags.
  26 + * @color_change_active: marks whether a color change is ongoing. Internally it is
  27 + *     write-protected by sdata_lock and local->mtx so holding either is fine
  28 + *     for read access.
  29 + * @color_change_color: the bss color that will be used after the change.
  30   */
  31  struct ieee80211_vif {
  32         enum nl80211_iftype type;
  33 @@ -1741,6 +1745,9 @@ struct ieee80211_vif {
  34
  35         bool txqs_stopped[IEEE80211_NUM_ACS];
  36
  37 +       bool color_change_active;
  38 +       u8 color_change_color;
  39 +
  40         /* must be last */
  41         u8 drv_priv[] __aligned(sizeof(void *));
  42  };
  43 @@ -4988,6 +4995,16 @@ void ieee80211_csa_finish(struct ieee802
  44  bool ieee80211_beacon_cntdwn_is_complete(struct ieee80211_vif *vif);
  45
  46  /**
  47 + * ieee80211_color_change_finish - notify mac80211 about color change
  48 + * @vif: &struct ieee80211_vif pointer from the add_interface callback.
  49 + *
  50 + * After a color change announcement was scheduled and the counter in this
  51 + * announcement hits 1, this function must be called by the driver to
  52 + * notify mac80211 that the color can be changed
  53 + */
  54 +void ieee80211_color_change_finish(struct ieee80211_vif *vif);
  55 +
  56 +/**
  57   * ieee80211_proberesp_get - retrieve a Probe Response template
  58   * @hw: pointer obtained from ieee80211_alloc_hw().
  59   * @vif: &struct ieee80211_vif pointer from the add_interface callback.
  60 @@ -6752,6 +6769,18 @@ ieee80211_get_unsol_bcast_probe_resp_tmp
  61                                           struct ieee80211_vif *vif);
  62
  63  /**
  64 + * ieeee80211_obss_color_collision_notify - notify userland about a BSS color
  65 + * collision.
  66 + *
  67 + * @vif: &struct ieee80211_vif pointer from the add_interface callback.
  68 + * @color_bitmap: a 64 bit bitmap representing the colors that the local BSS is
  69 + *     aware of.
  70 + */
  71 +void
  72 +ieeee80211_obss_color_collision_notify(struct ieee80211_vif *vif,
  73 +                                      u64 color_bitmap);
  74 +
  75 +/**
  76   * ieee80211_is_tx_data - check if frame is a data frame
  77   *
  78   * The function is used to check if a frame is a data frame. Frames with
  79 --- a/net/mac80211/cfg.c
  80 +++ b/net/mac80211/cfg.c
  81 @@ -809,9 +809,11 @@ static int ieee80211_set_monitor_channel
  82         return ret;
  83  }
  84
  85 -static int ieee80211_set_probe_resp(struct ieee80211_sub_if_data *sdata,
  86 -                                   const u8 *resp, size_t resp_len,
  87 -                                   const struct ieee80211_csa_settings *csa)
  88 +static int
  89 +ieee80211_set_probe_resp(struct ieee80211_sub_if_data *sdata,
  90 +                        const u8 *resp, size_t resp_len,
  91 +                        const struct ieee80211_csa_settings *csa,
  92 +                        const struct ieee80211_color_change_settings *cca)
  93  {
  94         struct probe_resp *new, *old;
  95
  96 @@ -831,6 +833,8 @@ static int ieee80211_set_probe_resp(stru
  97                 memcpy(new->cntdwn_counter_offsets, csa->counter_offsets_presp,
  98                        csa->n_counter_offsets_presp *
  99                        sizeof(new->cntdwn_counter_offsets[0]));
 100 +       else if (cca)
 101 +               new->cntdwn_counter_offsets[0] = cca->counter_offset_presp;
 102
 103         rcu_assign_pointer(sdata->u.ap.probe_resp, new);
 104         if (old)
 105 @@ -936,7 +940,8 @@ static int ieee80211_set_ftm_responder_p
 106
 107  static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 108                                    struct cfg80211_beacon_data *params,
 109 -                                  const struct ieee80211_csa_settings *csa)
 110 +                                  const struct ieee80211_csa_settings *csa,
 111 +                                  const struct ieee80211_color_change_settings *cca)
 112  {
 113         struct beacon_data *new, *old;
 114         int new_head_len, new_tail_len;
 115 @@ -985,6 +990,9 @@ static int ieee80211_assign_beacon(struc
 116                 memcpy(new->cntdwn_counter_offsets, csa->counter_offsets_beacon,
 117                        csa->n_counter_offsets_beacon *
 118                        sizeof(new->cntdwn_counter_offsets[0]));
 119 +       } else if (cca) {
 120 +               new->cntdwn_current_counter = cca->count;
 121 +               new->cntdwn_counter_offsets[0] = cca->counter_offset_beacon;
 122         }
 123
 124         /* copy in head */
 125 @@ -1001,7 +1009,7 @@ static int ieee80211_assign_beacon(struc
 126                         memcpy(new->tail, old->tail, new_tail_len);
 127
 128         err = ieee80211_set_probe_resp(sdata, params->probe_resp,
 129 -                                      params->probe_resp_len, csa);
 130 +                                      params->probe_resp_len, csa, cca);
 131         if (err < 0) {
 132                 kfree(new);
 133                 return err;
 134 @@ -1156,7 +1164,7 @@ static int ieee80211_start_ap(struct wip
 135         if (ieee80211_hw_check(&local->hw, HAS_RATE_CONTROL))
 136                 sdata->vif.bss_conf.beacon_tx_rate = params->beacon_rate;
 137
 138 -       err = ieee80211_assign_beacon(sdata, &params->beacon, NULL);
 139 +       err = ieee80211_assign_beacon(sdata, &params->beacon, NULL, NULL);
 140         if (err < 0)
 141                 goto error;
 142         changed |= err;
 143 @@ -1211,17 +1219,17 @@ static int ieee80211_change_beacon(struc
 144         sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 145         sdata_assert_lock(sdata);
 146
 147 -       /* don't allow changing the beacon while CSA is in place - offset
 148 +       /* don't allow changing the beacon while a countdown is in place - offset
 149          * of channel switch counter may change
 150          */
 151 -       if (sdata->vif.csa_active)
 152 +       if (sdata->vif.csa_active || sdata->vif.color_change_active)
 153                 return -EBUSY;
 154
 155         old = sdata_dereference(sdata->u.ap.beacon, sdata);
 156         if (!old)
 157                 return -ENOENT;
 158
 159 -       err = ieee80211_assign_beacon(sdata, params, NULL);
 160 +       err = ieee80211_assign_beacon(sdata, params, NULL, NULL);
 161         if (err < 0)
 162                 return err;
 163         ieee80211_bss_info_change_notify(sdata, err);
 164 @@ -3144,7 +3152,7 @@ static int ieee80211_set_after_csa_beaco
 165         switch (sdata->vif.type) {
 166         case NL80211_IFTYPE_AP:
 167                 err = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon,
 168 -                                             NULL);
 169 +                                             NULL, NULL);
 170                 kfree(sdata->u.ap.next_beacon);
 171                 sdata->u.ap.next_beacon = NULL;
 172
 173 @@ -3310,7 +3318,7 @@ static int ieee80211_set_csa_beacon(stru
 174                 csa.n_counter_offsets_presp = params->n_counter_offsets_presp;
 175                 csa.count = params->count;
 176
 177 -               err = ieee80211_assign_beacon(sdata, &params->beacon_csa, &csa);
 178 +               err = ieee80211_assign_beacon(sdata, &params->beacon_csa, &csa, NULL);
 179                 if (err < 0) {
 180                         kfree(sdata->u.ap.next_beacon);
 181                         return err;
 182 @@ -3399,6 +3407,15 @@ static int ieee80211_set_csa_beacon(stru
 183         return 0;
 184  }
 185
 186 +static void ieee80211_color_change_abort(struct ieee80211_sub_if_data  *sdata)
 187 +{
 188 +       sdata->vif.color_change_active = false;
 189 +       kfree(sdata->u.ap.next_beacon);
 190 +       sdata->u.ap.next_beacon = NULL;
 191 +
 192 +       cfg80211_color_change_aborted_notify(sdata->dev);
 193 +}
 194 +
 195  static int
 196  __ieee80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
 197                            struct cfg80211_csa_settings *params)
 198 @@ -3467,6 +3484,10 @@ __ieee80211_channel_switch(struct wiphy
 199                 goto out;
 200         }
 201
 202 +       /* if there is a color change in progress, abort it */
 203 +       if (sdata->vif.color_change_active)
 204 +               ieee80211_color_change_abort(sdata);
 205 +
 206         err = ieee80211_set_csa_beacon(sdata, params, &changed);
 207         if (err) {
 208                 ieee80211_vif_unreserve_chanctx(sdata);
 209 @@ -4118,6 +4139,196 @@ static int ieee80211_set_sar_specs(struc
 210         return local->ops->set_sar_specs(&local->hw, sar);
 211  }
 212
 213 +static int
 214 +ieee80211_set_after_color_change_beacon(struct ieee80211_sub_if_data *sdata,
 215 +                                       u32 *changed)
 216 +{
 217 +       switch (sdata->vif.type) {
 218 +       case NL80211_IFTYPE_AP: {
 219 +               int ret;
 220 +
 221 +               ret = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon,
 222 +                                             NULL, NULL);
 223 +               kfree(sdata->u.ap.next_beacon);
 224 +               sdata->u.ap.next_beacon = NULL;
 225 +
 226 +               if (ret < 0)
 227 +                       return ret;
 228 +
 229 +               *changed |= ret;
 230 +               break;
 231 +       }
 232 +       default:
 233 +               WARN_ON_ONCE(1);
 234 +               return -EINVAL;
 235 +       }
 236 +
 237 +       return 0;
 238 +}
 239 +
 240 +static int
 241 +ieee80211_set_color_change_beacon(struct ieee80211_sub_if_data *sdata,
 242 +                                 struct cfg80211_color_change_settings *params,
 243 +                                 u32 *changed)
 244 +{
 245 +       struct ieee80211_color_change_settings color_change = {};
 246 +       int err;
 247 +
 248 +       switch (sdata->vif.type) {
 249 +       case NL80211_IFTYPE_AP:
 250 +               sdata->u.ap.next_beacon =
 251 +                       cfg80211_beacon_dup(&params->beacon_next);
 252 +               if (!sdata->u.ap.next_beacon)
 253 +                       return -ENOMEM;
 254 +
 255 +               if (params->count <= 1)
 256 +                       break;
 257 +
 258 +               color_change.counter_offset_beacon =
 259 +                       params->counter_offset_beacon;
 260 +               color_change.counter_offset_presp =
 261 +                       params->counter_offset_presp;
 262 +               color_change.count = params->count;
 263 +
 264 +               err = ieee80211_assign_beacon(sdata, &params->beacon_color_change,
 265 +                                             NULL, &color_change);
 266 +               if (err < 0) {
 267 +                       kfree(sdata->u.ap.next_beacon);
 268 +                       return err;
 269 +               }
 270 +               *changed |= err;
 271 +               break;
 272 +       default:
 273 +               return -EOPNOTSUPP;
 274 +       }
 275 +
 276 +       return 0;
 277 +}
 278 +
 279 +static void
 280 +ieee80211_color_change_bss_config_notify(struct ieee80211_sub_if_data *sdata,
 281 +                                        u8 color, int enable, u32 changed)
 282 +{
 283 +       sdata->vif.bss_conf.he_bss_color.color = color;
 284 +       sdata->vif.bss_conf.he_bss_color.enabled = enable;
 285 +       changed |= BSS_CHANGED_HE_BSS_COLOR;
 286 +
 287 +       ieee80211_bss_info_change_notify(sdata, changed);
 288 +}
 289 +
 290 +static int ieee80211_color_change_finalize(struct ieee80211_sub_if_data *sdata)
 291 +{
 292 +       struct ieee80211_local *local = sdata->local;
 293 +       u32 changed = 0;
 294 +       int err;
 295 +
 296 +       sdata_assert_lock(sdata);
 297 +       lockdep_assert_held(&local->mtx);
 298 +
 299 +       sdata->vif.color_change_active = false;
 300 +
 301 +       err = ieee80211_set_after_color_change_beacon(sdata, &changed);
 302 +       if (err) {
 303 +               cfg80211_color_change_aborted_notify(sdata->dev);
 304 +               return err;
 305 +       }
 306 +
 307 +       ieee80211_color_change_bss_config_notify(sdata,
 308 +                                                sdata->vif.color_change_color,
 309 +                                                1, changed);
 310 +       cfg80211_color_change_notify(sdata->dev);
 311 +
 312 +       return 0;
 313 +}
 314 +
 315 +void ieee80211_color_change_finalize_work(struct work_struct *work)
 316 +{
 317 +       struct ieee80211_sub_if_data *sdata =
 318 +               container_of(work, struct ieee80211_sub_if_data,
 319 +                            color_change_finalize_work);
 320 +       struct ieee80211_local *local = sdata->local;
 321 +
 322 +       sdata_lock(sdata);
 323 +       mutex_lock(&local->mtx);
 324 +
 325 +       /* AP might have been stopped while waiting for the lock. */
 326 +       if (!sdata->vif.color_change_active)
 327 +               goto unlock;
 328 +
 329 +       if (!ieee80211_sdata_running(sdata))
 330 +               goto unlock;
 331 +
 332 +       ieee80211_color_change_finalize(sdata);
 333 +
 334 +unlock:
 335 +       mutex_unlock(&local->mtx);
 336 +       sdata_unlock(sdata);
 337 +}
 338 +
 339 +void ieee80211_color_change_finish(struct ieee80211_vif *vif)
 340 +{
 341 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
 342 +
 343 +       ieee80211_queue_work(&sdata->local->hw,
 344 +                            &sdata->color_change_finalize_work);
 345 +}
 346 +EXPORT_SYMBOL_GPL(ieee80211_color_change_finish);
 347 +
 348 +void
 349 +ieeee80211_obss_color_collision_notify(struct ieee80211_vif *vif,
 350 +                                      u64 color_bitmap)
 351 +{
 352 +       struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
 353 +
 354 +       if (sdata->vif.color_change_active || sdata->vif.csa_active)
 355 +               return;
 356 +
 357 +       cfg80211_obss_color_collision_notify(sdata->dev, color_bitmap);
 358 +}
 359 +EXPORT_SYMBOL_GPL(ieeee80211_obss_color_collision_notify);
 360 +
 361 +static int
 362 +ieee80211_color_change(struct wiphy *wiphy, struct net_device *dev,
 363 +                      struct cfg80211_color_change_settings *params)
 364 +{
 365 +       struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 366 +       struct ieee80211_local *local = sdata->local;
 367 +       u32 changed = 0;
 368 +       int err;
 369 +
 370 +       sdata_assert_lock(sdata);
 371 +
 372 +       mutex_lock(&local->mtx);
 373 +
 374 +       /* don't allow another color change if one is already active or if csa
 375 +        * is active
 376 +        */
 377 +       if (sdata->vif.color_change_active || sdata->vif.csa_active) {
 378 +               err = -EBUSY;
 379 +               goto out;
 380 +       }
 381 +
 382 +       err = ieee80211_set_color_change_beacon(sdata, params, &changed);
 383 +       if (err)
 384 +               goto out;
 385 +
 386 +       sdata->vif.color_change_active = true;
 387 +       sdata->vif.color_change_color = params->color;
 388 +
 389 +       cfg80211_color_change_started_notify(sdata->dev, params->count);
 390 +
 391 +       if (changed)
 392 +               ieee80211_color_change_bss_config_notify(sdata, 0, 0, changed);
 393 +       else
 394 +               /* if the beacon didn't change, we can finalize immediately */
 395 +               ieee80211_color_change_finalize(sdata);
 396 +
 397 +out:
 398 +       mutex_unlock(&local->mtx);
 399 +
 400 +       return err;
 401 +}
 402 +
 403  const struct cfg80211_ops mac80211_config_ops = {
 404         .add_virtual_intf = ieee80211_add_iface,
 405         .del_virtual_intf = ieee80211_del_iface,
 406 @@ -4221,4 +4432,5 @@ const struct cfg80211_ops mac80211_confi
 407         .set_tid_config = ieee80211_set_tid_config,
 408         .reset_tid_config = ieee80211_reset_tid_config,
 409         .set_sar_specs = ieee80211_set_sar_specs,
 410 +       .color_change = ieee80211_color_change,
 411  };
 412 --- a/net/mac80211/ieee80211_i.h
 413 +++ b/net/mac80211/ieee80211_i.h
 414 @@ -248,6 +248,12 @@ struct ieee80211_csa_settings {
 415         u8 count;
 416  };
 417
 418 +struct ieee80211_color_change_settings {
 419 +       u16 counter_offset_beacon;
 420 +       u16 counter_offset_presp;
 421 +       u8 count;
 422 +};
 423 +
 424  struct beacon_data {
 425         u8 *head, *tail;
 426         int head_len, tail_len;
 427 @@ -927,6 +933,8 @@ struct ieee80211_sub_if_data {
 428         bool csa_block_tx; /* write-protected by sdata_lock and local->mtx */
 429         struct cfg80211_chan_def csa_chandef;
 430
 431 +       struct work_struct color_change_finalize_work;
 432 +
 433         struct list_head assigned_chanctx_list; /* protected by chanctx_mtx */
 434         struct list_head reserved_chanctx_list; /* protected by chanctx_mtx */
 435
 436 @@ -1893,6 +1901,9 @@ void ieee80211_csa_finalize_work(struct
 437  int ieee80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
 438                              struct cfg80211_csa_settings *params);
 439
 440 +/* color change handling */
 441 +void ieee80211_color_change_finalize_work(struct work_struct *work);
 442 +
 443  /* interface handling */
 444  #define MAC80211_SUPPORTED_FEATURES_TX (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | \
 445                                          NETIF_F_HW_CSUM | NETIF_F_SG | \
 446 --- a/net/mac80211/iface.c
 447 +++ b/net/mac80211/iface.c
 448 @@ -461,6 +461,7 @@ static void ieee80211_do_stop(struct iee
 449         sdata_unlock(sdata);
 450
 451         cancel_work_sync(&sdata->csa_finalize_work);
 452 +       cancel_work_sync(&sdata->color_change_finalize_work);
 453
 454         cancel_delayed_work_sync(&sdata->dfs_cac_timer_work);
 455
 456 @@ -1572,6 +1573,7 @@ static void ieee80211_setup_sdata(struct
 457         INIT_WORK(&sdata->work, ieee80211_iface_work);
 458         INIT_WORK(&sdata->recalc_smps, ieee80211_recalc_smps_work);
 459         INIT_WORK(&sdata->csa_finalize_work, ieee80211_csa_finalize_work);
 460 +       INIT_WORK(&sdata->color_change_finalize_work, ieee80211_color_change_finalize_work);
 461         INIT_LIST_HEAD(&sdata->assigned_chanctx_list);
 462         INIT_LIST_HEAD(&sdata->reserved_chanctx_list);
 463
 464 --- a/net/mac80211/tx.c
 465 +++ b/net/mac80211/tx.c
 466 @@ -4773,11 +4773,11 @@ static int ieee80211_beacon_add_tim(stru
 467  static void ieee80211_set_beacon_cntdwn(struct ieee80211_sub_if_data *sdata,
 468                                         struct beacon_data *beacon)
 469  {
 470 +       u8 *beacon_data, count, max_count = 1;
 471         struct probe_resp *resp;
 472 -       u8 *beacon_data;
 473         size_t beacon_data_len;
 474 +       u16 *bcn_offsets;
 475         int i;
 476 -       u8 count = beacon->cntdwn_current_counter;
 477
 478         switch (sdata->vif.type) {
 479         case NL80211_IFTYPE_AP:
 480 @@ -4797,21 +4797,27 @@ static void ieee80211_set_beacon_cntdwn(
 481         }
 482
 483         rcu_read_lock();
 484 -       for (i = 0; i < IEEE80211_MAX_CNTDWN_COUNTERS_NUM; ++i) {
 485 -               resp = rcu_dereference(sdata->u.ap.probe_resp);
 486 +       resp = rcu_dereference(sdata->u.ap.probe_resp);
 487
 488 -               if (beacon->cntdwn_counter_offsets[i]) {
 489 -                       if (WARN_ON_ONCE(beacon->cntdwn_counter_offsets[i] >=
 490 -                                        beacon_data_len)) {
 491 +       bcn_offsets = beacon->cntdwn_counter_offsets;
 492 +       count = beacon->cntdwn_current_counter;
 493 +       if (sdata->vif.csa_active)
 494 +               max_count = IEEE80211_MAX_CNTDWN_COUNTERS_NUM;
 495 +
 496 +       for (i = 0; i < max_count; ++i) {
 497 +               if (bcn_offsets[i]) {
 498 +                       if (WARN_ON_ONCE(bcn_offsets[i] >= beacon_data_len)) {
 499                                 rcu_read_unlock();
 500                                 return;
 501                         }
 502 -
 503 -                       beacon_data[beacon->cntdwn_counter_offsets[i]] = count;
 504 +                       beacon_data[bcn_offsets[i]] = count;
 505                 }
 506
 507 -               if (sdata->vif.type == NL80211_IFTYPE_AP && resp)
 508 -                       resp->data[resp->cntdwn_counter_offsets[i]] = count;
 509 +               if (sdata->vif.type == NL80211_IFTYPE_AP && resp) {
 510 +                       u16 *resp_offsets = resp->cntdwn_counter_offsets;
 511 +
 512 +                       resp->data[resp_offsets[i]] = count;
 513 +               }
 514         }
 515         rcu_read_unlock();
 516  }
 517 @@ -5021,6 +5027,7 @@ __ieee80211_beacon_get(struct ieee80211_
 518                         if (offs) {
 519                                 offs->tim_offset = beacon->head_len;
 520                                 offs->tim_length = skb->len - beacon->head_len;
 521 +                               offs->cntdwn_counter_offs[0] = beacon->cntdwn_counter_offsets[0];
 522
 523                                 /* for AP the csa offsets are from tail */
 524                                 csa_off_base = skb->len;