wifi: iwlwifi: mvm: rxmq: report link ID to mac80211

Add a fw_id_to_link_sta array in mvm to track the link
STA for each firmware station ID, and then use that to
report the link a frame was received on (since we know
the station ID from firmware).

Notably, this fixes beacon tracking for the correct link
since mac80211 now queues and processes those on the one
link identified by the link ID only.
Signed-off-by: Johannes Berg <johannes.berg@intel.com>
Signed-off-by: Gregory Greenman <gregory.greenman@intel.com>
Link: https://lore.kernel.org/r/20230329100039.c7dd3ec18077.I12ef9eb4a5b8b5c2b9d6bcaa1fda73b59eba39d8@changeidSigned-off-by: Johannes Berg <johannes.berg@intel.com>

drivers/net/wireless/intel/iwlwifi/mvm/fw.c

@@ -1570,8 +1570,10 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	}
 
 	/* init the fw <-> mac80211 STA mapping */
-	for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++)
+	for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) {
 		RCU_INIT_POINTER(mvm->fw_id_to_mac_id[i], NULL);
+		RCU_INIT_POINTER(mvm->fw_id_to_link_sta[i], NULL);
+	}
 
 	memset(&mvm->fw_link_ids_map, 0, sizeof(mvm->fw_link_ids_map));
 
@@ -1757,8 +1759,10 @@ int iwl_mvm_load_d3_fw(struct iwl_mvm *mvm)
 		goto error;
 
 	/* init the fw <-> mac80211 STA mapping */
