kmod-redhat-rtw89/SOURCES/0053-wifi-rtw89-add-WoWLAN-pattern-match-support.patch

From 88f0524e1cdff80fcf2f547400d9fa369190cff9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=C3=8D=C3=B1igo=20Huguet?= <ihuguet@redhat.com>
Date: Wed, 24 May 2023 15:00:28 +0200
Subject: [PATCH 053/142] wifi: rtw89: add WoWLAN pattern match support
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Bugzilla: https://bugzilla.redhat.com/2207499

commit d2b68e95b5bc97d81d150a46447167bf21bd555f
Author: Chin-Yen Lee <timlee@realtek.com>
Date:   Thu Oct 27 13:27:07 2022 +0800

    wifi: rtw89: add WoWLAN pattern match support

    Pattern match is an option of WoWLAN to allow the device to be woken up
    from suspend mode when receiving packets matched user-designed patterns.

    The patterns are written into hardware via WoWLAN firmware in suspend
    flow if users have set up them. If packets matched designed pattern are
    received, WoWLAN firmware will send an interrupt and then wake up the
    device.

    Signed-off-by: Chin-Yen Lee <timlee@realtek.com>
    Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
    Signed-off-by: Kalle Valo <kvalo@kernel.org>
    Link: https://lore.kernel.org/r/20221027052707.14605-8-pkshih@realtek.com

Signed-off-by: Íñigo Huguet <ihuguet@redhat.com>
---
 drivers/net/wireless/realtek/rtw89/core.h     |  18 ++
 drivers/net/wireless/realtek/rtw89/fw.c       |  52 ++++++
 drivers/net/wireless/realtek/rtw89/fw.h       |  67 ++++++++
 drivers/net/wireless/realtek/rtw89/rtw8852a.c |   3 +
 drivers/net/wireless/realtek/rtw89/rtw8852c.c |   3 +
 drivers/net/wireless/realtek/rtw89/util.h     |  11 ++
 drivers/net/wireless/realtek/rtw89/wow.c      | 228 +++++++++++++++++++++++++-
 7 files changed, 381 insertions(+), 1 deletion(-)

diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index 9f80359e27aa2..b60de6662548b 100644
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -3489,9 +3489,27 @@ struct rtw89_phy_efuse_gain {
 	s8 comp[RF_PATH_MAX][RTW89_SUBBAND_NR]; /* S(8, 0) */
 };
 
+#define RTW89_MAX_PATTERN_NUM             18
+#define RTW89_MAX_PATTERN_MASK_SIZE       4
+#define RTW89_MAX_PATTERN_SIZE            128
+
+struct rtw89_wow_cam_info {
+	bool r_w;
+	u8 idx;
+	u32 mask[RTW89_MAX_PATTERN_MASK_SIZE];
+	u16 crc;
+	bool negative_pattern_match;
+	bool skip_mac_hdr;
+	bool uc;
+	bool mc;
+	bool bc;
+	bool valid;
+};
+
 struct rtw89_wow_param {
 	struct ieee80211_vif *wow_vif;
 	DECLARE_BITMAP(flags, RTW89_WOW_FLAG_NUM);
+	struct rtw89_wow_cam_info patterns[RTW89_MAX_PATTERN_NUM];
 	u8 pattern_cnt;
 	struct list_head pkt_list;
 };
diff --git a/drivers/net/wireless/realtek/rtw89/fw.c b/drivers/net/wireless/realtek/rtw89/fw.c
index c8f32a471e520..2763149586e27 100644
--- a/drivers/net/wireless/realtek/rtw89/fw.c
+++ b/drivers/net/wireless/realtek/rtw89/fw.c
@@ -3174,3 +3174,55 @@ int rtw89_fw_h2c_wow_wakeup_ctrl(struct rtw89_dev *rtwdev,
 
 	return ret;
 }
