From 2722263634d9890cfd956d924341c2239e1507b1 Mon Sep 17 00:00:00 2001
From: Bo Jiao <Bo.Jiao@mediatek.com>
Date: Tue, 11 Jan 2022 12:03:23 +0800
Subject: [PATCH 1003/1007] mt76: mt7915: air monitor support

---
 mt76_connac_mcu.h |   2 +
 mt7915/mac.c      |   4 +
 mt7915/main.c     |   3 +
 mt7915/mt7915.h   |  34 +++++
 mt7915/vendor.c   | 359 ++++++++++++++++++++++++++++++++++++++++++++++
 mt7915/vendor.h   |  38 +++++
 6 files changed, 440 insertions(+)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 96591a8c..1c24dbf4 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -991,6 +991,8 @@ enum {
 	MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
 	MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
 	MCU_EXT_CMD_PHY_STAT_INFO = 0xad,
+	/* for vendor csi and air monitor */
+	MCU_EXT_CMD_SMESH_CTRL = 0xae,
 	MCU_EXT_CMD_CSI_CTRL = 0xc2,
 };
 
diff --git a/mt7915/mac.c b/mt7915/mac.c
index 9e92b32f..4989f437 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -835,6 +835,10 @@ mt7915_mac_fill_rx(struct mt7915_dev *dev, struct sk_buff *skb)
 			seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
 			qos_ctl = *ieee80211_get_qos_ctl(hdr);
 		}
+#ifdef CONFIG_MTK_VENDOR
+		if (phy->amnt_ctrl.enable)
+			mt7915_vendor_amnt_fill_rx(phy, skb);
+#endif
 	} else {
 		status->flag |= RX_FLAG_8023;
 	}
diff --git a/mt7915/main.c b/mt7915/main.c
index 79349817..7d4e7838 100644
--- a/mt7915/main.c
+++ b/mt7915/main.c
@@ -671,6 +671,9 @@ int mt7915_mac_sta_add(struct mt76_dev *mdev, struct ieee80211_vif *vif,
 	if (ret)
 		return ret;
 
+#ifdef CONFIG_MTK_VENDOR
+	mt7915_vendor_amnt_sta_remove(mvif->phy, sta);
+#endif
 	return mt7915_mcu_add_rate_ctrl(dev, vif, sta, false);
 }
 
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index ff957425..ace01e15 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -232,6 +232,35 @@ struct mt7915_hif {
 	int irq;
 };
 
+#ifdef CONFIG_MTK_VENDOR
+#define MT7915_AIR_MONITOR_MAX_ENTRY	16
+#define MT7915_AIR_MONITOR_MAX_GROUP	MT7915_AIR_MONITOR_MAX_ENTRY >> 2
+
+struct mt7915_air_monitor_group {
+	bool enable;
+	bool used[2];
+};
+
+struct mt7915_air_monitor_entry {
+	bool enable;
+
+	u8 group_idx;
+	u8 group_used_idx;
+	u8 muar_idx;
+	u8 addr[ETH_ALEN];
+	unsigned int last_seen;
+	s8 rssi[4];
+	struct ieee80211_sta *sta;
+};
+
+struct mt7915_air_monitor_ctrl {
+	u8 enable;
+
+	struct mt7915_air_monitor_group group[MT7915_AIR_MONITOR_MAX_GROUP];
+	struct mt7915_air_monitor_entry entry[MT7915_AIR_MONITOR_MAX_ENTRY];
+};
+#endif
+
 struct mt7915_phy {
 	struct mt76_phy *mt76;
 	struct mt7915_dev *dev;
@@ -290,6 +319,8 @@ struct mt7915_phy {
 		u32 interval;
 		u32 last_record;
 	} csi;
+
+	struct mt7915_air_monitor_ctrl amnt_ctrl;
 #endif
 };
 
