[PATCH 6/6] rtw89: support MULTI_BSSID and correct BSSID mask of H2C

[Ping-Ke Shih <pkshih@realtek.com>]

The BSSID mask of H2C is used to match BSSID of receiving packets.
Normally, we set six bits BSSID mask to exactly match BSSID of packets
sent by target AP. After we support multiple BSSID, it could connect a
nontransmitted BSSID, so we can only match first five bytes of BSSID.
That means we could possibly receive other AP's packets if only the last
byte of BSSID is different from target AP.

Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
---
 drivers/net/wireless/realtek/rtw89/cam.c  | 9 +++++++++
 drivers/net/wireless/realtek/rtw89/cam.h  | 5 +++++
 drivers/net/wireless/realtek/rtw89/core.c | 1 +
 3 files changed, 15 insertions(+)

diff --git a/drivers/net/wireless/realtek/rtw89/cam.c b/drivers/net/wireless/realtek/rtw89/cam.c
index 8a26adeb23fb2..b211ed2ea0722 100644
--- a/drivers/net/wireless/realtek/rtw89/cam.c
+++ b/drivers/net/wireless/realtek/rtw89/cam.c
@@ -599,14 +599,23 @@ int rtw89_cam_init(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif)
 int rtw89_cam_fill_bssid_cam_info(struct rtw89_dev *rtwdev,
 				  struct rtw89_vif *rtwvif, u8 *cmd)
 {
+#define BSSID_MATCH_ALL GENMASK(5, 0)
+#define BSSID_MATCH_5_BYTES GENMASK(4, 0)
 	struct ieee80211_vif *vif = rtwvif_to_vif(rtwvif);
 	struct rtw89_bssid_cam_entry *bssid_cam = &rtwvif->bssid_cam;
 	u8 bss_color = vif->bss_conf.he_bss_color.color;
+	u8 bss_mask;
+
+	if (vif->bss_conf.nontransmitted)
+		bss_mask = BSSID_MATCH_5_BYTES;
+	else
+		bss_mask = BSSID_MATCH_ALL;
 
 	FWCMD_SET_ADDR_BSSID_IDX(cmd, bssid_cam->bssid_cam_idx);
 	FWCMD_SET_ADDR_BSSID_OFFSET(cmd, bssid_cam->offset);
 	FWCMD_SET_ADDR_BSSID_LEN(cmd, bssid_cam->len);
 	FWCMD_SET_ADDR_BSSID_VALID(cmd, bssid_cam->valid);
+	FWCMD_SET_ADDR_BSSID_MASK(cmd, bss_mask);
 	FWCMD_SET_ADDR_BSSID_BB_SEL(cmd, bssid_cam->phy_idx);
 	FWCMD_SET_ADDR_BSSID_BSS_COLOR(cmd, bss_color);
 
diff --git a/drivers/net/wireless/realtek/rtw89/cam.h b/drivers/net/wireless/realtek/rtw89/cam.h
index a3931d3e40d26..e6a1ce39e7fd0 100644
--- a/drivers/net/wireless/realtek/rtw89/cam.h
+++ b/drivers/net/wireless/realtek/rtw89/cam.h
@@ -309,6 +309,11 @@ static inline void FWCMD_SET_ADDR_BSSID_BB_SEL(void *cmd, u32 value)
 	le32p_replace_bits((__le32 *)(cmd) + 13, value, BIT(1));
 }
 
+static inline void FWCMD_SET_ADDR_BSSID_MASK(void *cmd, u32 value)
+{
+	le32p_replace_bits((__le32 *)(cmd) + 13, value, GENMASK(7, 2));
+}
+
 static inline void FWCMD_SET_ADDR_BSSID_BSS_COLOR(void *cmd, u32 value)
 {
 	le32p_replace_bits((__le32 *)(cmd) + 13, value, GENMASK(13, 8));
diff --git a/drivers/net/wireless/realtek/rtw89/core.c b/drivers/net/wireless/realtek/rtw89/core.c
index 958fe2787c6a1..4df81f00c2211 100644
--- a/drivers/net/wireless/realtek/rtw89/core.c
+++ b/drivers/net/wireless/realtek/rtw89/core.c
@@ -3011,6 +3011,7 @@ static int rtw89_core_register_hw(struct rtw89_dev *rtwdev)
 	ieee80211_hw_set(hw, SUPPORTS_PS);
 	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
 	ieee80211_hw_set(hw, SINGLE_SCAN_ON_ALL_BANDS);
+	ieee80211_hw_set(hw, SUPPORTS_MULTI_BSSID);
 
 	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 				     BIT(NL80211_IFTYPE_AP);
-- 
2.25.1