+
+#define H2C_WOW_CAM_UPD_LEN 24
+int rtw89_fw_wow_cam_update(struct rtw89_dev *rtwdev,
+			    struct rtw89_wow_cam_info *cam_info)
+{
+	struct sk_buff *skb;
+	int ret;
+
+	skb = rtw89_fw_h2c_alloc_skb_with_hdr(rtwdev, H2C_WOW_CAM_UPD_LEN);
+	if (!skb) {
+		rtw89_err(rtwdev, "failed to alloc skb for keep alive\n");
+		return -ENOMEM;
+	}
+
+	skb_put(skb, H2C_WOW_CAM_UPD_LEN);
+
+	RTW89_SET_WOW_CAM_UPD_R_W(skb->data, cam_info->r_w);
+	RTW89_SET_WOW_CAM_UPD_IDX(skb->data, cam_info->idx);
+	if (cam_info->valid) {
+		RTW89_SET_WOW_CAM_UPD_WKFM1(skb->data, cam_info->mask[0]);
+		RTW89_SET_WOW_CAM_UPD_WKFM2(skb->data, cam_info->mask[1]);
+		RTW89_SET_WOW_CAM_UPD_WKFM3(skb->data, cam_info->mask[2]);
+		RTW89_SET_WOW_CAM_UPD_WKFM4(skb->data, cam_info->mask[3]);
+		RTW89_SET_WOW_CAM_UPD_CRC(skb->data, cam_info->crc);
+		RTW89_SET_WOW_CAM_UPD_NEGATIVE_PATTERN_MATCH(skb->data,
+							     cam_info->negative_pattern_match);
+		RTW89_SET_WOW_CAM_UPD_SKIP_MAC_HDR(skb->data,
+						   cam_info->skip_mac_hdr);
+		RTW89_SET_WOW_CAM_UPD_UC(skb->data, cam_info->uc);
+		RTW89_SET_WOW_CAM_UPD_MC(skb->data, cam_info->mc);
+		RTW89_SET_WOW_CAM_UPD_BC(skb->data, cam_info->bc);
+	}
+	RTW89_SET_WOW_CAM_UPD_VALID(skb->data, cam_info->valid);
+
+	rtw89_h2c_pkt_set_hdr(rtwdev, skb, FWCMD_TYPE_H2C,
+			      H2C_CAT_MAC,
+			      H2C_CL_MAC_WOW,
+			      H2C_FUNC_WOW_CAM_UPD, 0, 1,
+			      H2C_WOW_CAM_UPD_LEN);
+
+	ret = rtw89_h2c_tx(rtwdev, skb, false);
+	if (ret) {
+		rtw89_err(rtwdev, "failed to send h2c\n");
+		goto fail;
+	}
+
+	return 0;
+fail:
+	dev_kfree_skb_any(skb);
+
+	return ret;
+}
diff --git a/drivers/net/wireless/realtek/rtw89/fw.h b/drivers/net/wireless/realtek/rtw89/fw.h
index 25dd7bf8e730d..509a3eac5ffe3 100644
--- a/drivers/net/wireless/realtek/rtw89/fw.h
+++ b/drivers/net/wireless/realtek/rtw89/fw.h
@@ -2033,6 +2033,71 @@ static inline void RTW89_SET_WOW_WAKEUP_CTRL_MAC_ID(void *h2c, u32 val)
 	le32p_replace_bits((__le32 *)h2c, val, GENMASK(31, 24));
 }
 