@@ -648,6 +679,9 @@ void mt7915_sta_add_debugfs(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 void mt7915_vendor_register(struct mt7915_phy *phy);
 int mt7915_mcu_set_csi(struct mt7915_phy *phy, u8 mode,
 			u8 cfg, u8 v1, u32 v2, u8 *mac_addr);
+void mt7915_vendor_amnt_fill_rx(struct mt7915_phy *phy, struct sk_buff *skb);
+int mt7915_vendor_amnt_sta_remove(struct mt7915_phy *phy,
+				  struct ieee80211_sta *sta);
 #endif
 
 #ifdef MTK_DEBUG
diff --git a/mt7915/vendor.c b/mt7915/vendor.c
index 98fd9c2d..b94d787e 100644
--- a/mt7915/vendor.c
+++ b/mt7915/vendor.c
@@ -430,6 +430,353 @@ out:
 	return err;
 }
 
+static const struct nla_policy
+amnt_ctrl_policy[NUM_MTK_VENDOR_ATTRS_AMNT_CTRL] = {
+	[MTK_VENDOR_ATTR_AMNT_CTRL_SET] = {.type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_AMNT_CTRL_DUMP] = { .type = NLA_NESTED },
+};
+
+static const struct nla_policy
+amnt_set_policy[NUM_MTK_VENDOR_ATTRS_AMNT_SET] = {
+	[MTK_VENDOR_ATTR_AMNT_SET_INDEX] = {.type = NLA_U8 },
+	[MTK_VENDOR_ATTR_AMNT_SET_MACADDR] = { .type = NLA_NESTED },
+};
+
+static const struct nla_policy
+amnt_dump_policy[NUM_MTK_VENDOR_ATTRS_AMNT_DUMP] = {
+	[MTK_VENDOR_ATTR_AMNT_DUMP_INDEX] = {.type = NLA_U8 },
+	[MTK_VENDOR_ATTR_AMNT_DUMP_LEN] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_AMNT_DUMP_RESULT] = { .type = NLA_NESTED },
+};
+
+struct mt7915_amnt_data {
+	u8 idx;
+	u8 addr[ETH_ALEN];
+	s8 rssi[4];
+	u32 last_seen;
+};
+
+struct mt7915_smesh {
+	u8 band;
+	u8 write;
+	u8 enable;
+	bool a2;
+	bool a1;
+	bool data;
+	bool mgnt;
+	bool ctrl;
+} __packed;
+
+struct mt7915_smesh_event {
+	u8 band;
+	__le32 value;
+} __packed;
+
+static int
+mt7915_vendor_smesh_ctrl(struct mt7915_phy *phy, u8 write,
+			 u8 enable, u32 *value)
+{
+	struct mt7915_dev *dev = phy->dev;
+	struct mt7915_smesh req = {
+		.band = phy != &dev->phy,
+		.write = write,
+		.enable = enable,
+		.a2 = 1,
+		.a1 = 1,
+		.data = 1,
+	};
+	struct mt7915_smesh_event *res;
+	struct sk_buff *skb;
+	int ret = 0;
+
+	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_EXT_CMD(SMESH_CTRL),
+					&req, sizeof(req), !write, &skb);
+
+	if (ret || write)
+		return ret;
+
+	res = (struct mt7915_smesh_event *) skb->data;
+
+	if (!value)
+		return -EINVAL;
+
+	*value = res->value;
+
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+static int
+mt7915_vendor_amnt_muar(struct mt7915_phy *phy, u8 muar_idx, u8 *addr)
+{
+	struct mt7915_dev *dev = phy->dev;
+	struct {
+		u8 mode;
+		u8 force_clear;
+		u8 clear_bitmap[8];
+		u8 entry_count;
+		u8 write;
+		u8 band;
+
+		u8 index;
+		u8 bssid;
+		u8 addr[ETH_ALEN];
+	} __packed req = {
+		.entry_count = 1,
+		.write = 1,
+		.band = phy != &dev->phy,
+		.index = muar_idx,
+	};
+
+	ether_addr_copy(req.addr, addr);
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(MUAR_UPDATE), &req,
+				 sizeof(req), true);
+}
+
+static int
+mt7915_vendor_amnt_set_en(struct mt7915_phy *phy, u8 enable)
+{
+	u32 status;
+	int ret;
+
+	ret = mt7915_vendor_smesh_ctrl(phy, 0, enable, &status);
+	if (ret)
+		return ret;
+
+	status = status & 0xff;
+
+	if (status == enable)
+		return 0;
+
+	ret = mt7915_vendor_smesh_ctrl(phy, 1, enable, &status);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int
+mt7915_vendor_amnt_set_addr(struct mt7915_phy *phy, u8 index, u8 *addr)
+{
+	struct mt7915_air_monitor_ctrl *amnt_ctrl = &phy->amnt_ctrl;
+	struct mt7915_air_monitor_group *group;
+	struct mt7915_air_monitor_entry *entry = &amnt_ctrl->entry[index];
+	const u8 zero_addr[ETH_ALEN] = {};
+	int enable = !ether_addr_equal(addr, zero_addr);
+	int ret, i, j;
+
+	if (enable == 1 && entry->enable == 1) {
+		ether_addr_copy(entry->addr, addr);
+	} else if (enable == 1 && entry->enable == 0){
+		for (i = 0; i < MT7915_AIR_MONITOR_MAX_GROUP; i++) {
+			group = &(amnt_ctrl->group[i]);
+			if (group->used[0] == 0)
+				j = 0;
+			else
+				j = 1;
+
+			group->enable = 1;
+			group->used[j] = 1;
+			entry->enable = 1;
+			entry->group_idx = i;
+			entry->group_used_idx = j;
+			entry->muar_idx = 32 + 2 * i + 2 * i + 2 * j;
+			ether_addr_copy(entry->addr, addr);
+			break;
+		}
+	} else {
+		group = &(amnt_ctrl->group[entry->group_idx]);
+
+		group->used[entry->group_used_idx] = 0;
+		if (group->used[0] == 0 && group->used[1] == 0)
+			group->enable = 0;
+
+		entry->enable = 0;
+		ether_addr_copy(entry->addr, addr);
+	}
+
+	amnt_ctrl->enable &= ~(1 << entry->group_idx);
+	amnt_ctrl->enable |= entry->enable << entry->group_idx;
+	ret = mt7915_vendor_amnt_muar(phy, entry->muar_idx, addr);
+	if (ret)
+		return ret;
+
+	return mt7915_vendor_amnt_set_en(phy, amnt_ctrl->enable);
+}
+
+void mt7915_vendor_amnt_fill_rx(struct mt7915_phy *phy, struct sk_buff *skb)
+{
+	struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
+	struct mt7915_air_monitor_ctrl *ctrl = &phy->amnt_ctrl;
+	struct ieee80211_hdr *hdr = mt76_skb_get_hdr(skb);
+	__le16 fc = hdr->frame_control;
+	u8 addr[ETH_ALEN];
+	int i;
+
+	if (!ieee80211_has_fromds(fc))
+		ether_addr_copy(addr, hdr->addr2);
+	else if (ieee80211_has_tods(fc))
+		ether_addr_copy(addr, hdr->addr4);
+	else
+		ether_addr_copy(addr, hdr->addr3);
+
+	for (i = 0; i < MT7915_AIR_MONITOR_MAX_ENTRY; i++) {
+		struct mt7915_air_monitor_entry *entry;
+
+		if (ether_addr_equal(addr, ctrl->entry[i].addr)) {
+			entry = &ctrl->entry[i];
+			entry->rssi[0] = status->chain_signal[0];
+			entry->rssi[1] = status->chain_signal[1];
+			entry->rssi[2] = status->chain_signal[2];
+			entry->rssi[3] = status->chain_signal[3];
+			entry->last_seen = jiffies;
+		}
+	}
+
+	if (ieee80211_has_tods(fc) &&
+	    !ether_addr_equal(hdr->addr3, phy->mt76->macaddr))
+		return;
+	else if (!ether_addr_equal(hdr->addr1, phy->mt76->macaddr))
+		return;
+}
+
+int mt7915_vendor_amnt_sta_remove(struct mt7915_phy *phy,
+				  struct ieee80211_sta *sta)
+{
+	u8 zero[ETH_ALEN] = {};
+	int i;
+
+	if (!phy->amnt_ctrl.enable)
+		return 0;
+
+	for (i = 0; i < MT7915_AIR_MONITOR_MAX_ENTRY; i++)
+		if (ether_addr_equal(sta->addr, phy->amnt_ctrl.entry[i].addr))
+			return mt7915_vendor_amnt_set_addr(phy, i, zero);
+
+	return 0;
+}
+
+static int
+mt7915_vendor_amnt_ctrl(struct wiphy *wiphy, struct wireless_dev *wdev,
+			const void *data, int data_len)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct mt7915_phy *phy = mt7915_hw_phy(hw);
+	struct nlattr *tb1[NUM_MTK_VENDOR_ATTRS_AMNT_CTRL];
+	struct nlattr *tb2[NUM_MTK_VENDOR_ATTRS_AMNT_SET];
+	struct nlattr *cur;
+	u8 index = 0, i = 0;
+	u8 mac_addr[ETH_ALEN] = {};
+	int err, rem;
+
+	err = nla_parse(tb1, MTK_VENDOR_ATTR_AMNT_CTRL_MAX, data, data_len,
+			amnt_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (!tb1[MTK_VENDOR_ATTR_AMNT_CTRL_SET])
+		return -EINVAL;
+
+	err = nla_parse_nested(tb2, MTK_VENDOR_ATTR_AMNT_SET_MAX,
+		tb1[MTK_VENDOR_ATTR_AMNT_CTRL_SET], amnt_set_policy, NULL);
+
+	if (!tb2[MTK_VENDOR_ATTR_AMNT_SET_INDEX] ||
+		!tb2[MTK_VENDOR_ATTR_AMNT_SET_MACADDR])
+		return -EINVAL;
+
+	index = nla_get_u8(tb2[MTK_VENDOR_ATTR_AMNT_SET_INDEX]);
+	nla_for_each_nested(cur, tb2[MTK_VENDOR_ATTR_AMNT_SET_MACADDR], rem) {
+		mac_addr[i++] = nla_get_u8(cur);
+	}
+
+	return mt7915_vendor_amnt_set_addr(phy, index, mac_addr);
+}
+
+static int
+mt7915_amnt_dump(struct mt7915_phy *phy, struct sk_buff *skb,
+		 u8 amnt_idx, int *attrtype)
+{
+	struct mt7915_air_monitor_entry *entry =
+			&phy->amnt_ctrl.entry[amnt_idx];
+	struct mt7915_amnt_data data;
+	u32 last_seen = 0;
+
+	if (entry->enable == 0)
+		return 0;
+
+	last_seen = jiffies_to_msecs(jiffies - entry->last_seen);
+
+	data.idx = amnt_idx;
+	ether_addr_copy(data.addr, entry->addr);
+	data.rssi[0] = entry->rssi[0];
+	data.rssi[1] = entry->rssi[1];
+	data.rssi[2] = entry->rssi[2];
+	data.rssi[3] = entry->rssi[3];
+	data.last_seen = last_seen;
+
+	nla_put(skb, (*attrtype)++, sizeof(struct mt7915_amnt_data), &data);
+
+	return 1;
+}
+
+static int
+mt7915_vendor_amnt_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
+			     struct sk_buff *skb, const void *data, int data_len,
+			     unsigned long *storage)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct mt7915_phy *phy = mt7915_hw_phy(hw);
+	struct nlattr *tb1[NUM_MTK_VENDOR_ATTRS_AMNT_CTRL];
+	struct nlattr *tb2[NUM_MTK_VENDOR_ATTRS_AMNT_DUMP];
+	void *a, *b;
+	int err = 0, attrtype = 0, i, len = 0;
+	u8 amnt_idx;
+
+	if (*storage == 1)
+		return -ENOENT;
+	*storage = 1;
+
+	err = nla_parse(tb1, MTK_VENDOR_ATTR_AMNT_CTRL_MAX, data, data_len,
+			amnt_ctrl_policy, NULL);
+	if (err)
+		return err;
+
+	if (!tb1[MTK_VENDOR_ATTR_AMNT_CTRL_DUMP])
+		return -EINVAL;
+
+	err = nla_parse_nested(tb2, MTK_VENDOR_ATTR_AMNT_DUMP_MAX,
+			       tb1[MTK_VENDOR_ATTR_AMNT_CTRL_DUMP],
+			       amnt_dump_policy, NULL);
+	if (err)
+		return err;
+
+	if (!tb2[MTK_VENDOR_ATTR_AMNT_DUMP_INDEX])
+		return -EINVAL;
+
+	amnt_idx = nla_get_u8(tb2[MTK_VENDOR_ATTR_AMNT_DUMP_INDEX]);
+
+	a = nla_nest_start(skb, MTK_VENDOR_ATTR_AMNT_CTRL_DUMP);
+	b = nla_nest_start(skb, MTK_VENDOR_ATTR_AMNT_DUMP_RESULT);
+
+	if (amnt_idx != 0xff) {
+		len += mt7915_amnt_dump(phy, skb, amnt_idx, &attrtype);
+	} else {
+		for (i = 0; i < MT7915_AIR_MONITOR_MAX_ENTRY; i++) {
+			len += mt7915_amnt_dump(phy, skb, i, &attrtype);
+		}
+	}
+
+	nla_nest_end(skb, b);
+
+	nla_put_u8(skb, MTK_VENDOR_ATTR_AMNT_DUMP_LEN, len);
+
+	nla_nest_end(skb, a);
+
+	return len + 1;
+}
+
 static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 	{
 		.info = {
@@ -442,6 +789,18 @@ static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 		.dumpit = mt7915_vendor_csi_ctrl_dump,
 		.policy = csi_ctrl_policy,
 		.maxattr = MTK_VENDOR_ATTR_CSI_CTRL_MAX,
+	},
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_AMNT_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = mt7915_vendor_amnt_ctrl,
+		.dumpit = mt7915_vendor_amnt_ctrl_dump,
+		.policy = amnt_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_AMNT_CTRL_MAX,
 	}
 };
 
