From 1ecc90eee785504c9671985cabd6631ff1640f64 Mon Sep 17 00:00:00 2001
From: Yi-Chia Hsieh <yi-chia.hsieh@mediatek.com>
Date: Thu, 26 Jan 2023 08:50:47 +0800
Subject: [PATCH] mt76: mt7915: add vendor cmd to get available color bitmap

Add a vendor cmd to notify user space available color bitmap.
The OBSS BSS color bitmap is maintained in mac80211, so mt76 will make use of that.

Signed-off-by: Yi-Chia Hsieh <yi-chia.hsieh@mediatek.com>
---
 mt7915/vendor.c | 37 +++++++++++++++++++++++++++++++++++++
 mt7915/vendor.h | 11 +++++++++++
 2 files changed, 48 insertions(+)

diff --git a/mt7915/vendor.c b/mt7915/vendor.c
index a1f526c..5cbe856 100644
--- a/mt7915/vendor.c
+++ b/mt7915/vendor.c
@@ -97,6 +97,11 @@ ibf_ctrl_policy[NUM_MTK_VENDOR_ATTRS_IBF_CTRL] = {
 	[MTK_VENDOR_ATTR_IBF_CTRL_ENABLE] = { .type = NLA_U8 },
 };
 
+static struct nla_policy
+bss_color_ctrl_policy[NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL] = {
+	[MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP] = { .type = NLA_U64 },
+};
+
 struct csi_null_tone {
 	u8 start;
 	u8 end;
@@ -1272,6 +1277,27 @@ mt7915_vendor_ibf_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
 	return 1;
 }
 
+static int
+mt7915_vendor_bss_color_ctrl_dump(struct wiphy *wiphy, struct wireless_dev *wdev,
+			     struct sk_buff *skb, const void *data, int data_len,
+			     unsigned long *storage)
+{
+	struct ieee80211_vif *vif = wdev_to_ieee80211_vif(wdev);
+	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	int len = 0;
+
+	if (*storage == 1)
+		return -ENOENT;
+	*storage = 1;
+
+	if (nla_put_u64_64bit(skb,
+	    MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP,
+	    ~bss_conf->he_bss_color.used_color_bitmap, NL80211_ATTR_PAD))
+		return -ENOMEM;
+	len += 1;
+
+	return len;
+}
 
 static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 	{
@@ -1378,6 +1404,17 @@ static const struct wiphy_vendor_command mt7915_vendor_commands[] = {
 		.dumpit = mt7915_vendor_ibf_ctrl_dump,
 		.policy = ibf_ctrl_policy,
 		.maxattr = MTK_VENDOR_ATTR_IBF_CTRL_MAX,
+	},
+	{
+		.info = {
+			.vendor_id = MTK_NL80211_VENDOR_ID,
+			.subcmd = MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL,
+		},
+		.flags = WIPHY_VENDOR_CMD_NEED_NETDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.dumpit = mt7915_vendor_bss_color_ctrl_dump,
+		.policy = bss_color_ctrl_policy,
+		.maxattr = MTK_VENDOR_ATTR_BSS_COLOR_CTRL_MAX,
 	}
 };
 
diff --git a/mt7915/vendor.h b/mt7915/vendor.h
index cbff910..dda1447 100644
--- a/mt7915/vendor.h
+++ b/mt7915/vendor.h
@@ -14,6 +14,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_EDCCA_CTRL = 0xc7,
 	MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL = 0xc8,
 	MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL = 0xc9,
+	MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL = 0xca,
 };
 
 
@@ -257,4 +258,14 @@ enum mtk_vendor_attr_ibf_dump {
 		NUM_MTK_VENDOR_ATTRS_IBF_DUMP - 1
 };
 
+enum mtk_vendor_attr_bss_color_ctrl {
+	MTK_VENDOR_ATTR_BSS_COLOR_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL,
+	MTK_VENDOR_ATTR_BSS_COLOR_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL - 1
+};
 #endif
-- 
2.39.0