1 // SPDX-License-Identifier: BSD-3-Clause-Clear
2 /* Copyright (C) 2025-2026 MediaTek Inc. */
3
4 #include <asm/byteorder.h>
5 #include <linux/bitfield.h>
6 #include <linux/errno.h>
7 #include <linux/kernel.h>
8 #include <linux/stddef.h>
9 #include <linux/string.h>
10 #include <linux/types.h>
11 #include <linux/ieee80211.h>
12 #include <net/cfg80211.h>
13 #include <net/mac80211.h>
14
15 #include "mt7925.h"
16 #include "mcu.h"
17 #include "nan.h"
18 #include "regd.h"
19
mt7925_nan_set_5g_channel(struct mt792x_dev * dev,struct mt7925_nan_enable_req_tlv * req,struct cfg80211_nan_conf * conf)20 static void mt7925_nan_set_5g_channel(struct mt792x_dev *dev,
21 struct mt7925_nan_enable_req_tlv *req,
22 struct cfg80211_nan_conf *conf)
23 {
24 struct ieee80211_channel *chan;
25 u32 ch5g = 0;
26
27 chan = conf->band_cfgs[NL80211_BAND_5GHZ].chan;
28
29 if (!chan)
30 return;
31
32 if (!mt7925_regd_is_valid_channel(dev, NL80211_BAND_5GHZ, chan))
33 return;
34
35 req->config_support_5g = 1;
36 req->support_5g_val = 1;
37 req->config_5g_channel = 1;
38
39 if (chan->hw_value == NAN_5G_LOW_DISC_CHANNEL)
40 ch5g |= BIT(0);
41 else if (chan->hw_value == NAN_5G_HIGH_DISC_CHANNEL)
42 ch5g |= BIT(1);
43
44 req->channel_5g_val = cpu_to_le32(ch5g);
45 }
46
mt7925_nan_set_2g_support(struct mt7925_nan_enable_req_tlv * req,struct cfg80211_nan_conf * conf)47 static void mt7925_nan_set_2g_support(struct mt7925_nan_enable_req_tlv *req,
48 struct cfg80211_nan_conf *conf)
49 {
50 if (!conf->band_cfgs[NL80211_BAND_2GHZ].chan)
51 return;
52
53 req->config_2dot4g_support = 1;
54 req->support_2dot4g_val = 1;
55 }
56
mt7925_nan_set_cluster_id(struct mt7925_nan_enable_req_tlv * req,const u8 * cluster_id)57 static void mt7925_nan_set_cluster_id(struct mt7925_nan_enable_req_tlv *req,
58 const u8 *cluster_id)
59 {
60 if (!cluster_id)
61 return;
62
63 req->cluster_high = cpu_to_le16(cluster_id[4] | cluster_id[5] << 8);
64 req->cluster_low = cpu_to_le16((u16)cluster_id[3]);
65 }
66
mt7925_nan_set_dw_interval(struct mt7925_nan_enable_req_tlv * req,struct cfg80211_nan_conf * conf)67 static void mt7925_nan_set_dw_interval(struct mt7925_nan_enable_req_tlv *req,
68 struct cfg80211_nan_conf *conf)
69 {
70 if (conf->band_cfgs[NL80211_BAND_2GHZ].awake_dw_interval > 0) {
71 req->config_dw.config_2dot4g_dw_band = 1;
72 req->config_dw.dw_2dot4g_interval_val =
73 cpu_to_le32(conf->band_cfgs[NL80211_BAND_2GHZ].awake_dw_interval);
74 }
75
76 if (conf->band_cfgs[NL80211_BAND_5GHZ].awake_dw_interval > 0) {
77 req->config_dw.config_5g_dw_band = 1;
78 req->config_dw.dw_5g_interval_val =
79 cpu_to_le32(conf->band_cfgs[NL80211_BAND_5GHZ].awake_dw_interval);
80 }
81 }
82
mt7925_nan_set_disc_beacon(struct mt7925_nan_enable_req_tlv * req,struct cfg80211_nan_conf * conf)83 static void mt7925_nan_set_disc_beacon(struct mt7925_nan_enable_req_tlv *req,
84 struct cfg80211_nan_conf *conf)
85 {
86 if (conf->discovery_beacon_interval > 0) {
87 req->config_2dot4g_beacons = true;
88 req->beacon_2dot4g_val = conf->discovery_beacon_interval;
89 }
90 }
91
mt7925_nan_set_rssi_thresholds(struct mt7925_nan_enable_req_tlv * req,struct cfg80211_nan_conf * conf)92 static void mt7925_nan_set_rssi_thresholds(struct mt7925_nan_enable_req_tlv *req,
93 struct cfg80211_nan_conf *conf)
94 {
95 if (conf->band_cfgs[NL80211_BAND_2GHZ].chan) {
96 req->config_2dot4g_rssi_close = 1;
97 req->rssi_close_2dot4g_val =
98 abs(conf->band_cfgs[NL80211_BAND_2GHZ].rssi_close);
99 req->config_2dot4g_rssi_middle = 1;
100 req->rssi_middle_2dot4g_val =
101 abs(conf->band_cfgs[NL80211_BAND_2GHZ].rssi_middle);
102 }
103
104 if (conf->band_cfgs[NL80211_BAND_5GHZ].chan) {
105 req->config_5g_rssi_close = 1;
106 req->rssi_close_5g_val =
107 abs(conf->band_cfgs[NL80211_BAND_5GHZ].rssi_close);
108 req->config_5g_rssi_middle = 1;
109 req->rssi_middle_5g_val =
110 abs(conf->band_cfgs[NL80211_BAND_5GHZ].rssi_middle);
111 }
112 }
113
mt7925_nan_set_scan_params(struct mt7925_nan_enable_req_tlv * req,struct cfg80211_nan_conf * conf)114 static void mt7925_nan_set_scan_params(struct mt7925_nan_enable_req_tlv *req,
115 struct cfg80211_nan_conf *conf)
116 {
117 req->scan_params_val.scan_period[0] =
118 cpu_to_le16(conf->scan_period < 255 ? conf->scan_period : 255);
119 req->scan_params_val.dwell_time[0] =
120 conf->scan_dwell_time < 255 ? conf->scan_dwell_time : 255;
121 }
122
123 static u16
mt7925_nan_avail_attr_ctrl(const struct ieee80211_nan_sched_cfg * sched)124 mt7925_nan_avail_attr_ctrl(const struct ieee80211_nan_sched_cfg *sched)
125 {
126 if (sched->avail_blob_len < NAN_AVAIL_ATTR_CTRL_OFFSET + 2)
127 return 0;
128
129 return sched->avail_blob[NAN_AVAIL_ATTR_CTRL_OFFSET] |
130 sched->avail_blob[NAN_AVAIL_ATTR_CTRL_OFFSET + 1] << 8;
131 }
132
133 static void
mt7925_nan_update_conf(struct mt792x_vif * mvif,const struct cfg80211_nan_conf * conf)134 mt7925_nan_update_conf(struct mt792x_vif *mvif,
135 const struct cfg80211_nan_conf *conf)
136 {
137 mvif->nan.conf.master_pref = conf->master_pref;
138 mvif->nan.conf.bands = conf->bands;
139 mvif->nan.conf.discovery_beacon_interval =
140 conf->discovery_beacon_interval;
141 mvif->nan.conf.enable_dw_notification =
142 conf->enable_dw_notification;
143
144 memcpy(mvif->nan.conf.cluster_id, conf->cluster_id, ETH_ALEN);
145 }
146
mt7925_nan_set_nmi_addr(struct mt792x_dev * dev,const u8 * addr)147 int mt7925_nan_set_nmi_addr(struct mt792x_dev *dev, const u8 *addr)
148 {
149 struct mt76_dev *mdev;
150 struct {
151 u8 rsv[4];
152 struct mt7925_nan_nmi_addr_tlv nmi_addr_tlv;
153 } nmi_cmd = {
154 .rsv = { 0 },
155 .nmi_addr_tlv = {
156 .tag = cpu_to_le16(NAN_UNI_CMD_CHANGE_NMI_ADDRESS),
157 .len = cpu_to_le16(sizeof(struct mt7925_nan_nmi_addr_tlv)),
158 },
159 };
160 int ret;
161
162 if (!dev || !addr)
163 return -EINVAL;
164
165 if (is_zero_ether_addr(addr) || is_multicast_ether_addr(addr)) {
166 dev_err(dev->mt76.dev, "NAN: invalid NMI address %pM\n", addr);
167 return -EINVAL;
168 }
169
170 mdev = &dev->mt76;
171 memcpy(nmi_cmd.nmi_addr_tlv.nmi_addr, addr, ETH_ALEN);
172
173 ret = mt76_mcu_send_msg(mdev, MCU_UNI_CMD(NAN), &nmi_cmd,
174 sizeof(nmi_cmd), true);
175
176 return ret;
177 }
178
mt7925_nan_enable(struct ieee80211_vif * vif,struct mt792x_dev * dev,struct cfg80211_nan_conf * conf)179 int mt7925_nan_enable(struct ieee80211_vif *vif,
180 struct mt792x_dev *dev,
181 struct cfg80211_nan_conf *conf)
182 {
183 struct mt792x_vif *mvif = (struct mt792x_vif *)vif->drv_priv;
184 struct mt76_dev *mdev = &dev->mt76;
185 struct {
186 u8 rsv[4];
187 struct mt7925_nan_enable_req_tlv nan_req_tlv;
188 } nan_cmd = {
189 .rsv = { 0 },
190 .nan_req_tlv = {
191 .tag = cpu_to_le16(NAN_UNI_CMD_ENABLE_REQUEST),
192 .len = cpu_to_le16(sizeof(struct mt7925_nan_enable_req_tlv)),
193 .config_random_factor_force = 0,
194 .random_factor_force_val = 0,
195 .config_hop_count_force = 0,
196 .hop_count_force_val = 0,
197 },
198 };
199 struct mt7925_nan_enable_req_tlv *p_nan_req_tlv = &nan_cmd.nan_req_tlv;
200 int ret;
201
202 if (!vif || !dev || !conf)
203 return -EINVAL;
204
205 p_nan_req_tlv->master_pref = conf->master_pref;
206
207 mt7925_nan_set_2g_support(p_nan_req_tlv, conf);
208 mt7925_nan_set_5g_channel(dev, p_nan_req_tlv, conf);
209 mt7925_nan_set_cluster_id(p_nan_req_tlv, conf->cluster_id);
210 mt7925_nan_set_dw_interval(p_nan_req_tlv, conf);
211 mt7925_nan_set_disc_beacon(p_nan_req_tlv, conf);
212 mt7925_nan_set_rssi_thresholds(p_nan_req_tlv, conf);
213 mt7925_nan_set_scan_params(p_nan_req_tlv, conf);
214
215 mt7925_nan_update_conf(mvif, conf);
216
217 ret = mt76_mcu_send_msg(mdev, MCU_UNI_CMD(NAN), &nan_cmd, sizeof(nan_cmd), true);
218
219 return ret;
220 }
221
mt7925_nan_disable(struct ieee80211_vif * vif,struct mt792x_dev * dev)222 int mt7925_nan_disable(struct ieee80211_vif *vif, struct mt792x_dev *dev)
223 {
224 struct mt76_dev *mdev = &dev->mt76;
225 struct {
226 u8 rsv[4];
227 struct tlv nan_dis_tlv;
228 } nan_cmd = {
229 .rsv = { 0 },
230 .nan_dis_tlv = {
231 .tag = cpu_to_le16(NAN_UNI_CMD_DISABLE_REQUEST),
232 .len = cpu_to_le16(sizeof(struct tlv)),
233 },
234 };
235
236 if (!dev)
237 return -EINVAL;
238
239 return mt76_mcu_send_msg(mdev, MCU_UNI_CMD(NAN), &nan_cmd, sizeof(nan_cmd), true);
240 }
241
242 static int
mt7925_nan_mp_tlv(struct sk_buff * skb,u8 master_pref)243 mt7925_nan_mp_tlv(struct sk_buff *skb, u8 master_pref)
244 {
245 struct mt7925_nan_master_preference_tlv *mp_tlv = NULL;
246 struct tlv *tlv = NULL;
247
248 if (!skb)
249 return -EINVAL;
250
251 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_SET_MASTER_PREFERENCE,
252 sizeof(struct mt7925_nan_master_preference_tlv));
253 if (!tlv)
254 return -ENOMEM;
255
256 mp_tlv = (struct mt7925_nan_master_preference_tlv *)tlv;
257
258 if (master_pref > NAN_MAX_MASTER_PREFERENCE)
259 return 0;
260
261 mp_tlv->master_preference = master_pref;
262
263 return 0;
264 }
265
266 static int
mt7925_nan_dw_tlv(struct sk_buff * skb,struct cfg80211_nan_conf * conf)267 mt7925_nan_dw_tlv(struct sk_buff *skb, struct cfg80211_nan_conf *conf)
268 {
269 struct mt7925_nan_dw_interval_tlv *dw_tlv = NULL;
270 struct tlv *tlv = NULL;
271 u16 interval;
272
273 if (!skb || !conf)
274 return -EINVAL;
275
276 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_SET_DW_INTERVAL,
277 sizeof(struct mt7925_nan_dw_interval_tlv));
278
279 if (!tlv)
280 return -ENOMEM;
281
282 dw_tlv = (struct mt7925_nan_dw_interval_tlv *)tlv;
283
284 /* Set DW interval for 2.4GHz and 5GHz bands if available */
285 if (conf->band_cfgs[NL80211_BAND_2GHZ].awake_dw_interval > 0) {
286 dw_tlv->dw_interval = conf->band_cfgs[NL80211_BAND_2GHZ].awake_dw_interval;
287 } else if (conf->band_cfgs[NL80211_BAND_5GHZ].awake_dw_interval > 0) {
288 dw_tlv->dw_interval = conf->band_cfgs[NL80211_BAND_5GHZ].awake_dw_interval;
289 } else {
290 /* Fallback to a default value or log a warning */
291 dw_tlv->dw_interval = NAN_DEFAULT_DW_INTERVAL;
292 }
293
294 /* Validate and set NAN Discovery Beacon Interval */
295 interval = conf->discovery_beacon_interval > 0 ?
296 conf->discovery_beacon_interval :
297 NAN_DEFAULT_DISC_BCN_INTERVAL;
298
299 dw_tlv->disc_bcn_interval = cpu_to_le16(interval);
300
301 return 0;
302 }
303
304 static int
mt7925_nan_cluster_id_tlv(struct sk_buff * skb,const u8 * cluster_id)305 mt7925_nan_cluster_id_tlv(struct sk_buff *skb, const u8 *cluster_id)
306 {
307 struct mt7925_nan_cluster_id_tlv *cluster_tlv = NULL;
308 struct tlv *tlv = NULL;
309
310 if (!skb || !cluster_id)
311 return -EINVAL;
312
313 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_SET_CLUSTER_ID,
314 sizeof(struct mt7925_nan_cluster_id_tlv));
315
316 if (!tlv)
317 return -ENOMEM;
318
319 cluster_tlv = (struct mt7925_nan_cluster_id_tlv *)tlv;
320
321 memcpy(cluster_tlv->cluster_id, cluster_id, ETH_ALEN);
322
323 return 0;
324 }
325
326 static int
mt7925_nan_sync_rssi_tlv(struct sk_buff * skb,struct cfg80211_nan_conf * conf)327 mt7925_nan_sync_rssi_tlv(struct sk_buff *skb, struct cfg80211_nan_conf *conf)
328 {
329 struct mt7925_nan_sync_rssi_tlv *rssi_tlv = NULL;
330 struct tlv *tlv = NULL;
331
332 if (!skb || !conf)
333 return -EINVAL;
334
335 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_SET_SYNC_RSSI,
336 sizeof(struct mt7925_nan_sync_rssi_tlv));
337
338 if (!tlv)
339 return -ENOMEM;
340
341 rssi_tlv = (struct mt7925_nan_sync_rssi_tlv *)tlv;
342
343 if (conf->band_cfgs[NL80211_BAND_2GHZ].chan) {
344 rssi_tlv->rssi_close_2g =
345 conf->band_cfgs[NL80211_BAND_2GHZ].rssi_close;
346 rssi_tlv->rssi_middle_2g =
347 conf->band_cfgs[NL80211_BAND_2GHZ].rssi_middle;
348 }
349
350 if (conf->band_cfgs[NL80211_BAND_5GHZ].chan) {
351 rssi_tlv->rssi_close_5g =
352 conf->band_cfgs[NL80211_BAND_5GHZ].rssi_close;
353 rssi_tlv->rssi_middle_5g =
354 conf->band_cfgs[NL80211_BAND_5GHZ].rssi_middle;
355 }
356
357 return 0;
358 }
359
mt7925_nan_change_configure(struct ieee80211_vif * vif,struct mt792x_dev * dev,struct cfg80211_nan_conf * conf)360 int mt7925_nan_change_configure(struct ieee80211_vif *vif,
361 struct mt792x_dev *dev,
362 struct cfg80211_nan_conf *conf)
363 {
364 struct mt792x_vif *mvif = (struct mt792x_vif *)vif->drv_priv;
365 struct mt7925_nan_common_hdr *hdr = NULL;
366 struct mt76_dev *mdev = &dev->mt76;
367 struct sk_buff *skb = NULL;
368
369 if (!vif || !dev || !conf)
370 return -EINVAL;
371
372 skb = mt76_mcu_msg_alloc(mdev, NULL, MT7925_NAN_CONF_MAX_SIZE);
373 if (!skb)
374 return -ENOMEM;
375
376 hdr = (struct mt7925_nan_common_hdr *)skb_put(skb, sizeof(*hdr));
377 memset(hdr, 0, sizeof(*hdr));
378
379 if (mt7925_nan_mp_tlv(skb, conf->master_pref) ||
380 mt7925_nan_dw_tlv(skb, conf) ||
381 mt7925_nan_cluster_id_tlv(skb, conf->cluster_id) ||
382 mt7925_nan_sync_rssi_tlv(skb, conf)) {
383 dev_kfree_skb(skb);
384 return -ENOMEM;
385 }
386
387 mt7925_nan_update_conf(mvif, conf);
388
389 return mt76_mcu_skb_send_msg(mdev, skb,
390 MCU_UNI_CMD(NAN), true);
391 }
392
393 static void
mt7925_nan_handle_dw_ind(struct mt792x_dev * dev,struct tlv * tlv)394 mt7925_nan_handle_dw_ind(struct mt792x_dev *dev, struct tlv *tlv)
395 {
396 struct ieee80211_channel *chan;
397 struct nan_rpt_dw_evt *evt;
398 struct wireless_dev *wdev;
399 u16 len, channel, dw_num;
400 struct mt792x_vif *mvif;
401 enum nl80211_band band;
402 int freq;
403
404 if (!dev || !tlv)
405 return;
406
407 len = le16_to_cpu(tlv->len);
408 if (len < sizeof(*tlv) + sizeof(*evt)) {
409 dev_warn(dev->mt76.dev,
410 "nan: short dw event tlv len=%u\n", len);
411 return;
412 }
413
414 if (!dev->nan_vif || !ieee80211_vif_nan_started(dev->nan_vif))
415 return;
416
417 wdev = ieee80211_vif_to_wdev(dev->nan_vif);
418 if (!wdev)
419 return;
420
421 mvif = (struct mt792x_vif *)dev->nan_vif->drv_priv;
422 if (!mvif->nan.conf.enable_dw_notification)
423 return;
424
425 evt = (struct nan_rpt_dw_evt *)tlv->data;
426 channel = le16_to_cpu(evt->channel);
427 dw_num = le16_to_cpu(evt->dw_num);
428
429 band = channel > 13 ? NL80211_BAND_5GHZ : NL80211_BAND_2GHZ;
430 freq = ieee80211_channel_to_frequency(channel, band);
431 chan = ieee80211_get_channel(dev->mt76.hw->wiphy, freq);
432 if (!chan) {
433 dev_dbg(dev->mt76.dev,
434 "nan: no channel for dw end event ch=%u dw=%u\n",
435 channel, dw_num);
436 return;
437 }
438
439 cfg80211_next_nan_dw_notif(wdev, chan, GFP_KERNEL);
440 }
441
442 static void
mt7925_nan_mcu_handle_de_event(struct mt792x_dev * dev,struct tlv * tlv)443 mt7925_nan_mcu_handle_de_event(struct mt792x_dev *dev, struct tlv *tlv)
444 {
445 u8 cluster_id[ETH_ALEN] __aligned(2) = {0x50, 0x6f, 0x9a, 0x01, 0x00, 0x00};
446 struct mt7925_nan_de_event *de_evt = NULL;
447 u16 len;
448
449 if (!dev || !tlv) {
450 if (dev)
451 dev_warn(dev->mt76.dev, "nan: failed to parse TLV\n");
452 return;
453 }
454
455 len = le16_to_cpu(tlv->len);
456 if (len < sizeof(*tlv) + sizeof(*de_evt)) {
457 dev_warn(dev->mt76.dev,
458 "nan: short de_event tlv len=%u\n", len);
459 return;
460 }
461
462 de_evt = (struct mt7925_nan_de_event *)tlv->data;
463 if (!de_evt) {
464 dev_warn(dev->mt76.dev, "nan: missing DE event payload\n");
465 return;
466 }
467
468 if (de_evt->event_type == NAN_EVENT_ID_DISC_MAC_ADDR)
469 return;
470
471 memcpy(cluster_id, de_evt->cluster_id, ETH_ALEN);
472
473 dev_dbg(dev->mt76.dev, "nan: evt=%u cluster=%pM\n",
474 de_evt->event_type, de_evt->cluster_id);
475
476 if (de_evt->event_type != NAN_EVENT_ID_JOINED_CLUSTER)
477 return;
478
479 if (!dev->nan_vif || !ieee80211_vif_nan_started(dev->nan_vif)) {
480 dev_warn(dev->mt76.dev, "nan: joined-cluster event but NAN not started\n");
481 return;
482 }
483
484 dev_dbg(dev->mt76.dev, "nan: anchor_master_rank=%*phN\n",
485 NAN_ANCHOR_MASTER_RANK_NUM, de_evt->anchor_master_rank);
486
487 dev_dbg(dev->mt76.dev, "nan: own_nmi=%pM master_nmi=%pM\n",
488 de_evt->own_nmi, de_evt->master_nmi);
489
490 ieee80211_nan_cluster_joined(dev->nan_vif, cluster_id, true, GFP_KERNEL);
491 }
492
mt7925_nan_mcu_event(struct mt792x_dev * dev,struct sk_buff * skb)493 void mt7925_nan_mcu_event(struct mt792x_dev *dev, struct sk_buff *skb)
494 {
495 struct tlv *tlv;
496 u32 tlv_len;
497
498 if (!dev || !skb)
499 return;
500
501 if (skb->len < sizeof(struct mt7925_mcu_rxd) + 4)
502 return;
503
504 skb_pull(skb, sizeof(struct mt7925_mcu_rxd) + 4);
505 tlv = (struct tlv *)skb->data;
506 tlv_len = skb->len;
507
508 while (tlv_len >= sizeof(*tlv)) {
509 u16 len = le16_to_cpu(tlv->len);
510
511 if (len < sizeof(*tlv) || len > tlv_len)
512 break;
513
514 switch (le16_to_cpu(tlv->tag)) {
515 case NAN_UNI_EVENT_ID_DE_EVENT_IND:
516 mt7925_nan_mcu_handle_de_event(dev, tlv);
517 break;
518 case NAN_UNI_EVENT_REPORT_DW_END:
519 mt7925_nan_handle_dw_ind(dev, tlv);
520 break;
521 default:
522 break;
523 }
524
525 tlv_len -= len;
526 tlv = (struct tlv *)((u8 *)tlv + len);
527 }
528 }
529
mt7925_nan_avail_ctrl_tlv(struct sk_buff * skb,struct ieee80211_vif * vif)530 static int mt7925_nan_avail_ctrl_tlv(struct sk_buff *skb,
531 struct ieee80211_vif *vif)
532 {
533 struct mt7925_nan_avail_ctrl_tlv *avail_ctrl_tlv;
534 struct ieee80211_nan_sched_cfg *sched;
535 struct tlv *tlv;
536 u8 seq_id = 0;
537 u16 ctrl = 0;
538
539 if (!skb || !vif)
540 return -EINVAL;
541
542 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_UPDATE_AVAILABILITY_CTRL,
543 sizeof(struct mt7925_nan_avail_ctrl_tlv));
544
545 if (!tlv)
546 return -ENOMEM;
547
548 sched = &vif->cfg.nan_sched;
549
550 ctrl = mt7925_nan_avail_attr_ctrl(sched);
551 if (sched->avail_blob_len >= NAN_AVAIL_ATTR_CTRL_OFFSET + 2)
552 seq_id = sched->avail_blob[NAN_AVAIL_SEQ_ID_OFFSET];
553
554 avail_ctrl_tlv = (struct mt7925_nan_avail_ctrl_tlv *)tlv;
555 avail_ctrl_tlv->avail_ctrl =
556 cpu_to_le16(ctrl & NAN_AVAIL_CTRL_CHECK_FOR_CHANGED);
557 avail_ctrl_tlv->seq_id = seq_id;
558
559 return 0;
560 }
561
mt7925_nan_slot_to_bitmap(struct ieee80211_vif * vif,struct mt7925_nan_ch_timeline * ch_list)562 static u32 mt7925_nan_slot_to_bitmap(struct ieee80211_vif *vif,
563 struct mt7925_nan_ch_timeline *ch_list)
564 {
565 struct ieee80211_nan_channel **slots = vif->cfg.nan_sched.schedule;
566 struct mt792x_vif *mvif = (struct mt792x_vif *)vif->drv_priv;
567 u32 num_channels = 0;
568 u32 i, j;
569
570 for (i = 0; i < ARRAY_SIZE(mvif->nan.local_sched); i++) {
571 struct cfg80211_chan_def *slot_chan = &mvif->nan.local_sched[i];
572 struct ieee80211_nan_channel *slot = slots[i];
573 bool is_found = false;
574
575 if (slot && !IS_ERR(slot) && slot->chanctx_conf) {
576 *slot_chan = slot->chanctx_conf->def;
577 } else {
578 memset(slot_chan, 0, sizeof(*slot_chan));
579 continue;
580 }
581
582 for (j = 0; j < num_channels; j++) {
583 u32 raw = le32_to_cpu(ch_list[j].ch_info);
584
585 if (FIELD_GET(NAN_CH_CTRL_PRIMARY_CH, raw) ==
586 slot_chan->chan->hw_value) {
587 u32 map = le32_to_cpu(ch_list[j].avail_map[0]);
588
589 ch_list[j].avail_map[0] = cpu_to_le32(map | BIT(i));
590 le32_add_cpu(&ch_list[j].num, 1);
591 is_found = true;
592 break;
593 }
594 }
595
596 if (!is_found && num_channels < NAN_TIMELINE_MGMT_CHNL_LIST_NUM) {
597 ch_list[num_channels].ch_info =
598 cpu_to_le32(FIELD_PREP(NAN_CH_CTRL_OP_CLASS,
599 slot->channel_entry[0]) |
600 FIELD_PREP(NAN_CH_CTRL_PRIMARY_CH,
601 slot_chan->chan->hw_value));
602 ch_list[num_channels].avail_map[0] = cpu_to_le32(BIT(i));
603 le32_add_cpu(&ch_list[num_channels].num, 1);
604 ch_list[num_channels].is_valid++;
605 num_channels++;
606 }
607 }
608
609 return num_channels;
610 }
611
mt7925_nan_avail_tlv(struct sk_buff * skb,struct ieee80211_vif * vif)612 static int mt7925_nan_avail_tlv(struct sk_buff *skb,
613 struct ieee80211_vif *vif)
614 {
615 struct mt7925_nan_avail_entry_tlv *avail_tlv;
616 struct ieee80211_nan_sched_cfg *sched;
617 struct tlv *tlv;
618 u16 ctrl = 0;
619
620 if (!skb || !vif)
621 return -EINVAL;
622
623 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_UPDATE_AVAILABILITY,
624 sizeof(struct mt7925_nan_avail_entry_tlv));
625
626 if (!tlv)
627 return -ENOMEM;
628
629 sched = &vif->cfg.nan_sched;
630
631 ctrl = mt7925_nan_avail_attr_ctrl(sched);
632
633 avail_tlv = (struct mt7925_nan_avail_entry_tlv *)tlv;
634 avail_tlv->map_id = ctrl & NAN_AVAIL_CTRL_MAPID;
635 avail_tlv->is_cond_avail = false;
636 avail_tlv->timeline_idx = 0;
637
638 mt7925_nan_slot_to_bitmap(vif, avail_tlv->ch_list);
639
640 avail_tlv->is_multi_map = false;
641
642 return 0;
643 }
644
mt7925_nan_local_sched_changed(struct mt792x_dev * dev,struct ieee80211_vif * vif)645 void mt7925_nan_local_sched_changed(struct mt792x_dev *dev,
646 struct ieee80211_vif *vif)
647 {
648 struct mt7925_nan_common_hdr *hdr;
649 struct mt76_dev *mdev;
650 bool deferred;
651 struct sk_buff *skb;
652 int ret = -ENOMEM;
653
654 if (!dev || !vif)
655 return;
656
657 mdev = &dev->mt76;
658 deferred = vif->cfg.nan_sched.deferred;
659
660 mt792x_mutex_acquire(dev);
661
662 skb = mt76_mcu_msg_alloc(mdev, NULL, MT7925_NAN_AVAIL_MAX_SIZE);
663 if (!skb)
664 goto out;
665
666 hdr = (struct mt7925_nan_common_hdr *)skb_put(skb, sizeof(*hdr));
667 memset(hdr, 0, sizeof(*hdr));
668
669 if (mt7925_nan_avail_ctrl_tlv(skb, vif) ||
670 mt7925_nan_avail_tlv(skb, vif)) {
671 dev_kfree_skb(skb);
672 goto out;
673 }
674
675 ret = mt76_mcu_skb_send_msg(mdev, skb,
676 MCU_UNI_CMD(NAN), true);
677 out:
678 mt792x_mutex_release(dev);
679
680 if (deferred) {
681 if (ret)
682 dev_err(mdev->dev,
683 "NAN: local schedule update failed: %d\n",
684 ret);
685
686 ieee80211_nan_sched_update_done(vif);
687 }
688 }
689
mt7925_nan_peer_rec_tlv(struct sk_buff * skb,struct ieee80211_sta * sta,struct mt792x_sta * msta,u8 is_activate)690 static int mt7925_nan_peer_rec_tlv(struct sk_buff *skb,
691 struct ieee80211_sta *sta,
692 struct mt792x_sta *msta,
693 u8 is_activate)
694 {
695 struct mt7925_nan_sched_manage_peer_rec_tlv *peer_rec_tlv;
696 struct tlv *tlv;
697
698 if (!skb || !sta || !msta)
699 return -EINVAL;
700
701 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_MANAGE_PEER_SCH_RECORD,
702 sizeof(struct mt7925_nan_sched_manage_peer_rec_tlv));
703
704 if (!tlv)
705 return -ENOMEM;
706
707 peer_rec_tlv = (struct mt7925_nan_sched_manage_peer_rec_tlv *)tlv;
708 peer_rec_tlv->sch_idx = cpu_to_le32(msta->nan_sched.sch_idx);
709 peer_rec_tlv->is_activate = is_activate;
710 memcpy(peer_rec_tlv->nmi_addr, sta->addr, ETH_ALEN);
711
712 return 0;
713 }
714
mt7925_nan_get_supported_bands(struct mt792x_vif * mvif)715 static u8 mt7925_nan_get_supported_bands(struct mt792x_vif *mvif)
716 {
717 struct wiphy *wiphy;
718 u8 bands = 0;
719
720 if (!mvif || !mvif->phy)
721 return BIT(NAN_SUPPORTED_BAND_ID_2P4G);
722
723 wiphy = mvif->phy->mt76->hw->wiphy;
724 if (wiphy->nan_supported_bands & BIT(NL80211_BAND_2GHZ))
725 bands |= BIT(NAN_SUPPORTED_BAND_ID_2P4G);
726 if (wiphy->nan_supported_bands & BIT(NL80211_BAND_5GHZ))
727 bands |= BIT(NAN_SUPPORTED_BAND_ID_5G);
728
729 return bands ?: BIT(NAN_SUPPORTED_BAND_ID_2P4G);
730 }
731
mt7925_nan_peer_cap_tlv(struct sk_buff * skb,struct ieee80211_sta * sta,struct mt792x_sta * msta)732 static int mt7925_nan_peer_cap_tlv(struct sk_buff *skb,
733 struct ieee80211_sta *sta,
734 struct mt792x_sta *msta)
735 {
736 struct mt7925_nan_sched_update_peer_cap_tlv *peer_cap_tlv;
737 struct ieee80211_nan_peer_sched *sched;
738 enum nl80211_band band;
739 struct tlv *tlv;
740 u16 primary_ch;
741 u32 i;
742
743 if (!skb || !sta || !msta)
744 return -EINVAL;
745
746 sched = sta->nan_sched;
747 if (!sched)
748 return -EINVAL;
749
750 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_UPDATE_PEER_CAPABILITY,
751 sizeof(struct mt7925_nan_sched_update_peer_cap_tlv));
752
753 if (!tlv)
754 return -ENOMEM;
755
756 peer_cap_tlv = (struct mt7925_nan_sched_update_peer_cap_tlv *)tlv;
757 peer_cap_tlv->sch_idx = cpu_to_le32(msta->nan_sched.sch_idx);
758 peer_cap_tlv->supported_bands =
759 mt7925_nan_get_supported_bands(msta->vif);
760 peer_cap_tlv->max_chnl_switch_time = cpu_to_le16(sched->max_chan_switch);
761
762 for (i = 0; i < sched->n_channels; i++) {
763 if (!sched->channels[i].chanctx_conf)
764 continue;
765
766 band = sched->channels[i].chanctx_conf->def.chan->band;
767 primary_ch =
768 sched->channels[i].chanctx_conf->def.chan->hw_value;
769
770 if (band == NL80211_BAND_2GHZ)
771 peer_cap_tlv->peer_supported_bands |=
772 BIT(NAN_SUPPORTED_BN_2G);
773 else if (primary_ch >= UNII1_LOWER_BOUND &&
774 primary_ch <= UNII1_UPPER_BOUND)
775 peer_cap_tlv->peer_supported_bands |=
776 BIT(NAN_SUPPORTED_BN_5G_LOW);
777 else if (primary_ch >= UNII3_LOWER_BOUND &&
778 primary_ch <= UNII3_UPPER_BOUND)
779 peer_cap_tlv->peer_supported_bands |=
780 BIT(NAN_SUPPORTED_BN_5G_HIGH);
781 }
782
783 return 0;
784 }
785
786 static void
mt7925_nan_fill_crb_committed(struct mt7925_nan_sched_update_crb_tlv * crb_tlv,struct ieee80211_vif * vif,struct ieee80211_nan_peer_sched * sched)787 mt7925_nan_fill_crb_committed(struct mt7925_nan_sched_update_crb_tlv *crb_tlv,
788 struct ieee80211_vif *vif,
789 struct ieee80211_nan_peer_sched *sched)
790 {
791 struct ieee80211_nan_sched_cfg *local_sched;
792 u8 local_map_id;
793 u32 m, slot;
794
795 if (!vif || !sched)
796 return;
797
798 local_sched = &vif->cfg.nan_sched;
799 local_map_id = mt7925_nan_avail_attr_ctrl(local_sched) &
800 NAN_AVAIL_CTRL_MAPID;
801
802 for (m = 0; m < CFG80211_NAN_MAX_PEER_MAPS &&
803 m < NAN_TIMELINE_MGMT_SIZE; m++) {
804 struct mt7925_nan_sched_timeline *tl =
805 &crb_tlv->comm_faw_timeline[m];
806 struct ieee80211_nan_peer_map *map = &sched->maps[m];
807 u32 avail_map = 0;
808
809 if (map->map_id == CFG80211_NAN_INVALID_MAP_ID)
810 continue;
811
812 tl->map_id = map->map_id;
813 tl->local_map_id = local_map_id;
814
815 for (slot = 0; slot < CFG80211_NAN_SCHED_NUM_TIME_SLOTS;
816 slot++) {
817 struct ieee80211_nan_channel *local_ch;
818 struct ieee80211_nan_channel *peer_ch;
819
820 local_ch = local_sched->schedule[slot];
821 peer_ch = map->slots[slot];
822
823 if (!local_ch || !local_ch->chanctx_conf ||
824 !peer_ch || !peer_ch->chanctx_conf)
825 continue;
826
827 if (local_ch->chanctx_conf != peer_ch->chanctx_conf)
828 continue;
829
830 avail_map |= BIT(slot);
831 }
832
833 tl->avail_map[0] = cpu_to_le32(avail_map);
834 }
835 }
836
mt7925_nan_update_crb_tlv(struct sk_buff * skb,struct ieee80211_sta * sta,struct mt792x_sta * msta)837 static int mt7925_nan_update_crb_tlv(struct sk_buff *skb,
838 struct ieee80211_sta *sta,
839 struct mt792x_sta *msta)
840 {
841 struct mt7925_nan_sched_update_crb_tlv *crb_tlv;
842 struct tlv *tlv;
843
844 if (!skb || !sta || !msta)
845 return -EINVAL;
846
847 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_UPDATE_CRB,
848 sizeof(struct mt7925_nan_sched_update_crb_tlv));
849
850 if (!tlv)
851 return -ENOMEM;
852
853 crb_tlv = (struct mt7925_nan_sched_update_crb_tlv *)tlv;
854 crb_tlv->sch_idx = cpu_to_le32(msta->nan_sched.sch_idx);
855 crb_tlv->flags = NAN_CRB_USE_DATA_PATH;
856 crb_tlv->is_use_ranging = false;
857 crb_tlv->comm_ndc_ctrl.is_valid = false;
858
859 mt7925_nan_fill_crb_committed(crb_tlv, msta->vif->phy->dev->nan_vif,
860 sta->nan_sched);
861
862 return 0;
863 }
864
mt792x_nan_set_peer_schedule(struct mt792x_dev * dev,struct ieee80211_sta * sta)865 int mt792x_nan_set_peer_schedule(struct mt792x_dev *dev,
866 struct ieee80211_sta *sta)
867 {
868 struct mt7925_nan_common_hdr *hdr;
869 bool idx_allocated = false;
870 struct mt792x_sta *msta;
871 struct mt792x_nan *nan;
872 struct mt76_dev *mdev;
873 struct sk_buff *skb;
874 int ret;
875
876 if (!dev || !sta)
877 return -EINVAL;
878
879 mdev = &dev->mt76;
880
881 skb = mt76_mcu_msg_alloc(mdev, NULL, MT7925_NAN_PEER_MAX_SIZE);
882 if (!skb)
883 return -ENOMEM;
884
885 hdr = (struct mt7925_nan_common_hdr *)skb_put(skb, sizeof(*hdr));
886 memset(hdr, 0, sizeof(*hdr));
887
888 msta = (struct mt792x_sta *)sta->drv_priv;
889 nan = &msta->vif->nan;
890
891 /* Allocate connection index on first call for this peer */
892 if (!msta->nan_sched.idx_assigned) {
893 int idx = find_first_zero_bit(&nan->conn_bitmap,
894 NAN_MAX_CONN_CFG);
895 if (idx >= NAN_MAX_CONN_CFG) {
896 dev_kfree_skb(skb);
897 return -ENOSPC;
898 }
899
900 set_bit(idx, &nan->conn_bitmap);
901 msta->nan_sched.sch_idx = idx;
902 msta->nan_sched.idx_assigned = true;
903 idx_allocated = true;
904
905 if (mt7925_nan_peer_rec_tlv(skb, sta, msta, true) ||
906 mt7925_nan_peer_cap_tlv(skb, sta, msta)) {
907 ret = -ENOMEM;
908 goto free_skb;
909 }
910 }
911
912 if (mt7925_nan_update_crb_tlv(skb, sta, msta)) {
913 ret = -ENOMEM;
914 goto free_skb;
915 }
916
917 ret = mt76_mcu_skb_send_msg(mdev, skb, MCU_UNI_CMD(NAN), true);
918 if (ret && idx_allocated)
919 goto clear_idx;
920
921 return ret;
922
923 free_skb:
924 dev_kfree_skb(skb);
925 if (!idx_allocated)
926 return ret;
927
928 clear_idx:
929 clear_bit(msta->nan_sched.sch_idx, &nan->conn_bitmap);
930 msta->nan_sched.idx_assigned = false;
931
932 return ret;
933 }
934
mt792x_nan_set_peer_rec(struct mt76_dev * mdev,struct ieee80211_sta * sta)935 int mt792x_nan_set_peer_rec(struct mt76_dev *mdev,
936 struct ieee80211_sta *sta)
937 {
938 struct mt7925_nan_common_hdr *hdr;
939 struct mt792x_sta *msta;
940 struct mt792x_nan *nan;
941 struct sk_buff *skb;
942 int ret;
943
944 if (!mdev || !sta)
945 return -EINVAL;
946
947 skb = mt76_mcu_msg_alloc(mdev, NULL,
948 sizeof(struct mt7925_nan_common_hdr) +
949 sizeof(struct mt7925_nan_sched_manage_peer_rec_tlv));
950 if (!skb)
951 return -ENOMEM;
952
953 hdr = (struct mt7925_nan_common_hdr *)skb_put(skb, sizeof(*hdr));
954 memset(hdr, 0, sizeof(*hdr));
955
956 msta = (struct mt792x_sta *)sta->drv_priv;
957 nan = &msta->vif->nan;
958
959 if (!msta->nan_sched.idx_assigned) {
960 dev_kfree_skb(skb);
961 return 0;
962 }
963
964 if (mt7925_nan_peer_rec_tlv(skb, sta, msta, false)) {
965 dev_kfree_skb(skb);
966 return -ENOMEM;
967 }
968
969 ret = mt76_mcu_skb_send_msg(mdev, skb, MCU_UNI_CMD(NAN), true);
970 if (ret)
971 return ret;
972
973 clear_bit(msta->nan_sched.sch_idx, &nan->conn_bitmap);
974 msta->nan_sched.idx_assigned = false;
975
976 return 0;
977 }
978
mt792x_nan_map_sta_rec(struct mt76_dev * mdev,struct ieee80211_vif * vif,struct ieee80211_sta * sta)979 int mt792x_nan_map_sta_rec(struct mt76_dev *mdev,
980 struct ieee80211_vif *vif,
981 struct ieee80211_sta *sta)
982 {
983 struct mt7925_nan_sched_map_sta_rec_tlv *map_tlv;
984 struct mt7925_nan_common_hdr *hdr;
985 struct ieee80211_sta *nmi_sta;
986 struct mt792x_sta *nmi_msta;
987 struct mt792x_vif *mvif;
988 struct mt792x_sta *msta;
989 u8 nmi_addr[ETH_ALEN];
990 struct sk_buff *skb;
991 int ndp_ctx_id = 0;
992 int ret = -ENOMEM;
993 struct mt792x_dev *dev;
994 struct tlv *tlv;
995
996 if (!mdev || !vif || !sta)
997 return -EINVAL;
998
999 dev = container_of(mdev, struct mt792x_dev, mt76);
1000 msta = (struct mt792x_sta *)sta->drv_priv;
1001 mvif = (struct mt792x_vif *)vif->drv_priv;
1002
1003 rcu_read_lock();
1004 nmi_sta = rcu_dereference(sta->nmi);
1005 if (!nmi_sta) {
1006 rcu_read_unlock();
1007 dev_err(mdev->dev, "NAN: NMI sta not found for NDI sta %pM\n",
1008 sta->addr);
1009 return -EINVAL;
1010 }
1011
1012 memcpy(nmi_addr, nmi_sta->addr, ETH_ALEN);
1013 nmi_msta = (struct mt792x_sta *)nmi_sta->drv_priv;
1014
1015 if (!nmi_msta->nan_sched.idx_assigned) {
1016 if (!nmi_sta->nan_sched) {
1017 rcu_read_unlock();
1018 dev_err(mdev->dev,
1019 "NAN: peer schedule missing for NDI sta %pM\n",
1020 sta->addr);
1021 return -EAGAIN;
1022 }
1023
1024 rcu_read_unlock();
1025 ret = mt792x_nan_set_peer_schedule(dev, nmi_sta);
1026 if (ret)
1027 return ret;
1028
1029 rcu_read_lock();
1030 nmi_sta = rcu_dereference(sta->nmi);
1031 if (!nmi_sta) {
1032 rcu_read_unlock();
1033 dev_err(mdev->dev,
1034 "NAN: NMI sta not found for NDI sta %pM\n",
1035 sta->addr);
1036 return -EINVAL;
1037 }
1038
1039 nmi_msta = (struct mt792x_sta *)nmi_sta->drv_priv;
1040 }
1041
1042 ndp_ctx_id = find_first_zero_bit(&nmi_msta->nan_sched.ndp_ctx_bitmap,
1043 NAN_MAX_NDP_CXT);
1044 if (ndp_ctx_id >= NAN_MAX_NDP_CXT) {
1045 rcu_read_unlock();
1046 return -ENOSPC;
1047 }
1048
1049 set_bit(ndp_ctx_id, &nmi_msta->nan_sched.ndp_ctx_bitmap);
1050 rcu_read_unlock();
1051
1052 msta->nan_sched.ndp_ctx_id = ndp_ctx_id;
1053 msta->nan_sched.ndp_ctx_assigned = true;
1054
1055 skb = mt76_mcu_msg_alloc(mdev, NULL,
1056 sizeof(struct mt7925_nan_common_hdr) +
1057 sizeof(struct mt7925_nan_sched_map_sta_rec_tlv));
1058 if (!skb)
1059 goto clear_ndp_ctx;
1060
1061 hdr = (struct mt7925_nan_common_hdr *)skb_put(skb, sizeof(*hdr));
1062 memset(hdr, 0, sizeof(*hdr));
1063
1064 tlv = mt76_connac_mcu_add_tlv(skb, NAN_UNI_CMD_MAP_STA_RECORD,
1065 sizeof(struct mt7925_nan_sched_map_sta_rec_tlv));
1066 if (!tlv) {
1067 dev_kfree_skb(skb);
1068 ret = -ENOMEM;
1069 goto clear_ndp_ctx;
1070 }
1071
1072 map_tlv = (struct mt7925_nan_sched_map_sta_rec_tlv *)tlv;
1073 memcpy(map_tlv->nmi_addr, nmi_addr, ETH_ALEN);
1074 map_tlv->sta_rec_idx = msta->deflink.wcid.idx;
1075 map_tlv->ndp_ctx_id = ndp_ctx_id;
1076 map_tlv->role_idx = cpu_to_le32(mvif->bss_conf.mt76.idx);
1077 memcpy(map_tlv->ndi_addr, vif->addr, ETH_ALEN);
1078
1079 ret = mt76_mcu_skb_send_msg(mdev, skb,
1080 MCU_UNI_CMD(NAN), true);
1081 if (ret)
1082 goto clear_ndp_ctx;
1083
1084 return 0;
1085
1086 clear_ndp_ctx:
1087 rcu_read_lock();
1088 nmi_sta = rcu_dereference(sta->nmi);
1089 if (nmi_sta) {
1090 nmi_msta = (struct mt792x_sta *)nmi_sta->drv_priv;
1091 clear_bit(msta->nan_sched.ndp_ctx_id,
1092 &nmi_msta->nan_sched.ndp_ctx_bitmap);
1093 }
1094 rcu_read_unlock();
1095 msta->nan_sched.ndp_ctx_assigned = false;
1096
1097 return ret ?: -ENOMEM;
1098 }
1099