1 files changed, 104 insertions, 3 deletions

diff --git a/drivers/net/wireless/realtek/rtw88/util.c b/drivers/net/wireless/realtek/rtw88/util.c
index 2c515af214e7..66819f694405 100644
--- a/drivers/net/wireless/realtek/rtw88/util.c
+++ b/drivers/net/wireless/realtek/rtw88/util.c
@@ -23,7 +23,7 @@ EXPORT_SYMBOL(check_hw_ready);
 
 bool ltecoex_read_reg(struct rtw_dev *rtwdev, u16 offset, u32 *val)
 {
-	struct rtw_chip_info *chip = rtwdev->chip;
+	const struct rtw_chip_info *chip = rtwdev->chip;
 	const struct rtw_ltecoex_addr *ltecoex = chip->ltecoex_addr;
 
 	if (!check_hw_ready(rtwdev, ltecoex->ctrl, LTECOEX_READY, 1))
@@ -37,7 +37,7 @@ bool ltecoex_read_reg(struct rtw_dev *rtwdev, u16 offset, u32 *val)
 
 bool ltecoex_reg_write(struct rtw_dev *rtwdev, u16 offset, u32 value)
 {
-	struct rtw_chip_info *chip = rtwdev->chip;
+	const struct rtw_chip_info *chip = rtwdev->chip;
 	const struct rtw_ltecoex_addr *ltecoex = chip->ltecoex_addr;
 
 	if (!check_hw_ready(rtwdev, ltecoex->ctrl, LTECOEX_READY, 1))
@@ -101,7 +101,108 @@ void rtw_desc_to_mcsrate(u16 rate, u8 *mcs, u8 *nss)
 		*nss = 4;
 		*mcs = rate - DESC_RATEVHT4SS_MCS0;
 	} else if (rate >= DESC_RATEMCS0 &&
-		   rate <= DESC_RATEMCS15) {
+		   rate <= DESC_RATEMCS31) {
+		*nss = 0;
 		*mcs = rate - DESC_RATEMCS0;
 	}
 }
+
+struct rtw_stas_entry {
+	struct list_head list;
+	struct ieee80211_sta *sta;
+};
+
+struct rtw_iter_stas_data {
+	struct rtw_dev *rtwdev;
+	struct list_head list;
+};
+
+static void rtw_collect_sta_iter(void *data, struct ieee80211_sta *sta)
+{
+	struct rtw_iter_stas_data *iter_stas = data;
+	struct rtw_stas_entry *stas_entry;
+
+	stas_entry = kmalloc(sizeof(*stas_entry), GFP_ATOMIC);
+	if (!stas_entry)
+		return;
+
+	stas_entry->sta = sta;
+	list_add_tail(&stas_entry->list, &iter_stas->list);
+}
+
+void rtw_iterate_stas(struct rtw_dev *rtwdev,
+		      void (*iterator)(void *data,
+				       struct ieee80211_sta *sta),
+		      void *data)
+{
+	struct rtw_iter_stas_data iter_data;
+	struct rtw_stas_entry *sta_entry, *tmp;
+
+	/* &rtwdev->mutex makes sure no stations can be removed between
+	 * collecting the stations and iterating over them.
+	 */
+	lockdep_assert_held(&rtwdev->mutex);
+
+	iter_data.rtwdev = rtwdev;
+	INIT_LIST_HEAD(&iter_data.list);
+
+	ieee80211_iterate_stations_atomic(rtwdev->hw, rtw_collect_sta_iter,
+					  &iter_data);
+
+	list_for_each_entry_safe(sta_entry, tmp, &iter_data.list,
+				 list) {
+		list_del_init(&sta_entry->list);
+		iterator(data, sta_entry->sta);
+		kfree(sta_entry);
+	}
+}
+
+struct rtw_vifs_entry {
+	struct list_head list;
+	struct ieee80211_vif *vif;
+};
+
+struct rtw_iter_vifs_data {
+	struct rtw_dev *rtwdev;
+	struct list_head list;
+};
+
+static void rtw_collect_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct rtw_iter_vifs_data *iter_stas = data;
+	struct rtw_vifs_entry *vifs_entry;
+
+	vifs_entry = kmalloc(sizeof(*vifs_entry), GFP_ATOMIC);
+	if (!vifs_entry)
+		return;
+
+	vifs_entry->vif = vif;
+	list_add_tail(&vifs_entry->list, &iter_stas->list);
+}
+
+void rtw_iterate_vifs(struct rtw_dev *rtwdev,
+		      void (*iterator)(void *data, struct ieee80211_vif *vif),
+		      void *data)
+{
+	struct rtw_iter_vifs_data iter_data;
+	struct rtw_vifs_entry *vif_entry, *tmp;
+
+	/* &rtwdev->mutex makes sure no interfaces can be removed between
+	 * collecting the interfaces and iterating over them.
+	 */
+	lockdep_assert_held(&rtwdev->mutex);
+
+	iter_data.rtwdev = rtwdev;
+	INIT_LIST_HEAD(&iter_data.list);
+
+	ieee80211_iterate_active_interfaces_atomic(rtwdev->hw,
+						   IEEE80211_IFACE_ITER_NORMAL,
+						   rtw_collect_vif_iter, &iter_data);
+
+	list_for_each_entry_safe(vif_entry, tmp, &iter_data.list,
+				 list) {
+		list_del_init(&vif_entry->list);
+		iterator(data, vif_entry->vif);
+		kfree(vif_entry);
+	}
+}