[PATCH rtw-next 3/8] wifi: rtw89: debug: add parser to diagnose along DIAG_MAC fw element

[<pkshih@realtek.com>]

From: Ping-Ke Shih <pkshih@realtek.com>

The rules to diagnose MAC have a common header, and a cmd field is used
to know the exact command and its format. The rules with the same tuple of
fields {sheet, seq} can be seen as a set of compound rules, which treat
it as positive rule if just one of the rules is positive.

Take EQUALV rules as example, if value of {addr, mask} is equal to
predefined value as field val, a rule is positive. Fields addr_name_offset
and msg_offset are offsets related to textual messages for human readable.

Format of common rule header (8 bytes)
 +-------+-----+--------+-----------+-----+---------+
 | sheet | cmd | seq[2] | io / band | len | rsvd[2] |
 +-------+-----+--------+-----------+-----+---------+

Format of rule command is EQUALV (equal value) (24 bytes):
 +------+------------------+------+-----+------------+---------+
 | addr | addr_name_offset | mask | val | msg_offset | rsvd[4] |
 +------+------------------+------+-----+------------+---------+

Format of message:
 +-----+----------+
 | len | string[] |
 +-----+----------+

An example of output:
 Plain(Ignore)/Rules/Positive: 115(4)/86/81

 Where, Plain is total rules written in firmware element.
        Ignore is the ignored rules, such as USB IO, but current is PCIE.
        Rules is number of set of compound rules.
        Positive is number of positive Rules.

Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
---
 drivers/net/wireless/realtek/rtw89/debug.c | 299 +++++++++++++++++++++
 1 file changed, 299 insertions(+)

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)
-- 
2.25.1