-	for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++)
+	for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) {
 		RCU_INIT_POINTER(mvm->fw_id_to_mac_id[i], NULL);
+		RCU_INIT_POINTER(mvm->fw_id_to_link_sta[i], NULL);
+	}
 
 	if (iwl_fw_lookup_cmd_ver(mvm->fw, ADD_STA, 0) < 12) {
 		/*

drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c

@@ -3319,9 +3319,11 @@ void iwl_mvm_sta_pre_rcu_remove(struct ieee80211_hw *hw,
 		link_sta = rcu_dereference_protected(mvm_sta->link[link_id],
 						     lockdep_is_held(&mvm->mutex));
 		sta_id = link_sta->sta_id;
-		if (sta == rcu_access_pointer(mvm->fw_id_to_mac_id[sta_id]))
+		if (sta == rcu_access_pointer(mvm->fw_id_to_mac_id[sta_id])) {
 			rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id],
 					   ERR_PTR(-ENOENT));
+			RCU_INIT_POINTER(mvm->fw_id_to_link_sta[sta_id], NULL);
+		}
 	}
 	mutex_unlock(&mvm->mutex);
 }

drivers/net/wireless/intel/iwlwifi/mvm/mld-sta.c

@@ -472,6 +472,7 @@ static void iwl_mvm_mld_sta_rm_all_sta_links(struct iwl_mvm *mvm,
 			continue;
 
 		RCU_INIT_POINTER(mvm->fw_id_to_mac_id[link->sta_id], NULL);
+		RCU_INIT_POINTER(mvm->fw_id_to_link_sta[link->sta_id], NULL);
 		RCU_INIT_POINTER(mvm_sta->link[link_id], NULL);
 
 		if (link != &mvm_sta->deflink)
@@ -485,6 +486,7 @@ static void iwl_mvm_mld_free_sta_link(struct iwl_mvm *mvm,
 				      unsigned int link_id)
 {
 	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta_link->sta_id], NULL);
+	RCU_INIT_POINTER(mvm->fw_id_to_link_sta[mvm_sta_link->sta_id], NULL);
 	RCU_INIT_POINTER(mvm_sta->link[link_id], NULL);
 
 	if (mvm_sta_link != &mvm_sta->deflink)
@@ -496,6 +498,8 @@ static int iwl_mvm_mld_alloc_sta_link(struct iwl_mvm *mvm,
 				      struct ieee80211_sta *sta,
 				      unsigned int link_id)
 {
+	struct ieee80211_link_sta *link_sta =
+		rcu_dereference_protected(sta->link[link_id], 1);
 	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_link_sta *link;
 	u32 sta_id = iwl_mvm_find_free_sta_id(mvm,
@@ -515,6 +519,8 @@ static int iwl_mvm_mld_alloc_sta_link(struct iwl_mvm *mvm,
 	link->sta_id = sta_id;
 	rcu_assign_pointer(mvm_sta->link[link_id], link);
 	rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta);
+	rcu_assign_pointer(mvm->fw_id_to_link_sta[link->sta_id],
+			   link_sta);
 
 	return 0;
 }
@@ -604,6 +610,7 @@ static int iwl_mvm_alloc_sta_after_restart(struct iwl_mvm *mvm,
 			return ret;
 
 		rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
+		rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id], link_sta);
 		iwl_mvm_realloc_queues_after_restart(mvm, sta);
 
 		/* since we need only one station, no need to continue */
@@ -800,6 +807,7 @@ int iwl_mvm_mld_rm_sta_id(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	lockdep_assert_held(&mvm->mutex);
 
 	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
+	RCU_INIT_POINTER(mvm->fw_id_to_link_sta[sta_id], NULL);
 	return ret;
 }
 

drivers/net/wireless/intel/iwlwifi/mvm/mvm.h

@@ -905,6 +905,7 @@ struct iwl_mvm {
 	/* data related to data path */
 	struct iwl_rx_phy_info last_phy_info;
 	struct ieee80211_sta __rcu *fw_id_to_mac_id[IWL_MVM_STATION_COUNT_MAX];
+	struct ieee80211_link_sta __rcu *fw_id_to_link_sta[IWL_MVM_STATION_COUNT_MAX];
 	unsigned long fw_link_ids_map;
 	u8 rx_ba_sessions;
 

drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c

@@ -253,12 +253,22 @@ static void iwl_mvm_add_rtap_sniffer_config(struct iwl_mvm *mvm,
 static void iwl_mvm_pass_packet_to_mac80211(struct iwl_mvm *mvm,
 					    struct napi_struct *napi,
 					    struct sk_buff *skb, int queue,
-					    struct ieee80211_sta *sta)
+					    struct ieee80211_sta *sta,
+					    struct ieee80211_link_sta *link_sta)
 {
-	if (iwl_mvm_check_pn(mvm, skb, queue, sta))
+	if (unlikely(iwl_mvm_check_pn(mvm, skb, queue, sta))) {
 		kfree_skb(skb);
-	else
-		ieee80211_rx_napi(mvm->hw, sta, skb, napi);
+		return;
+	}
+
+	if (sta && sta->valid_links && link_sta) {
+		struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
+
+		rx_status->link_valid = 1;
+		rx_status->link_id = link_sta->link_id;
+	}
+
+	ieee80211_rx_napi(mvm->hw, sta, skb, napi);
 }
 
 static void iwl_mvm_get_signal_strength(struct iwl_mvm *mvm,
@@ -631,7 +641,7 @@ static void iwl_mvm_release_frames(struct iwl_mvm *mvm,
 		while ((skb = __skb_dequeue(skb_list))) {
 			iwl_mvm_pass_packet_to_mac80211(mvm, napi, skb,
 							reorder_buf->queue,
-							sta);
+							sta, NULL /* FIXME */);
 			reorder_buf->num_stored--;
 		}
 	}
@@ -2298,6 +2308,7 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 	u32 len;
 	u32 pkt_len = iwl_rx_packet_payload_len(pkt);
 	struct ieee80211_sta *sta = NULL;
+	struct ieee80211_link_sta *link_sta = NULL;
 	struct sk_buff *skb;
 	u8 crypt_len = 0;
 	size_t desc_size;
@@ -2454,6 +2465,7 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 			sta = rcu_dereference(mvm->fw_id_to_mac_id[id]);
 			if (IS_ERR(sta))
 				sta = NULL;
+			link_sta = rcu_dereference(mvm->fw_id_to_link_sta[id]);
 		}
 	} else if (!is_multicast_ether_addr(hdr->addr2)) {
 		/*
@@ -2588,8 +2600,10 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 	}
 
 	if (!iwl_mvm_reorder(mvm, napi, queue, sta, skb, desc) &&
-	    (likely(!iwl_mvm_time_sync_frame(mvm, skb, hdr->addr2))))
-		iwl_mvm_pass_packet_to_mac80211(mvm, napi, skb, queue, sta);
+	    likely(!iwl_mvm_time_sync_frame(mvm, skb, hdr->addr2)))
+		iwl_mvm_pass_packet_to_mac80211(mvm, napi, skb, queue, sta,
+						link_sta);
+
 out:
 	rcu_read_unlock();
 }