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

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;
 };

drivers/net/wireless/realtek/rtw89/fw.c

@@ -3172,3 +3172,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;
+}

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;

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
 

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
 

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

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");