link.c

  1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
  2/*
  3 * Copyright (C) 2024-2025 Intel Corporation
  4 */
  5
  6#include "constants.h"
  7#include "link.h"
  8#include "iface.h"
  9#include "mlo.h"
 10#include "hcmd.h"
 11#include "phy.h"
 12#include "fw/api/rs.h"
 13#include "fw/api/txq.h"
 14#include "fw/api/mac.h"
 15
 16#include "fw/api/context.h"
 17#include "fw/dbg.h"
 18
 19static int iwl_mld_send_link_cmd(struct iwl_mld *mld,
 20				 struct iwl_link_config_cmd *cmd,
 21				 enum iwl_ctxt_action action)
 22{
 23	int ret;
 24
 25	lockdep_assert_wiphy(mld->wiphy);
 26
 27	cmd->action = cpu_to_le32(action);
 28	ret = iwl_mld_send_cmd_pdu(mld,
 29				   WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD),
 30				   cmd);
 31	if (ret)
 32		IWL_ERR(mld, "Failed to send LINK_CONFIG_CMD (action:%d): %d\n",
 33			action, ret);
 34	return ret;
 35}
 36
 37static int iwl_mld_add_link_to_fw(struct iwl_mld *mld,
 38				  struct ieee80211_bss_conf *link_conf)
 39{
 40	struct ieee80211_vif *vif = link_conf->vif;
 41	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
 42	struct iwl_mld_link *link = iwl_mld_link_from_mac80211(link_conf);
 43	struct iwl_link_config_cmd cmd = {};
 44
 45	lockdep_assert_wiphy(mld->wiphy);
 46
 47	if (WARN_ON(!link))
 48		return -EINVAL;
 49
 50	cmd.link_id = cpu_to_le32(link->fw_id);
 51	cmd.mac_id = cpu_to_le32(mld_vif->fw_id);
 52	cmd.spec_link_id = link_conf->link_id;
 53	cmd.phy_id = cpu_to_le32(FW_CTXT_ID_INVALID);
 54
 55	ether_addr_copy(cmd.local_link_addr, link_conf->addr);
 56
 57	if (vif->type == NL80211_IFTYPE_ADHOC && link_conf->bssid)
 58		ether_addr_copy(cmd.ibss_bssid_addr, link_conf->bssid);
 59
 60	return iwl_mld_send_link_cmd(mld, &cmd, FW_CTXT_ACTION_ADD);
 61}
 62
 63/* Get the basic rates of the used band and add the mandatory ones */
 64static void iwl_mld_fill_rates(struct iwl_mld *mld,
 65			       struct ieee80211_bss_conf *link,
 66			       struct ieee80211_chanctx_conf *chan_ctx,
 67			       __le32 *cck_rates, __le32 *ofdm_rates)
 68{
 69	struct cfg80211_chan_def *chandef =
 70		iwl_mld_get_chandef_from_chanctx(mld, chan_ctx);
 71	struct ieee80211_supported_band *sband =
 72		mld->hw->wiphy->bands[chandef->chan->band];
 73	unsigned long basic = link->basic_rates;
 74	int lowest_present_ofdm = 100;
 75	int lowest_present_cck = 100;
 76	u32 cck = 0;
 77	u32 ofdm = 0;
 78	int i;
 79
 80	for_each_set_bit(i, &basic, BITS_PER_LONG) {
 81		int hw = sband->bitrates[i].hw_value;
 82
 83		if (hw >= IWL_FIRST_OFDM_RATE) {
 84			ofdm |= BIT(hw - IWL_FIRST_OFDM_RATE);
 85			if (lowest_present_ofdm > hw)
 86				lowest_present_ofdm = hw;
 87		} else {
 88			BUILD_BUG_ON(IWL_FIRST_CCK_RATE != 0);
 89
 90			cck |= BIT(hw);
 91			if (lowest_present_cck > hw)
 92				lowest_present_cck = hw;
 93		}
 94	}
 95
 96	/* Now we've got the basic rates as bitmaps in the ofdm and cck
 97	 * variables. This isn't sufficient though, as there might not
 98	 * be all the right rates in the bitmap. E.g. if the only basic
 99	 * rates are 5.5 Mbps and 11 Mbps, we still need to add 1 Mbps
100	 * and 6 Mbps because the 802.11-2007 standard says in 9.6:
101	 *
102	 *    [...] a STA responding to a received frame shall transmit
103	 *    its Control Response frame [...] at the highest rate in the
104	 *    BSSBasicRateSet parameter that is less than or equal to the
105	 *    rate of the immediately previous frame in the frame exchange
106	 *    sequence ([...]) and that is of the same modulation class
107	 *    ([...]) as the received frame. If no rate contained in the
108	 *    BSSBasicRateSet parameter meets these conditions, then the
109	 *    control frame sent in response to a received frame shall be
110	 *    transmitted at the highest mandatory rate of the PHY that is
111	 *    less than or equal to the rate of the received frame, and
112	 *    that is of the same modulation class as the received frame.
113	 *
114	 * As a consequence, we need to add all mandatory rates that are
115	 * lower than all of the basic rates to these bitmaps.
116	 */
117
118	if (lowest_present_ofdm > IWL_RATE_24M_INDEX)
119		ofdm |= IWL_RATE_BIT_MSK(24) >> IWL_FIRST_OFDM_RATE;
120	if (lowest_present_ofdm > IWL_RATE_12M_INDEX)
121		ofdm |= IWL_RATE_BIT_MSK(12) >> IWL_FIRST_OFDM_RATE;
122	/* 6M already there or needed so always add */
123	ofdm |= IWL_RATE_BIT_MSK(6) >> IWL_FIRST_OFDM_RATE;
124
125	/* CCK is a bit more complex with DSSS vs. HR/DSSS vs. ERP.
126	 * Note, however:
127	 *  - if no CCK rates are basic, it must be ERP since there must
128	 *    be some basic rates at all, so they're OFDM => ERP PHY
129	 *    (or we're in 5 GHz, and the cck bitmap will never be used)
130	 *  - if 11M is a basic rate, it must be ERP as well, so add 5.5M
131	 *  - if 5.5M is basic, 1M and 2M are mandatory
132	 *  - if 2M is basic, 1M is mandatory
133	 *  - if 1M is basic, that's the only valid ACK rate.
134	 * As a consequence, it's not as complicated as it sounds, just add
135	 * any lower rates to the ACK rate bitmap.
136	 */
137	if (lowest_present_cck > IWL_RATE_11M_INDEX)
138		cck |= IWL_RATE_BIT_MSK(11) >> IWL_FIRST_CCK_RATE;
139	if (lowest_present_cck > IWL_RATE_5M_INDEX)
140		cck |= IWL_RATE_BIT_MSK(5) >> IWL_FIRST_CCK_RATE;
141	if (lowest_present_cck > IWL_RATE_2M_INDEX)
142		cck |= IWL_RATE_BIT_MSK(2) >> IWL_FIRST_CCK_RATE;
143	/* 1M already there or needed so always add */
144	cck |= IWL_RATE_BIT_MSK(1) >> IWL_FIRST_CCK_RATE;
145
146	*cck_rates = cpu_to_le32((u32)cck);
147	*ofdm_rates = cpu_to_le32((u32)ofdm);
148}
149
150static void iwl_mld_fill_protection_flags(struct iwl_mld *mld,
151					  struct ieee80211_bss_conf *link,
152					  __le32 *protection_flags)
153{
154	u8 protection_mode = link->ht_operation_mode &
155				IEEE80211_HT_OP_MODE_PROTECTION;
156	u8 ht_flag = LINK_PROT_FLG_HT_PROT | LINK_PROT_FLG_FAT_PROT;
157
158	IWL_DEBUG_RATE(mld, "HT protection mode: %d\n", protection_mode);
159
160	if (link->use_cts_prot)
161		*protection_flags |= cpu_to_le32(LINK_PROT_FLG_TGG_PROTECT);
162
163	/* See section 9.23.3.1 of IEEE 80211-2012.
164	 * Nongreenfield HT STAs Present is not supported.
165	 */
166	switch (protection_mode) {
167	case IEEE80211_HT_OP_MODE_PROTECTION_NONE:
168		break;
169	case IEEE80211_HT_OP_MODE_PROTECTION_NONMEMBER:
170	case IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED:
171		*protection_flags |= cpu_to_le32(ht_flag);
172		break;
173	case IEEE80211_HT_OP_MODE_PROTECTION_20MHZ:
174		/* Protect when channel wider than 20MHz */
175		if (link->chanreq.oper.width > NL80211_CHAN_WIDTH_20)
176			*protection_flags |= cpu_to_le32(ht_flag);
177		break;
178	}
179}
180
181static u8 iwl_mld_mac80211_ac_to_fw_ac(enum ieee80211_ac_numbers ac)
182{
183	static const u8 mac80211_ac_to_fw[] = {
184		AC_VO,
185		AC_VI,
186		AC_BE,
187		AC_BK
188	};
189
190	return mac80211_ac_to_fw[ac];
191}
192
193static void iwl_mld_fill_qos_params(struct ieee80211_bss_conf *link,
194				    struct iwl_ac_qos *ac, __le32 *qos_flags)
195{
196	struct iwl_mld_link *mld_link = iwl_mld_link_from_mac80211(link);
197
198	/* no need to check mld_link since it is done in the caller */
199
200	for (int mac_ac = 0; mac_ac < IEEE80211_NUM_ACS; mac_ac++) {
201		u8 txf = iwl_mld_mac80211_ac_to_fw_tx_fifo(mac_ac);
202		u8 fw_ac = iwl_mld_mac80211_ac_to_fw_ac(mac_ac);
203
204		ac[fw_ac].cw_min =
205			cpu_to_le16(mld_link->queue_params[mac_ac].cw_min);
206		ac[fw_ac].cw_max =
207			cpu_to_le16(mld_link->queue_params[mac_ac].cw_max);
208		ac[fw_ac].edca_txop =
209			cpu_to_le16(mld_link->queue_params[mac_ac].txop * 32);
210		ac[fw_ac].aifsn = mld_link->queue_params[mac_ac].aifs;
211		ac[fw_ac].fifos_mask = BIT(txf);
212	}
213
214	if (link->qos)
215		*qos_flags |= cpu_to_le32(MAC_QOS_FLG_UPDATE_EDCA);
216
217	if (link->chanreq.oper.width != NL80211_CHAN_WIDTH_20_NOHT)
218		*qos_flags |= cpu_to_le32(MAC_QOS_FLG_TGN);
219}
220
221static bool iwl_mld_fill_mu_edca(struct iwl_mld *mld,
222				 const struct iwl_mld_link *mld_link,
223				 struct iwl_he_backoff_conf *trig_based_txf)
224{
225	for (int mac_ac = 0; mac_ac < IEEE80211_NUM_ACS; mac_ac++) {
226		const struct ieee80211_he_mu_edca_param_ac_rec *mu_edca =
227			&mld_link->queue_params[mac_ac].mu_edca_param_rec;
228		u8 fw_ac = iwl_mld_mac80211_ac_to_fw_ac(mac_ac);
229
230		if (!mld_link->queue_params[mac_ac].mu_edca)
231			return false;
232
233		trig_based_txf[fw_ac].cwmin =
234			cpu_to_le16(mu_edca->ecw_min_max & 0xf);
235		trig_based_txf[fw_ac].cwmax =
236			cpu_to_le16((mu_edca->ecw_min_max & 0xf0) >> 4);
237		trig_based_txf[fw_ac].aifsn =
238			cpu_to_le16(mu_edca->aifsn & 0xf);
239		trig_based_txf[fw_ac].mu_time =
240			cpu_to_le16(mu_edca->mu_edca_timer);
241	}
242	return true;
243}
244
245int
246iwl_mld_change_link_in_fw(struct iwl_mld *mld, struct ieee80211_bss_conf *link,
247			  u32 changes)
248{
249	struct iwl_mld_link *mld_link = iwl_mld_link_from_mac80211(link);
250	struct ieee80211_vif *vif = link->vif;
251	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
252	struct ieee80211_chanctx_conf *chan_ctx;
253	struct iwl_link_config_cmd cmd = {};
254	u32 flags = 0;
255
256	lockdep_assert_wiphy(mld->wiphy);
257
258	if (WARN_ON(!mld_link))
259		return -EINVAL;
260
261	cmd.link_id = cpu_to_le32(mld_link->fw_id);
262	cmd.spec_link_id = link->link_id;
263	cmd.mac_id = cpu_to_le32(mld_vif->fw_id);
264
265	chan_ctx = wiphy_dereference(mld->wiphy, mld_link->chan_ctx);
266
267	cmd.phy_id = cpu_to_le32(chan_ctx ?
268		iwl_mld_phy_from_mac80211(chan_ctx)->fw_id :
269		FW_CTXT_ID_INVALID);
270
271	ether_addr_copy(cmd.local_link_addr, link->addr);
272
273	cmd.active = cpu_to_le32(mld_link->active);
274
275	if ((changes & LINK_CONTEXT_MODIFY_ACTIVE) && !mld_link->active &&
276	    mld_link->silent_deactivation) {
277		/* We are de-activating a link that is having CSA with
278		 * immediate quiet in EMLSR. Tell the firmware not to send any
279		 * frame.
280		 */
281		cmd.block_tx = 1;
282		mld_link->silent_deactivation = false;
283	}
284
285	if (vif->type == NL80211_IFTYPE_ADHOC && link->bssid)
286		ether_addr_copy(cmd.ibss_bssid_addr, link->bssid);
287
288	/* Channel context is needed to get the rates */
289	if (chan_ctx)
290		iwl_mld_fill_rates(mld, link, chan_ctx, &cmd.cck_rates,
291				   &cmd.ofdm_rates);
292
293	cmd.cck_short_preamble = cpu_to_le32(link->use_short_preamble);
294	cmd.short_slot = cpu_to_le32(link->use_short_slot);
295
296	iwl_mld_fill_protection_flags(mld, link, &cmd.protection_flags);
297
298	iwl_mld_fill_qos_params(link, cmd.ac, &cmd.qos_flags);
299
300	cmd.bi = cpu_to_le32(link->beacon_int);
301	cmd.dtim_interval = cpu_to_le32(link->beacon_int * link->dtim_period);
302
303	/* Configure HE parameters only if HE is supported, and only after
304	 * the parameters are set in mac80211 (meaning after assoc)
305	 */
306	if (!link->he_support || iwlwifi_mod_params.disable_11ax ||
307	    (vif->type == NL80211_IFTYPE_STATION && !vif->cfg.assoc)) {
308		changes &= ~LINK_CONTEXT_MODIFY_HE_PARAMS;
309		goto send_cmd;
310	}
311
312	/* ap_sta may be NULL if we're disconnecting */
313	if (mld_vif->ap_sta) {
314		struct ieee80211_link_sta *link_sta =
315			link_sta_dereference_check(mld_vif->ap_sta,
316						   link->link_id);
317
318		if (!WARN_ON(!link_sta) && link_sta->he_cap.has_he &&
319		    link_sta->he_cap.he_cap_elem.mac_cap_info[5] &
320		    IEEE80211_HE_MAC_CAP5_OM_CTRL_UL_MU_DATA_DIS_RX)
321			cmd.ul_mu_data_disable = 1;
322	}
323
324	cmd.htc_trig_based_pkt_ext = link->htc_trig_based_pkt_ext;
325
326	if (link->uora_exists) {
327		cmd.rand_alloc_ecwmin = link->uora_ocw_range & 0x7;
328		cmd.rand_alloc_ecwmax = (link->uora_ocw_range >> 3) & 0x7;
329	}
330
331	if (iwl_mld_fill_mu_edca(mld, mld_link, cmd.trig_based_txf))
332		flags |= LINK_FLG_MU_EDCA_CW;
333
334	cmd.bss_color = link->he_bss_color.color;
335
336	if (!link->he_bss_color.enabled)
337		flags |= LINK_FLG_BSS_COLOR_DIS;
338
339	cmd.frame_time_rts_th = cpu_to_le16(link->frame_time_rts_th);
340
341	/* Block 26-tone RU OFDMA transmissions */
342	if (mld_link->he_ru_2mhz_block)
343		flags |= LINK_FLG_RU_2MHZ_BLOCK;
344
345	if (link->nontransmitted) {
346		ether_addr_copy(cmd.ref_bssid_addr, link->transmitter_bssid);
347		cmd.bssid_index = link->bssid_index;
348	}
349
350	/* The only EHT parameter is puncturing, and starting from PHY cmd
351	 * version 6 - it is sent there. For older versions of the PHY cmd,
352	 * puncturing is not needed at all.
353	 */
354	if (WARN_ON(changes & LINK_CONTEXT_MODIFY_EHT_PARAMS))
355		changes &= ~LINK_CONTEXT_MODIFY_EHT_PARAMS;
356
357send_cmd:
358	cmd.modify_mask = cpu_to_le32(changes);
359	cmd.flags = cpu_to_le32(flags);
360
361	return iwl_mld_send_link_cmd(mld, &cmd, FW_CTXT_ACTION_MODIFY);
362}
363
364int iwl_mld_activate_link(struct iwl_mld *mld,
365			  struct ieee80211_bss_conf *link)
366{
367	struct iwl_mld_link *mld_link = iwl_mld_link_from_mac80211(link);
368	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(link->vif);
369	int ret;
370
371	lockdep_assert_wiphy(mld->wiphy);
372
373	if (WARN_ON(!mld_link || mld_link->active))
374		return -EINVAL;
375
376	mld_link->active = true;
377
378	ret = iwl_mld_change_link_in_fw(mld, link,
379					LINK_CONTEXT_MODIFY_ACTIVE);
380	if (ret)
381		mld_link->active = false;
382	else
383		mld_vif->last_link_activation_time =
384			ktime_get_boottime_seconds();
385
386	return ret;
387}
388
389void iwl_mld_deactivate_link(struct iwl_mld *mld,
390			     struct ieee80211_bss_conf *link)
391{
392	struct iwl_mld_link *mld_link = iwl_mld_link_from_mac80211(link);
393	struct iwl_probe_resp_data *probe_data;
394
395	lockdep_assert_wiphy(mld->wiphy);
396
397	if (WARN_ON(!mld_link || !mld_link->active))
398		return;
399
400	iwl_mld_cancel_session_protection(mld, link->vif, link->link_id);
401
402	/* If we deactivate the link, we will probably remove it, or switch
403	 * channel. In both cases, the CSA or Notice of Absence information is
404	 * now irrelevant. Remove the data here.
405	 */
406	probe_data = wiphy_dereference(mld->wiphy, mld_link->probe_resp_data);
407	RCU_INIT_POINTER(mld_link->probe_resp_data, NULL);
408	if (probe_data)
409		kfree_rcu(probe_data, rcu_head);
410
411	mld_link->active = false;
412
413	iwl_mld_change_link_in_fw(mld, link, LINK_CONTEXT_MODIFY_ACTIVE);
414
415	/* Now that the link is not active in FW, we don't expect any new
416	 * notifications for it. Cancel the ones that are already pending
417	 */
418	iwl_mld_cancel_notifications_of_object(mld, IWL_MLD_OBJECT_TYPE_LINK,
419					       mld_link->fw_id);
420}
421
422static void
423iwl_mld_rm_link_from_fw(struct iwl_mld *mld, struct ieee80211_bss_conf *link)
424{
425	struct iwl_mld_link *mld_link = iwl_mld_link_from_mac80211(link);
426	struct iwl_link_config_cmd cmd = {};
427
428	lockdep_assert_wiphy(mld->wiphy);
429
430	if (WARN_ON(!mld_link))
431		return;
432
433	cmd.link_id = cpu_to_le32(mld_link->fw_id);
434	cmd.spec_link_id = link->link_id;
435	cmd.phy_id = cpu_to_le32(FW_CTXT_ID_INVALID);
436
437	iwl_mld_send_link_cmd(mld, &cmd, FW_CTXT_ACTION_REMOVE);
438}
439
440IWL_MLD_ALLOC_FN(link, bss_conf)
441
442/* Constructor function for struct iwl_mld_link */
443static int
444iwl_mld_init_link(struct iwl_mld *mld, struct ieee80211_bss_conf *link,
445		  struct iwl_mld_link *mld_link)
446{
447	mld_link->average_beacon_energy = 0;
448
449	iwl_mld_init_internal_sta(&mld_link->bcast_sta);
450	iwl_mld_init_internal_sta(&mld_link->mcast_sta);
451	iwl_mld_init_internal_sta(&mld_link->mon_sta);
452
453	return iwl_mld_allocate_link_fw_id(mld, &mld_link->fw_id, link);
454}
455
456/* Initializes the link structure, maps fw id to the ieee80211_bss_conf, and
457 * adds a link to the fw
458 */
459int iwl_mld_add_link(struct iwl_mld *mld,
460		     struct ieee80211_bss_conf *bss_conf)
461{
462	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(bss_conf->vif);
463	struct iwl_mld_link *link = iwl_mld_link_from_mac80211(bss_conf);
464	bool is_deflink = bss_conf == &bss_conf->vif->bss_conf;
465	int ret;
466
467	if (!link) {
468		if (is_deflink) {
469			link = &mld_vif->deflink;
470		} else {
471			link = kzalloc(sizeof(*link), GFP_KERNEL);
472			if (!link)
473				return -ENOMEM;
474		}
475	} else {
476		WARN_ON(!mld->fw_status.in_hw_restart);
477	}
478
479	ret = iwl_mld_init_link(mld, bss_conf, link);
480	if (ret)
481		goto free;
482
483	rcu_assign_pointer(mld_vif->link[bss_conf->link_id], link);
484
485	ret = iwl_mld_add_link_to_fw(mld, bss_conf);
486	if (ret) {
487		RCU_INIT_POINTER(mld->fw_id_to_bss_conf[link->fw_id], NULL);
488		RCU_INIT_POINTER(mld_vif->link[bss_conf->link_id], NULL);
489		goto free;
490	}
491
492	return ret;
493
494free:
495	if (!is_deflink)
496		kfree(link);
497	return ret;
498}
499
500/* Remove link from fw, unmap the bss_conf, and destroy the link structure */
501void iwl_mld_remove_link(struct iwl_mld *mld,
502			 struct ieee80211_bss_conf *bss_conf)
503{
504	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(bss_conf->vif);
505	struct iwl_mld_link *link = iwl_mld_link_from_mac80211(bss_conf);
506	bool is_deflink = link == &mld_vif->deflink;
507	u8 fw_id = link->fw_id;
508
509	if (WARN_ON(!link || link->active))
510		return;
511
512	iwl_mld_rm_link_from_fw(mld, bss_conf);
513	/* Continue cleanup on failure */
514
515	if (!is_deflink)
516		kfree_rcu(link, rcu_head);
517
518	RCU_INIT_POINTER(mld_vif->link[bss_conf->link_id], NULL);
519
520	if (WARN_ON(fw_id >= mld->fw->ucode_capa.num_links))
521		return;
522
523	RCU_INIT_POINTER(mld->fw_id_to_bss_conf[fw_id], NULL);
524}
525
526void iwl_mld_handle_missed_beacon_notif(struct iwl_mld *mld,
527					struct iwl_rx_packet *pkt)
528{
529	const struct iwl_missed_beacons_notif *notif = (const void *)pkt->data;
530	union iwl_dbg_tlv_tp_data tp_data = { .fw_pkt = pkt };
531	u32 fw_link_id = le32_to_cpu(notif->link_id);
532	u32 missed_bcon = le32_to_cpu(notif->consec_missed_beacons);
533	u32 missed_bcon_since_rx =
534		le32_to_cpu(notif->consec_missed_beacons_since_last_rx);
535	u32 scnd_lnk_bcn_lost =
536		le32_to_cpu(notif->consec_missed_beacons_other_link);
537	struct ieee80211_bss_conf *link_conf =
538		iwl_mld_fw_id_to_link_conf(mld, fw_link_id);
539	struct ieee80211_bss_conf *other_link;
540	u32 bss_param_ch_cnt_link_id, other_link_fw_id;
541	struct ieee80211_vif *vif;
542	u8 link_id;
543
544	if (WARN_ON(!link_conf))
545		return;
546
547	vif = link_conf->vif;
548	link_id = link_conf->link_id;
549	bss_param_ch_cnt_link_id = link_conf->bss_param_ch_cnt_link_id;
550
551	IWL_DEBUG_INFO(mld,
552		       "missed bcn link_id=%u, %u consecutive=%u\n",
553		       link_id, missed_bcon, missed_bcon_since_rx);
554
555	if (WARN_ON(!vif))
556		return;
557
558	iwl_dbg_tlv_time_point(&mld->fwrt,
559			       IWL_FW_INI_TIME_POINT_MISSED_BEACONS, &tp_data);
560
561	if (missed_bcon >= IWL_MLD_MISSED_BEACONS_THRESHOLD_LONG) {
562		if (missed_bcon_since_rx >=
563		    IWL_MLD_MISSED_BEACONS_SINCE_RX_THOLD) {
564			ieee80211_connection_loss(vif);
565			return;
566		}
567		IWL_WARN(mld,
568			 "missed beacons exceeds threshold, but receiving data. Stay connected, Expect bugs.\n");
569		return;
570	}
571
572	if (missed_bcon_since_rx > IWL_MLD_MISSED_BEACONS_THRESHOLD) {
573		ieee80211_cqm_beacon_loss_notify(vif, GFP_ATOMIC);
574
575		/* Not in EMLSR and we can't hear the link.
576		 * Try to switch to a better link. EMLSR case is handled below.
577		 */
578		if (!iwl_mld_emlsr_active(vif)) {
579			IWL_DEBUG_EHT(mld,
580				      "missed beacons exceeds threshold. link_id=%u. Try to switch to a better link.\n",
581				      link_id);
582			iwl_mld_int_mlo_scan(mld, vif);
583		}
584	}
585
586	/* no more logic if we're not in EMLSR */
587	if (hweight16(vif->active_links) <= 1)
588		return;
589
590	/* We are processing a notification before link activation */
591	if (le32_to_cpu(notif->other_link_id) == FW_CTXT_ID_INVALID)
592		return;
593
594	other_link_fw_id = le32_to_cpu(notif->other_link_id);
595	other_link = iwl_mld_fw_id_to_link_conf(mld, other_link_fw_id);
596
597	if (IWL_FW_CHECK(mld, !other_link, "link doesn't exist for: %d\n",
598			 other_link_fw_id))
599		return;
600
601	IWL_DEBUG_EHT(mld,
602		      "missed bcn link_id=%u: %u consecutive=%u, other link_id=%u: %u\n",
603		      link_id, missed_bcon, missed_bcon_since_rx,
604		      other_link->link_id, scnd_lnk_bcn_lost);
605
606	/* Exit EMLSR if we lost more than
607	 * IWL_MLD_MISSED_BEACONS_EXIT_ESR_THRESH beacons on boths links
608	 * OR more than IWL_MLD_BCN_LOSS_EXIT_ESR_THRESH on current link.
609	 * OR more than IWL_MLD_BCN_LOSS_EXIT_ESR_THRESH_BSS_PARAM_CHANGED
610	 * on current link and the link's bss_param_ch_count has changed on
611	 * the other link's beacon.
612	 */
613	if ((missed_bcon >= IWL_MLD_BCN_LOSS_EXIT_ESR_THRESH_2_LINKS &&
614	     scnd_lnk_bcn_lost >= IWL_MLD_BCN_LOSS_EXIT_ESR_THRESH_2_LINKS) ||
615	    missed_bcon >= IWL_MLD_BCN_LOSS_EXIT_ESR_THRESH ||
616	    (bss_param_ch_cnt_link_id != link_id &&
617	     missed_bcon >=
618	     IWL_MLD_BCN_LOSS_EXIT_ESR_THRESH_BSS_PARAM_CHANGED)) {
619		iwl_mld_exit_emlsr(mld, vif, IWL_MLD_EMLSR_EXIT_MISSED_BEACON,
620				   iwl_mld_get_primary_link(vif));
621	}
622}
623EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mld_handle_missed_beacon_notif);
624
625bool iwl_mld_cancel_missed_beacon_notif(struct iwl_mld *mld,
626					struct iwl_rx_packet *pkt,
627					u32 removed_link_id)
628{
629	struct iwl_missed_beacons_notif *notif = (void *)pkt->data;
630
631	if (le32_to_cpu(notif->other_link_id) == removed_link_id) {
632		/* Second link is being removed. Don't cancel the notification,
633		 * but mark second link as invalid.
634		 */
635		notif->other_link_id = cpu_to_le32(FW_CTXT_ID_INVALID);
636	}
637
638	/* If the primary link is removed, cancel the notification */
639	return le32_to_cpu(notif->link_id) == removed_link_id;
640}
641
642int iwl_mld_link_set_associated(struct iwl_mld *mld, struct ieee80211_vif *vif,
643				struct ieee80211_bss_conf *link)
644{
645	return iwl_mld_change_link_in_fw(mld, link, LINK_CONTEXT_MODIFY_ALL &
646					 ~(LINK_CONTEXT_MODIFY_ACTIVE |
647					   LINK_CONTEXT_MODIFY_EHT_PARAMS));
648}
649
650struct iwl_mld_rssi_to_grade {
651	s8 rssi[2];
652	u16 grade;
653};
654
655#define RSSI_TO_GRADE_LINE(_lb, _hb_uhb, _grade) \
656	{ \
657		.rssi = {_lb, _hb_uhb}, \
658		.grade = _grade \
659	}
660
661/*
662 * This array must be sorted by increasing RSSI for proper functionality.
663 * The grades are actually estimated throughput, represented as fixed-point
664 * with a scale factor of 1/10.
665 */
666static const struct iwl_mld_rssi_to_grade rssi_to_grade_map[] = {
667	RSSI_TO_GRADE_LINE(-85, -89, 172),
668	RSSI_TO_GRADE_LINE(-83, -86, 344),
669	RSSI_TO_GRADE_LINE(-82, -85, 516),
670	RSSI_TO_GRADE_LINE(-80, -83, 688),
671	RSSI_TO_GRADE_LINE(-77, -79, 1032),
672	RSSI_TO_GRADE_LINE(-73, -76, 1376),
673	RSSI_TO_GRADE_LINE(-70, -74, 1548),
674	RSSI_TO_GRADE_LINE(-69, -72, 1720),
675	RSSI_TO_GRADE_LINE(-65, -68, 2064),
676	RSSI_TO_GRADE_LINE(-61, -66, 2294),
677	RSSI_TO_GRADE_LINE(-58, -61, 2580),
678	RSSI_TO_GRADE_LINE(-55, -58, 2868),
679	RSSI_TO_GRADE_LINE(-46, -55, 3098),
680	RSSI_TO_GRADE_LINE(-43, -54, 3442)
681};
682
683#define MAX_GRADE (rssi_to_grade_map[ARRAY_SIZE(rssi_to_grade_map) - 1].grade)
684
685#define DEFAULT_CHAN_LOAD_2GHZ	30
686#define DEFAULT_CHAN_LOAD_5GHZ	15
687#define DEFAULT_CHAN_LOAD_6GHZ	0
688
689/* Factors calculation is done with fixed-point with a scaling factor of 1/256 */
690#define SCALE_FACTOR 256
691#define MAX_CHAN_LOAD 256
692
693static unsigned int
694iwl_mld_get_n_subchannels(const struct ieee80211_bss_conf *link_conf)
695{
696	enum nl80211_chan_width chan_width =
697		link_conf->chanreq.oper.width;
698	int mhz = nl80211_chan_width_to_mhz(chan_width);
699	unsigned int n_subchannels;
700
701	if (WARN_ONCE(mhz < 20 || mhz > 320,
702		      "Invalid channel width : (%d)\n", mhz))
703		return 1;
704
705	/* total number of subchannels */
706	n_subchannels = mhz / 20;
707
708	/* No puncturing if less than 80 MHz */
709	if (mhz >= 80)
710		n_subchannels -= hweight16(link_conf->chanreq.oper.punctured);
711
712	return n_subchannels;
713}
714
715static int
716iwl_mld_get_chan_load_from_element(struct iwl_mld *mld,
717				   struct ieee80211_bss_conf *link_conf)
718{
719	const struct cfg80211_bss_ies *ies;
720	const struct element *bss_load_elem = NULL;
721	const struct ieee80211_bss_load_elem *bss_load;
722
723	guard(rcu)();
724
725	ies = rcu_dereference(link_conf->bss->beacon_ies);
726	if (ies)
727		bss_load_elem = cfg80211_find_elem(WLAN_EID_QBSS_LOAD,
728						   ies->data, ies->len);
729
730	if (!bss_load_elem ||
731	    bss_load_elem->datalen != sizeof(*bss_load))
732		return -EINVAL;
733
734	bss_load = (const void *)bss_load_elem->data;
735
736	return bss_load->channel_util;
737}
738
739static unsigned int
740iwl_mld_get_chan_load_by_us(struct iwl_mld *mld,
741			    struct ieee80211_bss_conf *link_conf,
742			    bool expect_active_link)
743{
744	struct iwl_mld_link *mld_link = iwl_mld_link_from_mac80211(link_conf);
745	struct ieee80211_chanctx_conf *chan_ctx;
746	struct iwl_mld_phy *phy;
747
748	if (!mld_link || !mld_link->active) {
749		WARN_ON(expect_active_link);
750		return 0;
751	}
752
753	if (WARN_ONCE(!rcu_access_pointer(mld_link->chan_ctx),
754		      "Active link (%u) without channel ctxt assigned!\n",
755		      link_conf->link_id))
756		return 0;
757
758	chan_ctx = wiphy_dereference(mld->wiphy, mld_link->chan_ctx);
759	phy = iwl_mld_phy_from_mac80211(chan_ctx);
760
761	return phy->channel_load_by_us;
762}
763
764/* Returns error if the channel utilization element is invalid/unavailable */
765int iwl_mld_get_chan_load_by_others(struct iwl_mld *mld,
766				    struct ieee80211_bss_conf *link_conf,
767				    bool expect_active_link)
768{
769	int chan_load;
770	unsigned int chan_load_by_us;
771
772	/* get overall load */
773	chan_load = iwl_mld_get_chan_load_from_element(mld, link_conf);
774	if (chan_load < 0)
775		return chan_load;
776
777	chan_load_by_us = iwl_mld_get_chan_load_by_us(mld, link_conf,
778						      expect_active_link);
779
780	/* channel load by us is given in percentage */
781	chan_load_by_us =
782		NORMALIZE_PERCENT_TO_255(chan_load_by_us);
783
784	/* Use only values that firmware sends that can possibly be valid */
785	if (chan_load_by_us <= chan_load)
786		chan_load -= chan_load_by_us;
787
788	return chan_load;
789}
790
791static unsigned int
792iwl_mld_get_default_chan_load(struct ieee80211_bss_conf *link_conf)
793{
794	enum nl80211_band band = link_conf->chanreq.oper.chan->band;
795
796	switch (band) {
797	case NL80211_BAND_2GHZ:
798		return DEFAULT_CHAN_LOAD_2GHZ;
799	case NL80211_BAND_5GHZ:
800		return DEFAULT_CHAN_LOAD_5GHZ;
801	case NL80211_BAND_6GHZ:
802		return DEFAULT_CHAN_LOAD_6GHZ;
803	default:
804		WARN_ON(1);
805		return 0;
806	}
807}
808
809unsigned int iwl_mld_get_chan_load(struct iwl_mld *mld,
810				   struct ieee80211_bss_conf *link_conf)
811{
812	int chan_load;
813
814	chan_load = iwl_mld_get_chan_load_by_others(mld, link_conf, false);
815	if (chan_load >= 0)
816		return chan_load;
817
818	/* No information from the element, take the defaults */
819	chan_load = iwl_mld_get_default_chan_load(link_conf);
820
821	/* The defaults are given in percentage */
822	return NORMALIZE_PERCENT_TO_255(chan_load);
823}
824
825static unsigned int
826iwl_mld_get_avail_chan_load(struct iwl_mld *mld,
827			    struct ieee80211_bss_conf *link_conf)
828{
829	return MAX_CHAN_LOAD - iwl_mld_get_chan_load(mld, link_conf);
830}
831
832/* This function calculates the grade of a link. Returns 0 in error case */
833unsigned int iwl_mld_get_link_grade(struct iwl_mld *mld,
834				    struct ieee80211_bss_conf *link_conf)
835{
836	enum nl80211_band band;
837	int rssi_idx;
838	s32 link_rssi;
839	unsigned int grade = MAX_GRADE;
840
841	if (WARN_ON_ONCE(!link_conf))
842		return 0;
843
844	band = link_conf->chanreq.oper.chan->band;
845	if (WARN_ONCE(band != NL80211_BAND_2GHZ &&
846		      band != NL80211_BAND_5GHZ &&
847		      band != NL80211_BAND_6GHZ,
848		      "Invalid band (%u)\n", band))
849		return 0;
850
851	link_rssi = MBM_TO_DBM(link_conf->bss->signal);
852	/*
853	 * For 6 GHz the RSSI of the beacons is lower than
854	 * the RSSI of the data.
855	 */
856	if (band == NL80211_BAND_6GHZ && link_rssi)
857		link_rssi += 4;
858
859	rssi_idx = band == NL80211_BAND_2GHZ ? 0 : 1;
860
861	/* No valid RSSI - take the lowest grade */
862	if (!link_rssi)
863		link_rssi = rssi_to_grade_map[0].rssi[rssi_idx];
864
865	IWL_DEBUG_EHT(mld,
866		      "Calculating grade of link %d: band = %d, bandwidth = %d, punctured subchannels =0x%x RSSI = %d\n",
867		      link_conf->link_id, band,
868		      link_conf->chanreq.oper.width,
869		      link_conf->chanreq.oper.punctured, link_rssi);
870
871	/* Get grade based on RSSI */
872	for (int i = 0; i < ARRAY_SIZE(rssi_to_grade_map); i++) {
873		const struct iwl_mld_rssi_to_grade *line =
874			&rssi_to_grade_map[i];
875
876		if (link_rssi > line->rssi[rssi_idx])
877			continue;
878		grade = line->grade;
879		break;
880	}
881
882	/* Apply the channel load and puncturing factors */
883	grade = grade * iwl_mld_get_avail_chan_load(mld, link_conf) / SCALE_FACTOR;
884	grade = grade * iwl_mld_get_n_subchannels(link_conf);
885
886	IWL_DEBUG_EHT(mld, "Link %d's grade: %d\n", link_conf->link_id, grade);
887
888	return grade;
889}
890EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mld_get_link_grade);
891
892void iwl_mld_handle_beacon_filter_notif(struct iwl_mld *mld,
893					struct iwl_rx_packet *pkt)
894{
895	const struct iwl_beacon_filter_notif *notif = (const void *)pkt->data;
896	u32 link_id = le32_to_cpu(notif->link_id);
897	struct ieee80211_bss_conf *link_conf =
898		iwl_mld_fw_id_to_link_conf(mld, link_id);
899	struct iwl_mld_link *mld_link;
900
901	if (IWL_FW_CHECK(mld, !link_conf, "invalid link ID %d\n", link_id))
902		return;
903
904	mld_link = iwl_mld_link_from_mac80211(link_conf);
905	if (WARN_ON_ONCE(!mld_link))
906		return;
907
908	mld_link->average_beacon_energy = le32_to_cpu(notif->average_energy);
909}