+static inline void RTW89_SET_WOW_CAM_UPD_R_W(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c, val, BIT(0));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_IDX(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c, val, GENMASK(7, 1));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_WKFM1(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 1, val, GENMASK(31, 0));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_WKFM2(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 2, val, GENMASK(31, 0));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_WKFM3(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 3, val, GENMASK(31, 0));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_WKFM4(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 4, val, GENMASK(31, 0));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_CRC(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 5, val, GENMASK(15, 0));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_NEGATIVE_PATTERN_MATCH(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 5, val, BIT(22));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_SKIP_MAC_HDR(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 5, val, BIT(23));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_UC(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 5, val, BIT(24));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_MC(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 5, val, BIT(25));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_BC(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 5, val, BIT(26));
+}
+
+static inline void RTW89_SET_WOW_CAM_UPD_VALID(void *h2c, u32 val)
+{
+	le32p_replace_bits((__le32 *)h2c + 5, val, BIT(31));
+}
+
 enum rtw89_btc_btf_h2c_class {
 	BTFC_SET = 0x10,
 	BTFC_GET = 0x11,
@@ -3039,6 +3104,8 @@ int rtw89_fw_h2c_wow_global(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
 int rtw89_fw_h2c_wow_wakeup_ctrl(struct rtw89_dev *rtwdev,
 				 struct rtw89_vif *rtwvif, bool enable);
 
+int rtw89_fw_wow_cam_update(struct rtw89_dev *rtwdev,
+			    struct rtw89_wow_cam_info *cam_info);
 static inline void rtw89_fw_h2c_init_ba_cam(struct rtw89_dev *rtwdev)
 {
 	const struct rtw89_chip_info *chip = rtwdev->chip;
diff --git a/drivers/net/wireless/realtek/rtw89/rtw8852a.c b/drivers/net/wireless/realtek/rtw89/rtw8852a.c
index becce3b48f518..4cea5fb4327d7 100644
--- a/drivers/net/wireless/realtek/rtw89/rtw8852a.c
+++ b/drivers/net/wireless/realtek/rtw89/rtw8852a.c
@@ -1993,6 +1993,9 @@ static void rtw8852a_query_ppdu(struct rtw89_dev *rtwdev,
 #ifdef CONFIG_PM
 static const struct wiphy_wowlan_support rtw_wowlan_stub_8852a = {
 	.flags = WIPHY_WOWLAN_MAGIC_PKT | WIPHY_WOWLAN_DISCONNECT,
+	.n_patterns = RTW89_MAX_PATTERN_NUM,
+	.pattern_max_len = RTW89_MAX_PATTERN_SIZE,
+	.pattern_min_len = 1,
 };
 #endif
 
diff --git a/drivers/net/wireless/realtek/rtw89/rtw8852c.c b/drivers/net/wireless/realtek/rtw89/rtw8852c.c
index acb0e2e0adbac..6619ba7307199 100644
--- a/drivers/net/wireless/realtek/rtw89/rtw8852c.c
+++ b/drivers/net/wireless/realtek/rtw89/rtw8852c.c
@@ -2799,6 +2799,9 @@ static int rtw8852c_mac_disable_bb_rf(struct rtw89_dev *rtwdev)
 #ifdef CONFIG_PM
 static const struct wiphy_wowlan_support rtw_wowlan_stub_8852c = {
 	.flags = WIPHY_WOWLAN_MAGIC_PKT | WIPHY_WOWLAN_DISCONNECT,
+	.n_patterns = RTW89_MAX_PATTERN_NUM,
+	.pattern_max_len = RTW89_MAX_PATTERN_SIZE,
+	.pattern_min_len = 1,
 };
 #endif
 
diff --git a/drivers/net/wireless/realtek/rtw89/util.h b/drivers/net/wireless/realtek/rtw89/util.h
index 1ae80b7561daa..e2ed4565025dd 100644
--- a/drivers/net/wireless/realtek/rtw89/util.h
+++ b/drivers/net/wireless/realtek/rtw89/util.h
@@ -44,4 +44,15 @@ static inline s32 s32_div_u32_round_closest(s32 dividend, u32 divisor)
 	return s32_div_u32_round_down(dividend + divisor / 2, divisor, NULL);
 }
 
+static inline void ether_addr_copy_mask(u8 *dst, const u8 *src, u8 mask)
+{
+	int i;
+
+	eth_zero_addr(dst);
+	for (i = 0; i < ETH_ALEN; i++) {
+		if (mask & BIT(i))
+			dst[i] = src[i];
+	}
+}
+
 #endif
diff --git a/drivers/net/wireless/realtek/rtw89/wow.c b/drivers/net/wireless/realtek/rtw89/wow.c
index 3d30083fcb9a8..7de4dd047d6b3 100644
--- a/drivers/net/wireless/realtek/rtw89/wow.c
+++ b/drivers/net/wireless/realtek/rtw89/wow.c
@@ -162,6 +162,227 @@ static void rtw89_wow_vif_iter(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvi
 	}
 }
 
+static u16 __rtw89_cal_crc16(u8 data, u16 crc)
+{
+	u8 shift_in, data_bit;
+	u8 crc_bit4, crc_bit11, crc_bit15;
+	u16 crc_result;
+	int index;
+
+	for (index = 0; index < 8; index++) {
+		crc_bit15 = crc & BIT(15) ? 1 : 0;
+		data_bit = data & BIT(index) ? 1 : 0;
+		shift_in = crc_bit15 ^ data_bit;
+
+		crc_result = crc << 1;
+
+		if (shift_in == 0)
+			crc_result &= ~BIT(0);
+		else
+			crc_result |= BIT(0);
+
+		crc_bit11 = (crc & BIT(11) ? 1 : 0) ^ shift_in;
+
+		if (crc_bit11 == 0)
+			crc_result &= ~BIT(12);
+		else
+			crc_result |= BIT(12);
+
+		crc_bit4 = (crc & BIT(4) ? 1 : 0) ^ shift_in;
+
+		if (crc_bit4 == 0)
+			crc_result &= ~BIT(5);
+		else
+			crc_result |= BIT(5);
+
+		crc = crc_result;
+	}
+	return crc;
+}
+
+static u16 rtw89_calc_crc(u8 *pdata, int length)
+{
+	u16 crc = 0xffff;
+	int i;
+
+	for (i = 0; i < length; i++)
+		crc = __rtw89_cal_crc16(pdata[i], crc);
+
+	/* get 1' complement */
+	return ~crc;
+}
+
+static int rtw89_wow_pattern_get_type(struct rtw89_vif *rtwvif,
+				      struct rtw89_wow_cam_info *rtw_pattern,
+				      const u8 *pattern, u8 da_mask)
+{
+	u8 da[ETH_ALEN];
+
+	ether_addr_copy_mask(da, pattern, da_mask);
+
+	/* Each pattern is divided into different kinds by DA address
+	 *  a. DA is broadcast address: set bc = 0;
+	 *  b. DA is multicast address: set mc = 0
+	 *  c. DA is unicast address same as dev's mac address: set uc = 0
+	 *  d. DA is unmasked. Also called wildcard type: set uc = bc = mc = 0
+	 *  e. Others is invalid type.
+	 */
+
+	if (is_broadcast_ether_addr(da))
+		rtw_pattern->bc = true;
+	else if (is_multicast_ether_addr(da))
+		rtw_pattern->mc = true;
+	else if (ether_addr_equal(da, rtwvif->mac_addr) &&
+		 da_mask == GENMASK(5, 0))
+		rtw_pattern->uc = true;
+	else if (!da_mask) /*da_mask == 0 mean wildcard*/
+		return 0;
+	else
+		return -EPERM;
+
+	return 0;
+}
+
+static int rtw89_wow_pattern_generate(struct rtw89_dev *rtwdev,
+				      struct rtw89_vif *rtwvif,
+				      const struct cfg80211_pkt_pattern *pkt_pattern,
+				      struct rtw89_wow_cam_info *rtw_pattern)
+{
+	u8 mask_hw[RTW89_MAX_PATTERN_MASK_SIZE * 4] = {0};
+	u8 content[RTW89_MAX_PATTERN_SIZE] = {0};
+	const u8 *mask;
+	const u8 *pattern;
+	u8 mask_len;
+	u16 count;
+	u32 len;
+	int i, ret;
+
+	pattern = pkt_pattern->pattern;
+	len = pkt_pattern->pattern_len;
+	mask = pkt_pattern->mask;
+	mask_len = DIV_ROUND_UP(len, 8);
+	memset(rtw_pattern, 0, sizeof(*rtw_pattern));
+
+	ret = rtw89_wow_pattern_get_type(rtwvif, rtw_pattern, pattern,
+					 mask[0] & GENMASK(5, 0));
+	if (ret)
+		return ret;
+
+	/* translate mask from os to mask for hw
+	 * pattern from OS uses 'ethenet frame', like this:
+	 * |    6   |    6   |   2  |     20    |  Variable  |  4  |
+	 * |--------+--------+------+-----------+------------+-----|
+	 * |    802.3 Mac Header    | IP Header | TCP Packet | FCS |
+	 * |   DA   |   SA   | Type |
+	 *
+	 * BUT, packet catched by our HW is in '802.11 frame', begin from LLC
+	 * |     24 or 30      |    6   |   2  |     20    |  Variable  |  4  |
+	 * |-------------------+--------+------+-----------+------------+-----|
+	 * | 802.11 MAC Header |       LLC     | IP Header | TCP Packet | FCS |
+	 *		       | Others | Tpye |
+	 *
+	 * Therefore, we need translate mask_from_OS to mask_to_hw.
+	 * We should left-shift mask by 6 bits, then set the new bit[0~5] = 0,
+	 * because new mask[0~5] means 'SA', but our HW packet begins from LLC,
+	 * bit[0~5] corresponds to first 6 Bytes in LLC, they just don't match.
+	 */
+
+	/* Shift 6 bits */
+	for (i = 0; i < mask_len - 1; i++) {
+		mask_hw[i] = u8_get_bits(mask[i], GENMASK(7, 6)) |
+			     u8_get_bits(mask[i + 1], GENMASK(5, 0)) << 2;
+	}
+	mask_hw[i] = u8_get_bits(mask[i], GENMASK(7, 6));
+
+	/* Set bit 0-5 to zero */
+	mask_hw[0] &= ~GENMASK(5, 0);
+
+	memcpy(rtw_pattern->mask, mask_hw, sizeof(rtw_pattern->mask));
+
+	/* To get the wake up pattern from the mask.
+	 * We do not count first 12 bits which means
+	 * DA[6] and SA[6] in the pattern to match HW design.
+	 */
+	count = 0;
+	for (i = 12; i < len; i++) {
+		if ((mask[i / 8] >> (i % 8)) & 0x01) {
+			content[count] = pattern[i];
+			count++;
+		}
+	}
+
+	rtw_pattern->crc = rtw89_calc_crc(content, count);
+
+	return 0;
+}
+
+static int rtw89_wow_parse_patterns(struct rtw89_dev *rtwdev,
+				    struct rtw89_vif *rtwvif,
+				    struct cfg80211_wowlan *wowlan)
+{
+	struct rtw89_wow_param *rtw_wow = &rtwdev->wow;
+	struct rtw89_wow_cam_info *rtw_pattern = rtw_wow->patterns;
+	int i;
+	int ret;
+
+	if (!wowlan->n_patterns || !wowlan->patterns)
+		return 0;
+
+	for (i = 0; i < wowlan->n_patterns; i++) {
+		rtw_pattern = &rtw_wow->patterns[i];
+		ret = rtw89_wow_pattern_generate(rtwdev, rtwvif,
+						 &wowlan->patterns[i],
+						 rtw_pattern);
+		if (ret) {
+			rtw89_err(rtwdev, "failed to generate pattern(%d)\n", i);
+			rtw_wow->pattern_cnt = 0;
+			return ret;
+		}
+
+		rtw_pattern->r_w = true;
+		rtw_pattern->idx = i;
+		rtw_pattern->negative_pattern_match = false;
+		rtw_pattern->skip_mac_hdr = true;
+		rtw_pattern->valid = true;
+	}
+	rtw_wow->pattern_cnt = wowlan->n_patterns;
+
+	return 0;
+}
+
+static void rtw89_wow_pattern_clear_cam(struct rtw89_dev *rtwdev)
+{
+	struct rtw89_wow_param *rtw_wow = &rtwdev->wow;
+	struct rtw89_wow_cam_info *rtw_pattern = rtw_wow->patterns;
+	int i = 0;
+
+	for (i = 0; i < rtw_wow->pattern_cnt; i++) {
+		rtw_pattern = &rtw_wow->patterns[i];
+		rtw_pattern->valid = false;
+		rtw89_fw_wow_cam_update(rtwdev, rtw_pattern);
+	}
+}
+
+static void rtw89_wow_pattern_write(struct rtw89_dev *rtwdev)
+{
+	struct rtw89_wow_param *rtw_wow = &rtwdev->wow;
+	struct rtw89_wow_cam_info *rtw_pattern = rtw_wow->patterns;
+	int i;
+
+	for (i = 0; i < rtw_wow->pattern_cnt; i++)
+		rtw89_fw_wow_cam_update(rtwdev, rtw_pattern + i);
+}
+
+static void rtw89_wow_pattern_clear(struct rtw89_dev *rtwdev)
+{
+	struct rtw89_wow_param *rtw_wow = &rtwdev->wow;
+
+	rtw89_wow_pattern_clear_cam(rtwdev);
+
+	rtw_wow->pattern_cnt = 0;
+	memset(rtw_wow->patterns, 0, sizeof(rtw_wow->patterns));
+}
+
 static void rtw89_wow_clear_wakeups(struct rtw89_dev *rtwdev)
 {
 	struct rtw89_wow_param *rtw_wow = &rtwdev->wow;
@@ -188,7 +409,8 @@ static int rtw89_wow_set_wakeups(struct rtw89_dev *rtwdev,
 	if (!rtw_wow->wow_vif)
 		return -EPERM;
 
-	return 0;
+	rtwvif = (struct rtw89_vif *)rtw_wow->wow_vif->drv_priv;
+	return rtw89_wow_parse_patterns(rtwdev, rtwvif, wowlan);
 }
 
 static int rtw89_wow_cfg_wake(struct rtw89_dev *rtwdev, bool wow)
@@ -442,6 +664,8 @@ static int rtw89_wow_fw_start(struct rtw89_dev *rtwdev)
 	struct rtw89_vif *rtwvif = (struct rtw89_vif *)rtw_wow->wow_vif->drv_priv;
 	int ret;
 
+	rtw89_wow_pattern_write(rtwdev);
+
 	ret = rtw89_fw_h2c_keep_alive(rtwdev, rtwvif, true);
 	if (ret) {
 		rtw89_err(rtwdev, "wow: failed to enable keep alive\n");
@@ -476,6 +700,8 @@ static int rtw89_wow_fw_stop(struct rtw89_dev *rtwdev)
 	struct rtw89_vif *rtwvif = (struct rtw89_vif *)rtw_wow->wow_vif->drv_priv;
 	int ret;
 
+	rtw89_wow_pattern_clear(rtwdev);
+
 	ret = rtw89_fw_h2c_keep_alive(rtwdev, rtwvif, false);
 	if (ret) {
 		rtw89_err(rtwdev, "wow: failed to disable keep alive\n");
-- 
2.13.6