1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * NAN mode implementation
4 * Copyright(c) 2025-2026 Intel Corporation
5 */
6 #include <net/mac80211.h>
7
8 #include "ieee80211_i.h"
9 #include "driver-ops.h"
10 #include "sta_info.h"
11
12 static void
ieee80211_nan_init_channel(struct ieee80211_nan_channel * nan_channel,struct cfg80211_nan_channel * cfg_nan_channel)13 ieee80211_nan_init_channel(struct ieee80211_nan_channel *nan_channel,
14 struct cfg80211_nan_channel *cfg_nan_channel)
15 {
16 memset(nan_channel, 0, sizeof(*nan_channel));
17
18 nan_channel->chanreq.oper = cfg_nan_channel->chandef;
19 memcpy(nan_channel->channel_entry, cfg_nan_channel->channel_entry,
20 sizeof(nan_channel->channel_entry));
21 nan_channel->needed_rx_chains = cfg_nan_channel->rx_nss;
22 }
23
24 static void
ieee80211_nan_update_channel(struct ieee80211_local * local,struct ieee80211_nan_channel * nan_channel,struct cfg80211_nan_channel * cfg_nan_channel,bool deferred)25 ieee80211_nan_update_channel(struct ieee80211_local *local,
26 struct ieee80211_nan_channel *nan_channel,
27 struct cfg80211_nan_channel *cfg_nan_channel,
28 bool deferred)
29 {
30 struct ieee80211_chanctx_conf *conf;
31 bool reducing_nss;
32
33 if (WARN_ON(!cfg80211_chandef_identical(&nan_channel->chanreq.oper,
34 &cfg_nan_channel->chandef)))
35 return;
36
37 if (WARN_ON(memcmp(nan_channel->channel_entry,
38 cfg_nan_channel->channel_entry,
39 sizeof(nan_channel->channel_entry))))
40 return;
41
42 if (nan_channel->needed_rx_chains == cfg_nan_channel->rx_nss)
43 return;
44
45 reducing_nss = nan_channel->needed_rx_chains > cfg_nan_channel->rx_nss;
46 nan_channel->needed_rx_chains = cfg_nan_channel->rx_nss;
47
48 conf = nan_channel->chanctx_conf;
49
50 /*
51 * If we are adding NSSs, we need to be ready before notifying the peer,
52 * if we are reducing NSSs, we need to wait until the peer is notified.
53 */
54 if (!conf || (deferred && reducing_nss))
55 return;
56
57 ieee80211_recalc_smps_chanctx(local, container_of(conf,
58 struct ieee80211_chanctx,
59 conf));
60 }
61
62 static int
ieee80211_nan_use_chanctx(struct ieee80211_sub_if_data * sdata,struct ieee80211_nan_channel * nan_channel,bool assign_on_failure)63 ieee80211_nan_use_chanctx(struct ieee80211_sub_if_data *sdata,
64 struct ieee80211_nan_channel *nan_channel,
65 bool assign_on_failure)
66 {
67 struct ieee80211_chanctx *ctx;
68 bool reused_ctx;
69
70 if (!nan_channel->chanreq.oper.chan)
71 return -EINVAL;
72
73 if (ieee80211_check_combinations(sdata, &nan_channel->chanreq.oper,
74 IEEE80211_CHANCTX_SHARED, 0, -1))
75 return -EBUSY;
76
77 ctx = ieee80211_find_or_create_chanctx(sdata, &nan_channel->chanreq,
78 IEEE80211_CHANCTX_SHARED,
79 assign_on_failure,
80 &reused_ctx);
81 if (IS_ERR(ctx))
82 return PTR_ERR(ctx);
83
84 nan_channel->chanctx_conf = &ctx->conf;
85
86 /*
87 * In case an existing channel context is being used, we marked it as
88 * will_be_used, now that it is assigned - clear this indication
89 */
90 if (reused_ctx) {
91 WARN_ON(!ctx->will_be_used);
92 ctx->will_be_used = false;
93 }
94 ieee80211_recalc_chanctx_min_def(sdata->local, ctx);
95 ieee80211_recalc_smps_chanctx(sdata->local, ctx);
96
97 return 0;
98 }
99
100 static void
ieee80211_nan_update_peer_channels(struct ieee80211_sub_if_data * sdata,struct ieee80211_chanctx_conf * removed_conf)101 ieee80211_nan_update_peer_channels(struct ieee80211_sub_if_data *sdata,
102 struct ieee80211_chanctx_conf *removed_conf)
103 {
104 struct ieee80211_local *local = sdata->local;
105 struct sta_info *sta;
106
107 lockdep_assert_wiphy(local->hw.wiphy);
108
109 list_for_each_entry(sta, &local->sta_list, list) {
110 struct ieee80211_nan_peer_sched *peer_sched;
111 int write_idx = 0;
112 bool updated = false;
113
114 if (sta->sdata != sdata)
115 continue;
116
117 peer_sched = sta->sta.nan_sched;
118 if (!peer_sched)
119 continue;
120
121 /* NULL out map slots for channels being removed */
122 for (int i = 0; i < peer_sched->n_channels; i++) {
123 if (peer_sched->channels[i].chanctx_conf != removed_conf)
124 continue;
125
126 for (int m = 0; m < CFG80211_NAN_MAX_PEER_MAPS; m++) {
127 struct ieee80211_nan_peer_map *map =
128 &peer_sched->maps[m];
129
130 if (map->map_id == CFG80211_NAN_INVALID_MAP_ID)
131 continue;
132
133 for (int s = 0; s < ARRAY_SIZE(map->slots); s++)
134 if (map->slots[s] == &peer_sched->channels[i])
135 map->slots[s] = NULL;
136 }
137 }
138
139 /* Compact channels array, removing those with removed_conf */
140 for (int i = 0; i < peer_sched->n_channels; i++) {
141 if (peer_sched->channels[i].chanctx_conf == removed_conf) {
142 updated = true;
143 continue;
144 }
145
146 if (write_idx != i) {
147 /* Update map pointers before moving */
148 for (int m = 0; m < CFG80211_NAN_MAX_PEER_MAPS; m++) {
149 struct ieee80211_nan_peer_map *map =
150 &peer_sched->maps[m];
151
152 if (map->map_id == CFG80211_NAN_INVALID_MAP_ID)
153 continue;
154
155 for (int s = 0; s < ARRAY_SIZE(map->slots); s++)
156 if (map->slots[s] == &peer_sched->channels[i])
157 map->slots[s] = &peer_sched->channels[write_idx];
158 }
159
160 peer_sched->channels[write_idx] = peer_sched->channels[i];
161 }
162 write_idx++;
163 }
164
165 /* Clear any remaining entries at the end */
166 for (int i = write_idx; i < peer_sched->n_channels; i++)
167 memset(&peer_sched->channels[i], 0, sizeof(peer_sched->channels[i]));
168
169 peer_sched->n_channels = write_idx;
170
171 if (updated)
172 drv_nan_peer_sched_changed(local, sdata, sta);
173 }
174 }
175
176 static void
ieee80211_nan_remove_channel(struct ieee80211_sub_if_data * sdata,struct ieee80211_nan_channel * nan_channel)177 ieee80211_nan_remove_channel(struct ieee80211_sub_if_data *sdata,
178 struct ieee80211_nan_channel *nan_channel)
179 {
180 struct ieee80211_chanctx_conf *conf;
181 struct ieee80211_chanctx *ctx;
182 struct ieee80211_nan_sched_cfg *sched_cfg = &sdata->vif.cfg.nan_sched;
183
184 if (WARN_ON(!nan_channel))
185 return;
186
187 lockdep_assert_wiphy(sdata->local->hw.wiphy);
188
189 if (!nan_channel->chanreq.oper.chan)
190 return;
191
192 for (int slot = 0; slot < ARRAY_SIZE(sched_cfg->schedule); slot++)
193 if (sched_cfg->schedule[slot] == nan_channel)
194 sched_cfg->schedule[slot] = NULL;
195
196 conf = nan_channel->chanctx_conf;
197
198 /* If any peer nan schedule uses this chanctx, update them */
199 if (conf)
200 ieee80211_nan_update_peer_channels(sdata, conf);
201
202 memset(nan_channel, 0, sizeof(*nan_channel));
203
204 /* Update the driver before (possibly) releasing the channel context */
205 drv_vif_cfg_changed(sdata->local, sdata, BSS_CHANGED_NAN_LOCAL_SCHED);
206
207 /* Channel might not have a chanctx if it was ULWed */
208 if (!conf)
209 return;
210
211 ctx = container_of(conf, struct ieee80211_chanctx, conf);
212
213 if (ieee80211_chanctx_num_assigned(sdata->local, ctx) > 0) {
214 ieee80211_recalc_chanctx_chantype(sdata->local, ctx);
215 ieee80211_recalc_smps_chanctx(sdata->local, ctx);
216 ieee80211_recalc_chanctx_min_def(sdata->local, ctx);
217 }
218
219 if (ieee80211_chanctx_refcount(sdata->local, ctx) == 0)
220 ieee80211_free_chanctx(sdata->local, ctx, false);
221 }
222
223 static void
ieee80211_nan_update_all_ndi_carriers(struct ieee80211_local * local)224 ieee80211_nan_update_all_ndi_carriers(struct ieee80211_local *local)
225 {
226 struct ieee80211_sub_if_data *sdata;
227
228 lockdep_assert_wiphy(local->hw.wiphy);
229
230 /* Iterate all interfaces and update carrier for NDI interfaces */
231 list_for_each_entry(sdata, &local->interfaces, list) {
232 if (!ieee80211_sdata_running(sdata) ||
233 sdata->vif.type != NL80211_IFTYPE_NAN_DATA)
234 continue;
235
236 ieee80211_nan_update_ndi_carrier(sdata);
237 }
238 }
239
240 static struct ieee80211_nan_channel *
ieee80211_nan_find_free_channel(struct ieee80211_nan_sched_cfg * sched_cfg)241 ieee80211_nan_find_free_channel(struct ieee80211_nan_sched_cfg *sched_cfg)
242 {
243 for (int i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
244 if (!sched_cfg->channels[i].chanreq.oper.chan)
245 return &sched_cfg->channels[i];
246 }
247
248 return NULL;
249 }
250
ieee80211_nan_set_local_sched(struct ieee80211_sub_if_data * sdata,struct cfg80211_nan_local_sched * sched)251 int ieee80211_nan_set_local_sched(struct ieee80211_sub_if_data *sdata,
252 struct cfg80211_nan_local_sched *sched)
253 {
254 struct ieee80211_nan_channel *sched_idx_to_chan[IEEE80211_NAN_MAX_CHANNELS] = {};
255 struct ieee80211_nan_sched_cfg *sched_cfg = &sdata->vif.cfg.nan_sched;
256 struct ieee80211_nan_sched_cfg *backup_sched __free(kfree) = kmalloc_obj(*backup_sched);
257 int ret;
258
259 if (!backup_sched)
260 return -ENOMEM;
261
262 if (sched->n_channels > IEEE80211_NAN_MAX_CHANNELS)
263 return -EOPNOTSUPP;
264
265 if (sched->nan_avail_blob_len > IEEE80211_NAN_AVAIL_BLOB_MAX_LEN)
266 return -EINVAL;
267
268 /*
269 * If a deferred schedule update is pending completion, new updates are
270 * not allowed. Only allow to configure an empty schedule so NAN can be
271 * stopped in the middle of a deferred update. This is fine because
272 * empty schedule means the local NAN device will not be available for
273 * peers anymore so there is no need to update peers about a new
274 * schedule.
275 */
276 if (WARN_ON(sched_cfg->deferred && sched->n_channels))
277 return -EBUSY;
278
279 bitmap_zero(sdata->u.nan.removed_channels, IEEE80211_NAN_MAX_CHANNELS);
280
281 memcpy(backup_sched->schedule, sched_cfg->schedule,
282 sizeof(backup_sched->schedule));
283 memcpy(backup_sched->channels, sched_cfg->channels,
284 sizeof(backup_sched->channels));
285 memcpy(backup_sched->avail_blob, sched_cfg->avail_blob,
286 sizeof(backup_sched->avail_blob));
287 backup_sched->avail_blob_len = sched_cfg->avail_blob_len;
288
289 memcpy(sched_cfg->avail_blob, sched->nan_avail_blob,
290 sched->nan_avail_blob_len);
291 sched_cfg->avail_blob_len = sched->nan_avail_blob_len;
292
293 /*
294 * Remove channels that are no longer in the new schedule to free up
295 * resources before adding new channels. For deferred schedule, channels
296 * will be removed when the schedule is applied.
297 * Create a mapping from sched index to sched_cfg channel
298 */
299 for (int i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
300 bool still_needed = false;
301
302 if (!sched_cfg->channels[i].chanreq.oper.chan)
303 continue;
304
305 for (int j = 0; j < sched->n_channels; j++) {
306 if (cfg80211_chandef_identical(&sched_cfg->channels[i].chanreq.oper,
307 &sched->nan_channels[j].chandef)) {
308 sched_idx_to_chan[j] =
309 &sched_cfg->channels[i];
310 still_needed = true;
311 break;
312 }
313 }
314
315 if (!still_needed) {
316 __set_bit(i, sdata->u.nan.removed_channels);
317 if (!sched->deferred)
318 ieee80211_nan_remove_channel(sdata,
319 &sched_cfg->channels[i]);
320 }
321 }
322
323 for (int i = 0; i < sched->n_channels; i++) {
324 struct ieee80211_nan_channel *chan = sched_idx_to_chan[i];
325
326 if (chan) {
327 ieee80211_nan_update_channel(sdata->local, chan,
328 &sched->nan_channels[i],
329 sched->deferred);
330 } else {
331 chan = ieee80211_nan_find_free_channel(sched_cfg);
332 if (WARN_ON(!chan)) {
333 ret = -EINVAL;
334 goto err;
335 }
336
337 sched_idx_to_chan[i] = chan;
338 ieee80211_nan_init_channel(chan,
339 &sched->nan_channels[i]);
340 }
341
342 /* Also a pre-existing channel might have been ULWed, so no chanctx */
343 if (!chan->chanctx_conf) {
344 ret = ieee80211_nan_use_chanctx(sdata, chan, false);
345 if (ret) {
346 memset(chan, 0, sizeof(*chan));
347 goto err;
348 }
349 }
350 }
351
352 for (int s = 0; s < ARRAY_SIZE(sched_cfg->schedule); s++) {
353 if (sched->schedule[s] < ARRAY_SIZE(sched_idx_to_chan))
354 sched_cfg->schedule[s] =
355 sched_idx_to_chan[sched->schedule[s]];
356 else
357 sched_cfg->schedule[s] = NULL;
358 }
359
360 sched_cfg->deferred = sched->deferred;
361
362 drv_vif_cfg_changed(sdata->local, sdata, BSS_CHANGED_NAN_LOCAL_SCHED);
363
364 /*
365 * For deferred update, don't update NDI carriers yet as the new
366 * schedule is not yet applied so common slots don't change. The NDI
367 * carrier will be updated once the driver notifies the new schedule is
368 * applied.
369 */
370 if (sched_cfg->deferred)
371 return 0;
372
373 ieee80211_nan_update_all_ndi_carriers(sdata->local);
374 bitmap_zero(sdata->u.nan.removed_channels, IEEE80211_NAN_MAX_CHANNELS);
375
376 return 0;
377 err:
378 /* Remove newly added channels */
379 for (int i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
380 struct cfg80211_chan_def *chan_def =
381 &sched_cfg->channels[i].chanreq.oper;
382
383 if (!chan_def->chan)
384 continue;
385
386 if (!cfg80211_chandef_identical(&backup_sched->channels[i].chanreq.oper,
387 chan_def))
388 ieee80211_nan_remove_channel(sdata,
389 &sched_cfg->channels[i]);
390 }
391
392 /* Re-add all backed up channels */
393 for (int i = 0; i < ARRAY_SIZE(backup_sched->channels); i++) {
394 struct ieee80211_nan_channel *chan = &sched_cfg->channels[i];
395
396 *chan = backup_sched->channels[i];
397
398 /*
399 * For deferred update, no channels were removed and the channel
400 * context didn't change, so nothing else to do.
401 */
402 if (!chan->chanctx_conf || sched->deferred)
403 continue;
404
405 if (test_bit(i, sdata->u.nan.removed_channels)) {
406 /* Clear the stale chanctx pointer */
407 chan->chanctx_conf = NULL;
408 /*
409 * We removed the newly added channels so we don't lack
410 * resources. So the only reason that this would fail
411 * is a FW error which we ignore. Therefore, this
412 * should never fail.
413 */
414 WARN_ON(ieee80211_nan_use_chanctx(sdata, chan, true));
415 } else {
416 struct ieee80211_chanctx_conf *conf = chan->chanctx_conf;
417
418 /* FIXME: detect no-op? */
419 /* Channel was not removed but may have been updated */
420 ieee80211_recalc_smps_chanctx(sdata->local,
421 container_of(conf,
422 struct ieee80211_chanctx,
423 conf));
424 }
425 }
426
427 memcpy(sched_cfg->schedule, backup_sched->schedule,
428 sizeof(backup_sched->schedule));
429 memcpy(sched_cfg->avail_blob, backup_sched->avail_blob,
430 sizeof(backup_sched->avail_blob));
431 sched_cfg->avail_blob_len = backup_sched->avail_blob_len;
432 sched_cfg->deferred = false;
433 bitmap_zero(sdata->u.nan.removed_channels, IEEE80211_NAN_MAX_CHANNELS);
434
435 drv_vif_cfg_changed(sdata->local, sdata, BSS_CHANGED_NAN_LOCAL_SCHED);
436 ieee80211_nan_update_all_ndi_carriers(sdata->local);
437 return ret;
438 }
439
ieee80211_nan_sched_update_done(struct ieee80211_vif * vif)440 void ieee80211_nan_sched_update_done(struct ieee80211_vif *vif)
441 {
442 struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
443 struct ieee80211_nan_sched_cfg *sched_cfg = &vif->cfg.nan_sched;
444 unsigned int i;
445
446 lockdep_assert_wiphy(sdata->local->hw.wiphy);
447
448 if (WARN_ON(!sched_cfg->deferred))
449 return;
450
451 ieee80211_nan_update_all_ndi_carriers(sdata->local);
452
453 /*
454 * Clear the deferred flag before removing channels. Removing channels
455 * will trigger another schedule update to the driver, and there is no
456 * need for this update to be deferred since removed channels are not
457 * part of the schedule anymore, so no need to notify peers about
458 * removing them.
459 */
460 sched_cfg->deferred = false;
461
462 for (i = 0; i < ARRAY_SIZE(sched_cfg->channels); i++) {
463 struct ieee80211_nan_channel *chan = &sched_cfg->channels[i];
464 struct ieee80211_chanctx_conf *conf = chan->chanctx_conf;
465
466 if (!chan->chanreq.oper.chan)
467 continue;
468
469 if (test_bit(i, sdata->u.nan.removed_channels))
470 ieee80211_nan_remove_channel(sdata, chan);
471 else if (conf)
472 /*
473 * We might have called this already for some channels,
474 * but this knows to handle a no-op.
475 */
476 ieee80211_recalc_smps_chanctx(sdata->local,
477 container_of(conf,
478 struct ieee80211_chanctx,
479 conf));
480 }
481
482 bitmap_zero(sdata->u.nan.removed_channels, IEEE80211_NAN_MAX_CHANNELS);
483 cfg80211_nan_sched_update_done(ieee80211_vif_to_wdev(vif), true,
484 GFP_KERNEL);
485 }
486 EXPORT_SYMBOL(ieee80211_nan_sched_update_done);
487
ieee80211_nan_free_peer_sched(struct ieee80211_nan_peer_sched * sched)488 void ieee80211_nan_free_peer_sched(struct ieee80211_nan_peer_sched *sched)
489 {
490 if (!sched)
491 return;
492
493 kfree(sched->init_ulw);
494 kfree(sched);
495 }
496
497 static int
ieee80211_nan_init_peer_channel(struct ieee80211_sub_if_data * sdata,const struct sta_info * sta,const struct cfg80211_nan_channel * cfg_chan,struct ieee80211_nan_channel * new_chan)498 ieee80211_nan_init_peer_channel(struct ieee80211_sub_if_data *sdata,
499 const struct sta_info *sta,
500 const struct cfg80211_nan_channel *cfg_chan,
501 struct ieee80211_nan_channel *new_chan)
502 {
503 struct ieee80211_nan_sched_cfg *sched_cfg = &sdata->vif.cfg.nan_sched;
504
505 /* Find compatible local channel */
506 for (int j = 0; j < ARRAY_SIZE(sched_cfg->channels); j++) {
507 struct ieee80211_nan_channel *local_chan =
508 &sched_cfg->channels[j];
509 const struct cfg80211_chan_def *compat;
510
511 if (!local_chan->chanreq.oper.chan)
512 continue;
513
514 compat = cfg80211_chandef_compatible(&local_chan->chanreq.oper,
515 &cfg_chan->chandef);
516 if (!compat)
517 continue;
518
519 /* compat is the wider chandef, and we want the narrower one */
520 new_chan->chanreq.oper = compat == &local_chan->chanreq.oper ?
521 cfg_chan->chandef : local_chan->chanreq.oper;
522 new_chan->needed_rx_chains = min(local_chan->needed_rx_chains,
523 cfg_chan->rx_nss);
524 new_chan->chanctx_conf = local_chan->chanctx_conf;
525
526 break;
527 }
528
529 /*
530 * nl80211 already validated that each peer channel is compatible
531 * with at least one local channel, so this should never happen.
532 */
533 if (WARN_ON(!new_chan->chanreq.oper.chan))
534 return -EINVAL;
535
536 memcpy(new_chan->channel_entry, cfg_chan->channel_entry,
537 sizeof(new_chan->channel_entry));
538
539 return 0;
540 }
541
542 static void
ieee80211_nan_init_peer_map(struct ieee80211_nan_peer_sched * peer_sched,const struct cfg80211_nan_peer_map * cfg_map,struct ieee80211_nan_peer_map * new_map)543 ieee80211_nan_init_peer_map(struct ieee80211_nan_peer_sched *peer_sched,
544 const struct cfg80211_nan_peer_map *cfg_map,
545 struct ieee80211_nan_peer_map *new_map)
546 {
547 new_map->map_id = cfg_map->map_id;
548
549 if (new_map->map_id == CFG80211_NAN_INVALID_MAP_ID)
550 return;
551
552 /* Set up the slots array */
553 for (int slot = 0; slot < ARRAY_SIZE(new_map->slots); slot++) {
554 u8 chan_idx = cfg_map->schedule[slot];
555
556 if (chan_idx < peer_sched->n_channels)
557 new_map->slots[slot] = &peer_sched->channels[chan_idx];
558 }
559 }
560
561 /*
562 * Check if the local schedule and a peer schedule have at least one common
563 * slot - a slot where both schedules are active on compatible channels.
564 */
565 static bool
ieee80211_nan_has_common_slots(struct ieee80211_sub_if_data * sdata,struct ieee80211_nan_peer_sched * peer_sched)566 ieee80211_nan_has_common_slots(struct ieee80211_sub_if_data *sdata,
567 struct ieee80211_nan_peer_sched *peer_sched)
568 {
569 for (int slot = 0; slot < CFG80211_NAN_SCHED_NUM_TIME_SLOTS; slot++) {
570 struct ieee80211_nan_channel *local_chan =
571 sdata->vif.cfg.nan_sched.schedule[slot];
572
573 if (!local_chan || !local_chan->chanctx_conf)
574 continue;
575
576 /* Check all peer maps for this slot */
577 for (int m = 0; m < CFG80211_NAN_MAX_PEER_MAPS; m++) {
578 struct ieee80211_nan_peer_map *map = &peer_sched->maps[m];
579 struct ieee80211_nan_channel *peer_chan;
580
581 if (map->map_id == CFG80211_NAN_INVALID_MAP_ID)
582 continue;
583
584 peer_chan = map->slots[slot];
585 if (!peer_chan)
586 continue;
587
588 if (local_chan->chanctx_conf == peer_chan->chanctx_conf)
589 return true;
590 }
591 }
592
593 return false;
594 }
595
ieee80211_nan_update_ndi_carrier(struct ieee80211_sub_if_data * ndi_sdata)596 void ieee80211_nan_update_ndi_carrier(struct ieee80211_sub_if_data *ndi_sdata)
597 {
598 struct ieee80211_local *local = ndi_sdata->local;
599 struct ieee80211_sub_if_data *nmi_sdata;
600 struct sta_info *sta;
601
602 lockdep_assert_wiphy(local->hw.wiphy);
603
604 if (WARN_ON(ndi_sdata->vif.type != NL80211_IFTYPE_NAN_DATA ||
605 !ndi_sdata->dev) || !ieee80211_sdata_running(ndi_sdata))
606 return;
607
608 nmi_sdata = wiphy_dereference(local->hw.wiphy, ndi_sdata->u.nan_data.nmi);
609 if (WARN_ON(!nmi_sdata))
610 return;
611
612 list_for_each_entry(sta, &local->sta_list, list) {
613 struct ieee80211_sta *nmi_sta;
614
615 if (sta->sdata != ndi_sdata ||
616 !test_sta_flag(sta, WLAN_STA_AUTHORIZED))
617 continue;
618
619 nmi_sta = wiphy_dereference(local->hw.wiphy, sta->sta.nmi);
620 if (WARN_ON(!nmi_sta) || !nmi_sta->nan_sched)
621 continue;
622
623 if (ieee80211_nan_has_common_slots(nmi_sdata, nmi_sta->nan_sched)) {
624 netif_carrier_on(ndi_sdata->dev);
625 return;
626 }
627 }
628
629 netif_carrier_off(ndi_sdata->dev);
630 }
631
632 static void
ieee80211_nan_update_peer_ndis_carrier(struct ieee80211_local * local,struct sta_info * nmi_sta)633 ieee80211_nan_update_peer_ndis_carrier(struct ieee80211_local *local,
634 struct sta_info *nmi_sta)
635 {
636 struct sta_info *sta;
637
638 lockdep_assert_wiphy(local->hw.wiphy);
639
640 list_for_each_entry(sta, &local->sta_list, list) {
641 if (rcu_access_pointer(sta->sta.nmi) == &nmi_sta->sta)
642 ieee80211_nan_update_ndi_carrier(sta->sdata);
643 }
644 }
645
ieee80211_nan_set_peer_sched(struct ieee80211_sub_if_data * sdata,struct cfg80211_nan_peer_sched * sched)646 int ieee80211_nan_set_peer_sched(struct ieee80211_sub_if_data *sdata,
647 struct cfg80211_nan_peer_sched *sched)
648 {
649 struct ieee80211_nan_peer_sched *new_sched, *old_sched, *to_free;
650 struct sta_info *sta;
651 int ret;
652
653 lockdep_assert_wiphy(sdata->local->hw.wiphy);
654
655 if (!sdata->u.nan.started)
656 return -EINVAL;
657
658 sta = sta_info_get(sdata, sched->peer_addr);
659 if (!sta)
660 return -ENOENT;
661
662 new_sched = kzalloc(struct_size(new_sched, channels, sched->n_channels),
663 GFP_KERNEL);
664 if (!new_sched)
665 return -ENOMEM;
666
667 to_free = new_sched;
668
669 new_sched->seq_id = sched->seq_id;
670 new_sched->committed_dw = sched->committed_dw;
671 new_sched->max_chan_switch = sched->max_chan_switch;
672 new_sched->n_channels = sched->n_channels;
673
674 if (sched->ulw_size && sched->init_ulw) {
675 new_sched->init_ulw = kmemdup(sched->init_ulw, sched->ulw_size,
676 GFP_KERNEL);
677 if (!new_sched->init_ulw) {
678 ret = -ENOMEM;
679 goto out;
680 }
681 new_sched->ulw_size = sched->ulw_size;
682 }
683
684 for (int i = 0; i < sched->n_channels; i++) {
685 ret = ieee80211_nan_init_peer_channel(sdata, sta,
686 &sched->nan_channels[i],
687 &new_sched->channels[i]);
688 if (ret)
689 goto out;
690 }
691
692 for (int m = 0; m < ARRAY_SIZE(sched->maps); m++)
693 ieee80211_nan_init_peer_map(new_sched, &sched->maps[m],
694 &new_sched->maps[m]);
695
696 /* Install the new schedule before calling the driver */
697 old_sched = sta->sta.nan_sched;
698 sta->sta.nan_sched = new_sched;
699
700 ret = drv_nan_peer_sched_changed(sdata->local, sdata, sta);
701 if (ret) {
702 /* Revert to old schedule */
703 sta->sta.nan_sched = old_sched;
704 goto out;
705 }
706
707 ieee80211_nan_update_peer_ndis_carrier(sdata->local, sta);
708
709 /* Success - free old schedule */
710 to_free = old_sched;
711 ret = 0;
712
713 out:
714 ieee80211_nan_free_peer_sched(to_free);
715 return ret;
716 }
717
718 void
ieee80211_nan_evacuate_channel(struct ieee80211_sub_if_data * sdata,struct ieee80211_nan_channel * nan_channel)719 ieee80211_nan_evacuate_channel(struct ieee80211_sub_if_data *sdata,
720 struct ieee80211_nan_channel *nan_channel)
721 {
722 struct ieee80211_chanctx_conf *conf;
723 struct ieee80211_chanctx *ctx;
724
725 lockdep_assert_wiphy(sdata->local->hw.wiphy);
726
727 if (WARN_ON(!nan_channel || !nan_channel->chanreq.oper.chan))
728 return;
729
730 conf = nan_channel->chanctx_conf;
731 if (WARN_ON(!conf))
732 return;
733
734 nan_channel->chanctx_conf = NULL;
735
736 /* Update all peer channels that reference this chanctx */
737 ieee80211_nan_update_peer_channels(sdata, conf);
738
739 drv_vif_cfg_changed(sdata->local, sdata, BSS_CHANGED_NAN_LOCAL_SCHED);
740
741 cfg80211_nan_channel_evac(&sdata->wdev, &nan_channel->chanreq.oper,
742 GFP_KERNEL);
743
744 /* Update NDI carrier states */
745 ieee80211_nan_update_all_ndi_carriers(sdata->local);
746
747 /* Clean up the channel context if no longer used */
748 ctx = container_of(conf, struct ieee80211_chanctx, conf);
749
750 if (ieee80211_chanctx_num_assigned(sdata->local, ctx) > 0) {
751 ieee80211_recalc_chanctx_chantype(sdata->local, ctx);
752 ieee80211_recalc_smps_chanctx(sdata->local, ctx);
753 ieee80211_recalc_chanctx_min_def(sdata->local, ctx);
754 }
755
756 if (ieee80211_chanctx_refcount(sdata->local, ctx) == 0)
757 ieee80211_free_chanctx(sdata->local, ctx, false);
758 }
759
760 struct ieee80211_nan_channel *
ieee80211_nan_find_evac_chan(struct ieee80211_local * local,struct ieee80211_sub_if_data * sdata,struct ieee80211_chanctx * ctx)761 ieee80211_nan_find_evac_chan(struct ieee80211_local *local,
762 struct ieee80211_sub_if_data *sdata,
763 struct ieee80211_chanctx *ctx)
764 {
765 struct ieee80211_nan_sched_cfg *sched_cfg;
766 struct ieee80211_nan_channel *evac_chan = NULL;
767 int min_slot_count = INT_MAX;
768 int usable_channels = 0;
769
770 lockdep_assert_wiphy(local->hw.wiphy);
771
772 if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_NAN))
773 return NULL;
774
775 sched_cfg = &sdata->vif.cfg.nan_sched;
776
777 /* Find the channel to evacuate and count usable channels */
778 for (int i = 0; i < IEEE80211_NAN_MAX_CHANNELS; i++) {
779 struct ieee80211_nan_channel *chan =
780 &sched_cfg->channels[i];
781 struct ieee80211_chanctx *chan_ctx;
782 int slot_count = 0;
783
784 if (!chan->chanreq.oper.chan || !chan->chanctx_conf)
785 continue;
786
787 usable_channels++;
788
789 chan_ctx = container_of(chan->chanctx_conf,
790 struct ieee80211_chanctx, conf);
791
792 /* If ctx specified, only consider that specific chanctx */
793 if (ctx) {
794 if (chan_ctx == ctx)
795 evac_chan = chan;
796 continue;
797 }
798
799 /* Can only evacuate channels whose chanctx is NAN-only */
800 if (ieee80211_chanctx_refcount(local, chan_ctx) > 1)
801 continue;
802
803 /* Count how many time slots use this channel */
804 for (int s = 0; s < CFG80211_NAN_SCHED_NUM_TIME_SLOTS; s++)
805 if (sched_cfg->schedule[s] == chan)
806 slot_count++;
807
808 if (slot_count < min_slot_count) {
809 min_slot_count = slot_count;
810 evac_chan = chan;
811 }
812 }
813
814 /* No suitable NAN channel found */
815 if (!evac_chan)
816 return NULL;
817
818 /* NAN needs at least one remaining usable channel after evacuation */
819 if (usable_channels < 2)
820 return NULL;
821
822 return evac_chan;
823 }
824
ieee80211_nan_try_evacuate(struct ieee80211_hw * hw,struct ieee80211_chanctx_conf * conf)825 bool ieee80211_nan_try_evacuate(struct ieee80211_hw *hw,
826 struct ieee80211_chanctx_conf *conf)
827 {
828 struct ieee80211_local *local = hw_to_local(hw);
829 struct ieee80211_sub_if_data *sdata =
830 ieee80211_find_nan_sdata(local);
831 struct ieee80211_nan_channel *evac_chan;
832 struct ieee80211_chanctx *ctx = NULL;
833
834 lockdep_assert_wiphy(local->hw.wiphy);
835
836 if (!sdata)
837 return false;
838
839 if (conf)
840 ctx = container_of(conf, struct ieee80211_chanctx, conf);
841
842 evac_chan = ieee80211_nan_find_evac_chan(local, sdata, ctx);
843 if (!evac_chan)
844 return false;
845
846 ieee80211_nan_evacuate_channel(sdata, evac_chan);
847
848 return true;
849 }
850 EXPORT_SYMBOL(ieee80211_nan_try_evacuate);
851