1 files changed, 299 insertions, 0 deletions

diff --git a/drivers/net/wireless/realtek/rtw89/debug.c b/drivers/net/wireless/realtek/rtw89/debug.c
index 3dc7981c510f..1264c2f82600 100644
--- a/drivers/net/wireless/realtek/rtw89/debug.c
+++ b/drivers/net/wireless/realtek/rtw89/debug.c
@@ -87,6 +87,7 @@ struct rtw89_debugfs {
 	struct rtw89_debugfs_priv disable_dm;
 	struct rtw89_debugfs_priv mlo_mode;
 	struct rtw89_debugfs_priv beacon_info;
+	struct rtw89_debugfs_priv diag_mac;
 };
 
 struct rtw89_debugfs_iter_data {
@@ -4361,6 +4362,302 @@ rtw89_debug_priv_mlo_mode_set(struct rtw89_dev *rtwdev,
 	return count;
 }
 
+enum __diag_mac_cmd {
+	__CMD_EQUALV,
+	__CMD_EQUALO,
+	__CMD_NEQUALV,
+	__CMD_NEQUALO,
+	__CMD_SETEQUALV,
+	__CMD_SETEQUALO,
+	__CMD_CMPWCR,
+	__CMD_CMPWWD,
+	__CMD_NEQ_CMPWCR,
+	__CMD_NEQ_CMPWWD,
+	__CMD_INCREMENT,
+	__CMD_MESSAGE,
+};
+
+enum __diag_mac_io {
+	__IO_NORMAL,
+	__IO_NORMAL_PCIE,
+	__IO_NORMAL_USB,
+	__IO_NORMAL_SDIO,
+	__IO_PCIE_CFG,
+	__IO_SDIO_CCCR,
+};
+
+struct __diag_mac_rule_header {
+	u8 sheet;
+	u8 cmd;
+	u8 seq_major;
+	u8 seq_minor;
+	u8 io_band;
+	#define __DIAG_MAC_IO GENMASK(3, 0)
+	#define __DIAG_MAC_N_BAND BIT(4)
+	#define __DIAG_MAC_HAS_BAND BIT(5)
+	u8 len; /* include header. Unit: 4 bytes */
+	u8 rsvd[2];
+} __packed;
+
+struct __diag_mac_rule_equal {
+	struct __diag_mac_rule_header header;
+	__le32 addr;
+	__le32 addr_name_offset;
+	__le32 mask;
+	__le32 val;
+	__le32 msg_offset;
+	u8 rsvd[4];
+} __packed;
+
+struct __diag_mac_rule_increment {
+	struct __diag_mac_rule_header header;
+	__le32 addr;
+	__le32 addr_name_offset;
+	__le32 mask;
+	__le16 sel;
+	__le16 delay;
+	__le32 msg_offset;
+	u8 rsvd[4];
+} __packed;
+
+struct __diag_mac_msg_buf {
+	__le16 len;
+	char string[];
+} __packed;
+
+static ssize_t rtw89_mac_diag_do_equalv(struct rtw89_dev *rtwdev,
+					char *buf, size_t bufsz,
+					const struct __diag_mac_rule_equal *r,
+					const void *msg_start,
+					u64 *positive_bmp)
+{
+	const struct __diag_mac_msg_buf *name = msg_start +
+						le32_to_cpu(r->addr_name_offset);
+	const struct __diag_mac_msg_buf *msg = msg_start +
+					       le32_to_cpu(r->msg_offset);
+	bool want_eq = r->header.cmd == __CMD_EQUALV;
+	char *p = buf, *end = buf + bufsz;
+	bool equal = false;
+	u32 val;
+
+	*positive_bmp <<= 1;
+
+	if (u8_get_bits(r->header.io_band, __DIAG_MAC_IO) == __IO_PCIE_CFG)
+		val = rtw89_read32_pci_cfg(rtwdev, le32_to_cpu(r->addr));
+	else
+		val = rtw89_read32(rtwdev, le32_to_cpu(r->addr));
+
+	if ((val & le32_to_cpu(r->mask)) == le32_to_cpu(r->val))
+		equal = true;
+
+	if (want_eq == equal) {
+		*positive_bmp |= BIT(0);
+		return p - buf;
+	}
+
+	p += scnprintf(p, end - p, "sheet: %d, cmd: %d, Reg: %.*s => %x, %.*s\n",
+		       r->header.sheet, r->header.cmd, le16_to_cpu(name->len),
+		       name->string, val, le16_to_cpu(msg->len), msg->string);
+
+	return p - buf;
+}
+
+static ssize_t rtw89_mac_diag_do_increment(struct rtw89_dev *rtwdev,
+					   char *buf, size_t bufsz,
+					   const struct __diag_mac_rule_increment *r,
+					   const void *msg_start,
+					   u64 *positive_bmp)
+{
+	const struct __diag_mac_msg_buf *name = msg_start +
+						le32_to_cpu(r->addr_name_offset);
+	const struct __diag_mac_msg_buf *msg = msg_start +
+					       le32_to_cpu(r->msg_offset);
+	char *p = buf, *end = buf + bufsz;
+	u32 addr = le32_to_cpu(r->addr);
+	u32 mask = le32_to_cpu(r->mask);
+	u16 sel = le16_to_cpu(r->sel);
+	u32 val1, val2;
+
+	*positive_bmp <<= 1;
+
+	rtw89_write32(rtwdev, addr, sel);
+
+	if (u8_get_bits(r->header.io_band, __DIAG_MAC_IO) == __IO_PCIE_CFG)
+		val1 = rtw89_read32_pci_cfg(rtwdev, addr);
+	else
+		val1 = rtw89_read32(rtwdev, addr);
+
+	mdelay(le16_to_cpu(r->delay));
+
+	if (u8_get_bits(r->header.io_band, __DIAG_MAC_IO) == __IO_PCIE_CFG)
+		val2 = rtw89_read32_pci_cfg(rtwdev, addr);
+	else
+		val2 = rtw89_read32(rtwdev, addr);
+
+	if ((val2 & mask) > (val1 & mask)) {
+		*positive_bmp |= BIT(0);
+		return p - buf;
+	}
+
+	p += scnprintf(p, end - p, "sheet: %d, cmd: %d, Reg: %.*s [%d]=> %x, %.*s\n",
+		       r->header.sheet, r->header.cmd, le16_to_cpu(name->len),
+		       name->string, le16_to_cpu(r->sel), val1,
+		       le16_to_cpu(msg->len), msg->string);
+
+	return p - buf;
+}
+
+static bool rtw89_mac_diag_match_hci(struct rtw89_dev *rtwdev,
+				     const struct __diag_mac_rule_header *rh)
+{
+	switch (u8_get_bits(rh->io_band, __DIAG_MAC_IO)) {
+	case __IO_NORMAL:
+	default:
+		return true;
+	case __IO_NORMAL_PCIE:
+	case __IO_PCIE_CFG:
+		if (rtwdev->hci.type == RTW89_HCI_TYPE_PCIE)
+			return true;
+		break;
+	case __IO_NORMAL_USB:
+		if (rtwdev->hci.type == RTW89_HCI_TYPE_USB)
+			return true;
+		break;
+	case __IO_NORMAL_SDIO:
+	case __IO_SDIO_CCCR:
+		if (rtwdev->hci.type == RTW89_HCI_TYPE_SDIO)
+			return true;
+		break;
+	}
+
+	return false;
+}
+
+static bool rtw89_mac_diag_match_band(struct rtw89_dev *rtwdev,
+				      const struct __diag_mac_rule_header *rh)
+{
+	u8 active_bands;
+	bool has_band;
+	u8 band;
+
+	has_band = u8_get_bits(rh->io_band, __DIAG_MAC_HAS_BAND);
+	if (!has_band)
+		return true;
+
+	band = u8_get_bits(rh->io_band, __DIAG_MAC_N_BAND);
+	active_bands = rtw89_get_active_phy_bitmap(rtwdev);
+
+	if (active_bands & BIT(band))
+		return true;
+
+	return false;
+}
+
+static ssize_t rtw89_mac_diag_iter_all(struct rtw89_dev *rtwdev,
+				       char *buf, size_t bufsz)
+{
+	const struct rtw89_fw_element_hdr *elm = rtwdev->fw.elm_info.diag_mac;
+	u32 n_plains = 0, n_rules = 0, n_positive = 0, n_ignore = 0;
+	char *p = buf, *end = buf + bufsz, *p_rewind;
+	const void *rule, *rule_end;
+	u32 elm_size, rule_size;
+	const void *msg_start;
+	u64 positive_bmp = 0;
+	u8 prev_sheet = 0;
+	u8 prev_seq = 0;
+	int limit;
+
+	if (!elm) {
+		p += scnprintf(p, end - p, "No diag_mac entry\n");
+		goto out;
+	}
+
+	rule_size = le32_to_cpu(elm->u.diag_mac.rule_size);
+	elm_size = le32_to_cpu(elm->size);
+
+	if (ALIGN(rule_size, 16) > elm_size) {
+		p += scnprintf(p, end - p, "rule size (%u) exceed elm_size (%u)\n",
+			       ALIGN(rule_size, 16), elm_size);
+		goto out;
+	}
+
+	rule = &elm->u.diag_mac.rules_and_msgs[0];
+	rule_end = &elm->u.diag_mac.rules_and_msgs[rule_size];
+	msg_start = &elm->u.diag_mac.rules_and_msgs[ALIGN(rule_size, 16)];
+
+	for (limit = 0; limit < 5000 && rule < rule_end; limit++) {
+		const struct __diag_mac_rule_header *rh = rule;
+		u8 sheet = rh->sheet;
+		u8 seq = rh->seq_major;
+
+		if (!rtw89_mac_diag_match_hci(rtwdev, rh) ||
+		    !rtw89_mac_diag_match_band(rtwdev, rh)) {
+			n_ignore++;
+			goto next;
+		}
+
+		if (!seq || prev_sheet != sheet || prev_seq != seq) {
+			if (positive_bmp) {
+				n_positive++;
+				/*
+				 * discard output for negative results if one in
+				 * a sequence set is positive.
+				 */
+				if (p_rewind)
+					p = p_rewind;
+			}
+			p_rewind = seq ? p : NULL;
+			positive_bmp = 0;
+			n_rules++;
+		}
+
+		switch (rh->cmd) {
+		case __CMD_EQUALV:
+		case __CMD_NEQUALV:
+			p += rtw89_mac_diag_do_equalv(rtwdev, p, end - p, rule,
+						      msg_start, &positive_bmp);
+			break;
+		case __CMD_INCREMENT:
+			p += rtw89_mac_diag_do_increment(rtwdev, p, end - p, rule,
+							 msg_start, &positive_bmp);
+			break;
+		default:
+			p += scnprintf(p, end - p, "unknown rule cmd %u\n", rh->cmd);
+			break;
+		}
+
+next:
+		n_plains++;
+		rule += rh->len * 4;
+		prev_seq = seq;
+		prev_sheet = sheet;
+	}
+
+	if (positive_bmp) {
+		n_positive++;
+		if (p_rewind)
+			p = p_rewind;
+	}
+
+	p += scnprintf(p, end - p, "\nPlain(Ignore)/Rules/Positive: %u(%u)/%u/%u\n",
+		       n_plains, n_ignore, n_rules, n_positive);
+
+out:
+	return p - buf;
+}
+
+static ssize_t
+rtw89_debug_priv_diag_mac_get(struct rtw89_dev *rtwdev,
+			      struct rtw89_debugfs_priv *debugfs_priv,
+			      char *buf, size_t bufsz)
+{
+	lockdep_assert_wiphy(rtwdev->hw->wiphy);
+
+	rtw89_leave_lps(rtwdev);
+
+	return rtw89_mac_diag_iter_all(rtwdev, buf, bufsz);
+}
+
 static ssize_t
 rtw89_debug_priv_beacon_info_get(struct rtw89_dev *rtwdev,
 				 struct rtw89_debugfs_priv *debugfs_priv,
@@ -4478,6 +4775,7 @@ static const struct rtw89_debugfs rtw89_debugfs_templ = {
 	.disable_dm = rtw89_debug_priv_set_and_get(disable_dm, RWLOCK),
 	.mlo_mode = rtw89_debug_priv_set_and_get(mlo_mode, RWLOCK),
 	.beacon_info = rtw89_debug_priv_get(beacon_info),
+	.diag_mac = rtw89_debug_priv_get(diag_mac, RSIZE_16K, RLOCK),
 };
 
 #define rtw89_debugfs_add(name, mode, fopname, parent)				\
@@ -4524,6 +4822,7 @@ void rtw89_debugfs_add_sec1(struct rtw89_dev *rtwdev, struct dentry *debugfs_top
 	rtw89_debugfs_add_rw(disable_dm);
 	rtw89_debugfs_add_rw(mlo_mode);
 	rtw89_debugfs_add_r(beacon_info);
+	rtw89_debugfs_add_r(diag_mac);
 }
 
 void rtw89_debugfs_init(struct rtw89_dev *rtwdev)