diff --git a/mt7915/vendor.h b/mt7915/vendor.h
index 9d3db2a7..976817f3 100644
--- a/mt7915/vendor.h
+++ b/mt7915/vendor.h
@@ -4,6 +4,7 @@
 #define MTK_NL80211_VENDOR_ID	0x0ce7
 
 enum mtk_nl80211_vendor_subcmds {
+	MTK_NL80211_VENDOR_SUBCMD_AMNT_CTRL = 0xae,
 	MTK_NL80211_VENDOR_SUBCMD_CSI_CTRL = 0xc2,
 };
 
@@ -57,4 +58,41 @@ enum mtk_vendor_attr_csi_data {
 		NUM_MTK_VENDOR_ATTRS_CSI_DATA - 1
 };
 
+enum mtk_vendor_attr_mnt_ctrl {
+	MTK_VENDOR_ATTR_AMNT_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_AMNT_CTRL_SET,
+	MTK_VENDOR_ATTR_AMNT_CTRL_DUMP,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_AMNT_CTRL,
+	MTK_VENDOR_ATTR_AMNT_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_AMNT_CTRL - 1
+};
+
+enum mtk_vendor_attr_mnt_set {
+	MTK_VENDOR_ATTR_AMNT_SET_UNSPEC,
+
+	MTK_VENDOR_ATTR_AMNT_SET_INDEX,
+	MTK_VENDOR_ATTR_AMNT_SET_MACADDR,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_AMNT_SET,
+	MTK_VENDOR_ATTR_AMNT_SET_MAX =
+		NUM_MTK_VENDOR_ATTRS_AMNT_SET - 1
+};
+
+enum mtk_vendor_attr_mnt_dump {
+	MTK_VENDOR_ATTR_AMNT_DUMP_UNSPEC,
+
+	MTK_VENDOR_ATTR_AMNT_DUMP_INDEX,
+	MTK_VENDOR_ATTR_AMNT_DUMP_LEN,
+	MTK_VENDOR_ATTR_AMNT_DUMP_RESULT,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_AMNT_DUMP,
+	MTK_VENDOR_ATTR_AMNT_DUMP_MAX =
+		NUM_MTK_VENDOR_ATTRS_AMNT_DUMP - 1
+};
+
 #endif
-- 
2.18.0