From a2dbe60b46b44d3f742452f02946c78534fdee50 Mon Sep 17 00:00:00 2001
From: John Crispin <john@phrozen.org>
Date: Tue, 10 Nov 2020 10:03:35 -0800
Subject: [PATCH] nl80211: add support for BSS coloring

This patch adds support for BSS color collisions to the wireless subsystem.
Add the required functionality to nl80211 that will notify about color
collisions, triggering the color change and notifying when it is completed.

Signed-off-by: John Crispin <john@phrozen.org>
---
 include/net/cfg80211.h       |  96 +++++++++++++++++++++++++
 include/uapi/linux/nl80211.h |  46 ++++++++++++
 net/wireless/nl80211.c       | 135 +++++++++++++++++++++++++++++++++++
 net/wireless/rdev-ops.h      |  12 ++++
 net/wireless/trace.h         |  48 +++++++++++++
 5 files changed, 337 insertions(+)

--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -1277,6 +1277,27 @@ struct cfg80211_csa_settings {
 	u8 count;
 };
 
+/**
+ * struct cfg80211_color_change_settings - color change settings
+ *
+ * Used for color change
+ *
+ * @beacon_color_change: beacon data while performing the change
+ * @counter_offsets_beacon: offsets of the counters within the beacon (tail)
+ * @counter_offsets_presp: offsets of the counters within the probe response
+ * @beacon_after: beacon data to be used after the change
+ * @count: number of beacons until the change
+ * @color: the color that we will have after the change
+ */
+struct cfg80211_color_change_settings {
+	struct cfg80211_beacon_data beacon_color_change;
+	u16 counter_offset_beacon;
+	u16 counter_offset_presp;
+	struct cfg80211_beacon_data beacon_after;
+	u8 count;
+	u8 color;
+};
+
 #define CFG80211_MAX_NUM_DIFFERENT_CHANNELS 10
 
 /**
@@ -4020,6 +4041,7 @@ struct mgmt_frame_regs {
  * @set_fils_discovery: Set FILS discovery transmission parameters.
  * @set_unsol_bcast_probe_resp: Set unsolicited broadcast probe response
  * 	transmission parameters.
+ * @color_change: initiate a color change (with color change).
  */
 struct cfg80211_ops {
 	int	(*suspend)(struct wiphy *wiphy, struct cfg80211_wowlan *wow);
@@ -4353,6 +4375,9 @@ struct cfg80211_ops {
 	int	(*set_unsol_bcast_probe_resp)(struct wiphy *wiphy,
 					      struct net_device *dev,
 					      struct cfg80211_unsol_bcast_probe_resp *params);
+	int     (*color_change)(struct wiphy *wiphy,
+				struct net_device *dev,
+				struct cfg80211_color_change_settings *params);
 };
 
 /*
@@ -8247,4 +8272,75 @@ void cfg80211_update_muedca_params_event
 					 struct ieee80211_mu_edca_param_set
 					 *params, gfp_t gfp);
 
+/*
+ * cfg80211_bss_color_notify - notify about bss color event
+ * @dev: network device
+ * @gfp: allocation flags
+ * notif: the actual event we want to notify
+ * @count: the number of TBTTs until the color change happens
+ * @color_bitmap: representations of the colors that the local BSS is aware of
+ */
+
+void cfg80211_bss_color_notify(struct net_device *dev,
+			       gfp_t gfp, enum nl80211_commands notify,
+			       u8 count, u64 color_bitmap);
+
+/*
+ * cfg80211_obss_color_collision_notify - notify about bss color collisions
+ * @dev: network device
+ * @color_bitmap: representations of the colors that the local BSS is aware of
+ */
+static inline void cfg80211_obss_color_collision_notify(struct net_device *dev,
+							u64 color_bitmap)
+
+{
+	cfg80211_bss_color_notify(dev, GFP_KERNEL,
+				  NL80211_CMD_OBSS_COLOR_COLLISION,
+				  0, color_bitmap);
+}
+
+/*
+ * cfg80211_color_change_started_notify - notify color change start
+ * @dev: the device on which the color is switched
+ * @count: the number of TBTTs until the color change happens
+ *
+ * Inform the userspace about the color change that has just
+ * started.
+ */
+static inline void cfg80211_color_change_started_notify(struct net_device *dev,
+							u8 count)
+{
+	cfg80211_bss_color_notify(dev, GFP_KERNEL,
+				  NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_STARTED,
+				  count, 0);
+}
+
+/*
+ * cfg80211_color_change_aborted_notify - notify color change abort
+ * @dev: the device on which the color is switched
+ *
+ * Inform the userspace about the color change that has just
+ * started.
+ */
+static inline void cfg80211_color_change_aborted_notify(struct net_device *dev)
+{
+	cfg80211_bss_color_notify(dev, GFP_KERNEL,
+				  NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_ABORTED,
+				  0, 0);
+}
+
+/*
+ * cfg80211_color_change_notify - notify color change completion
+ * @dev: the device on which the color was switched
+ *
+ * Inform the userspace about the color change that has just
+ * completed.
+ */
+static inline void cfg80211_color_change_notify(struct net_device *dev)
+{
+	cfg80211_bss_color_notify(dev, GFP_KERNEL,
+				  NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_COMPLETED,
+				  0, 0);
+}
+
 #endif /* __NET_CFG80211_H */
--- a/include/uapi/linux/nl80211.h
+++ b/include/uapi/linux/nl80211.h
@@ -1194,6 +1194,21 @@
  *	probe response transmission parameters. Inputs to the command are
  *	provided using the attribute %NL80211_ATTR_UNSOL_BCAST_PROBE_RESP.
  *
+ * @NL80211_CMD_OBSS_COLOR_COLLISION: This notification is sent out whenever a
+ *	mac detects a bss color collision.
+ *
+ * @NL80211_CMD_COLOR_CHANGE: This command is used to indicate that we want to
+ *      change the BSS color.
+ *
+ @NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_STARTED: Notify userland, that we color change has
+ *      started
+ *
+ * @NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_ABORTED: Notify userland, that we color change has
+ *      been aborted
+ *
+ * @NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_COMPLETED: Notify userland that the color change
+ *     has completed
+ *
  * @NL80211_CMD_MAX: highest used command number
  * @__NL80211_CMD_AFTER_LAST: internal use
  */
@@ -1431,6 +1446,13 @@ enum nl80211_commands {
 	NL80211_CMD_SET_FILS_DISCOVERY,
 	NL80211_CMD_SET_UNSOL_BCAST_PROBE_RESP,
 
+	NL80211_CMD_OBSS_COLOR_COLLISION,
+
+	NL80211_CMD_COLOR_CHANGE,
+	NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_STARTED,
+	NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_ABORTED,
+	NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_COMPLETED,
+
 	/* add new commands above here */
 
 	/* used to define NL80211_CMD_MAX below */
@@ -2591,6 +2613,20 @@ enum nl80211_commands {
  * @NL80211_ATTR_MULTIPLE_BSSID_IES: The Elements that describe our multiple BSS group.
  *	these get passed separately as the kernel might need to split them up for EMA VAP.
  *
+ * @NL80211_ATTR_OBSS_COLOR_BITMAP: bitmap of the u64 BSS colors for the
+ *      %NL80211_CMD_OBSS_COLOR_COLLISION event.
+ *
+ * @NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COUNT: u8 attribute specifying the number od TBTT's
+ *      until the color switch event.
+ * @NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COLOR: u8 attribute specifying the color that we are
+ *      switching to
+ * @NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_IES: Nested set of attributes containing the IE
+ *      information for the time while performing a color switch.
+ * @NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_C_OFF_BEACON: An array of offsets (u16) to the color
+ *      switch counters in the beacons tail (%NL80211_ATTR_BEACON_TAIL).
+ * @NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_C_OFF_PRESP: An array of offsets (u16) to the color
+ switch counters in the probe response (%NL80211_ATTR_PROBE_RESP).
+ *
  * @NUM_NL80211_ATTR: total number of nl80211_attrs available
  * @NL80211_ATTR_MAX: highest attribute number currently defined
  * @__NL80211_ATTR_AFTER_LAST: internal use
@@ -3096,6 +3132,12 @@ enum nl80211_attrs {
 	NL80211_ATTR_MULTIPLE_BSSID_COUNT,
 	NL80211_ATTR_MULTIPLE_BSSID_IES,
 
+	NL80211_ATTR_OBSS_COLOR_BITMAP,
+
+	NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COUNT,
+	NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COLOR,
+	NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_IES,
+
 	/* add attributes here, update the policy in nl80211.c */
 
 	__NL80211_ATTR_AFTER_LAST,
@@ -5984,6 +6026,9 @@ enum nl80211_feature_flags {
  * @NL80211_EXT_FEATURE_WIDE_BAND_SCAN: Driver/device supports wide band scan
  *	 on a frequency along with its corresponding phymode (40Mhz, 80Mhz)
  *
+ * @NL80211_EXT_FEATURE_BSS_COLOR: The driver supports BSS color collision
+ *     detection and change announcemnts.
+ *
  * @NUM_NL80211_EXT_FEATURES: number of extended features.
  * @MAX_NL80211_EXT_FEATURES: highest extended feature index.
  */
@@ -6046,6 +6091,7 @@ enum nl80211_ext_feature_index {
 	NL80211_EXT_FEATURE_UNSOL_BCAST_PROBE_RESP,
 	NL80211_EXT_FEATURE_BEACON_RATE_HE,
 	NL80211_EXT_FEATURE_WIDE_BAND_SCAN,
+	NL80211_EXT_FEATURE_BSS_COLOR,
 
 	/* add new features before the definition below */
 	NUM_NL80211_EXT_FEATURES,
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -778,6 +778,10 @@ static const struct nla_policy nl80211_p
 	[NL80211_ATTR_MULTIPLE_BSSID_INDEX] = { .type = NLA_U8 },
 	[NL80211_ATTR_MULTIPLE_BSSID_COUNT] = { .type = NLA_U8 },
 	[NL80211_ATTR_MULTIPLE_BSSID_IES] = { .type = NLA_NESTED },
+	[NL80211_ATTR_OBSS_COLOR_BITMAP] = { .type = NLA_U64 },
+	[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COUNT] = { .type = NLA_U8 },
+	[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COLOR] = { .type = NLA_U8 },
+	[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_IES] = NLA_POLICY_NESTED(nl80211_policy),
 };
 
 /* policy for the key attributes */
@@ -14883,6 +14887,82 @@ bad_tid_conf:
 	return ret;
 }
 
+static int nl80211_color_change(struct sk_buff *skb, struct genl_info *info)
+{
+	struct cfg80211_registered_device *rdev = info->user_ptr[0];
+	struct net_device *dev = info->user_ptr[1];
+	struct wireless_dev *wdev = dev->ieee80211_ptr;
+	struct cfg80211_color_change_settings params;
+	static struct nlattr *color_change_attrs[NL80211_ATTR_MAX + 1];
+	int err, len;
+
+	if (!rdev->ops->color_change ||
+	    !(wiphy_ext_feature_isset(&rdev->wiphy, NL80211_EXT_FEATURE_BSS_COLOR)))
+		return -EOPNOTSUPP;
+
+	if (dev->ieee80211_ptr->iftype != NL80211_IFTYPE_AP)
+		return -EOPNOTSUPP;
+
+	memset(&params, 0, sizeof(params));
+
+	if (!info->attrs[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COUNT] ||
+	    !info->attrs[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COLOR] ||
+	    !info->attrs[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_IES])
+		return -EINVAL;
+
+	params.count = nla_get_u8(info->attrs[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COUNT]);
+	params.color = nla_get_u8(info->attrs[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COLOR]);
+
+	err = nl80211_parse_beacon(rdev, info->attrs, &params.beacon_after);
+	if (err)
+		return err;
+
+	err = nla_parse_nested(color_change_attrs, NL80211_ATTR_MAX,
+			       info->attrs[NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_IES],
+			       nl80211_policy, NULL);
+	if (err)
+		return err;
+
+	err = nl80211_parse_beacon(rdev, color_change_attrs, &params.beacon_color_change);
+	if (err)
+		return err;
+
+	if (!info->attrs[NL80211_ATTR_CNTDWN_OFFS_BEACON])
+		return -EINVAL;
+
+	len = nla_len(info->attrs[NL80211_ATTR_CNTDWN_OFFS_BEACON]);
+	if (len != sizeof(u16))
+		return -EINVAL;
+
+	memcpy(&params.counter_offset_beacon,
+	       nla_data(info->attrs[NL80211_ATTR_CNTDWN_OFFS_BEACON]),
+	       sizeof(u16));
+
+	if (params.counter_offset_beacon >= params.beacon_color_change.tail_len)
+		return -EINVAL;
+
+	if (params.beacon_color_change.tail[params.counter_offset_beacon] != params.count)
+		return -EINVAL;
+
+	if (info->attrs[NL80211_ATTR_CNTDWN_OFFS_PRESP]) {
+		params.counter_offset_presp =
+			nla_get_u16(info->attrs[NL80211_ATTR_CNTDWN_OFFS_PRESP]);
+
+		if (params.counter_offset_presp >= params.beacon_color_change.probe_resp_len)
+			return -EINVAL;
+
+		if (params.beacon_color_change.probe_resp[params.counter_offset_presp] !=
+		    params.count)
+			return -EINVAL;
+	}
+
+	wdev_lock(wdev);
+	err = rdev_color_change(rdev, dev, &params);
+	wdev_unlock(wdev);
+
+	return err;
+}
+
 #define NL80211_FLAG_NEED_WIPHY		0x01
 #define NL80211_FLAG_NEED_NETDEV	0x02
 #define NL80211_FLAG_NEED_RTNL		0x04
@@ -15895,6 +15975,14 @@ static const struct genl_small_ops nl802
 		.internal_flags = NL80211_FLAG_NEED_NETDEV_UP |
 				  NL80211_FLAG_NEED_RTNL,
 	},
+	{
+		.cmd = NL80211_CMD_COLOR_CHANGE,
+		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
+		.doit = nl80211_color_change,
+		.flags = GENL_UNS_ADMIN_PERM,
+		.internal_flags = NL80211_FLAG_NEED_NETDEV_UP |
+				  NL80211_FLAG_NEED_RTNL,
+	},
 };
 
 static struct genl_family nl80211_fam __genl_ro_after_init = {
@@ -17526,6 +17614,53 @@ void cfg80211_ch_switch_started_notify(s
 }
 EXPORT_SYMBOL(cfg80211_ch_switch_started_notify);
 
+void cfg80211_bss_color_notify(struct net_device *dev,
+			       gfp_t gfp, enum nl80211_commands cmd,
+			       u8 count, u64 color_bitmap)
+{
+	struct wireless_dev *wdev = dev->ieee80211_ptr;
+	struct wiphy *wiphy = wdev->wiphy;
+	struct cfg80211_registered_device *rdev = wiphy_to_rdev(wiphy);
+	struct sk_buff *msg;
+	void *hdr;
+
+	ASSERT_WDEV_LOCK(wdev);
+
+	trace_cfg80211_bss_color_notify(dev, cmd, count, color_bitmap);
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, gfp);
+	if (!msg)
+		return;
+
+	hdr = nl80211hdr_put(msg, 0, 0, 0, cmd);
+	if (!hdr) {
+		nlmsg_free(msg);
+		return;
+	}
+
+	if (nla_put_u32(msg, NL80211_ATTR_IFINDEX, dev->ifindex))
+		goto nla_put_failure;
+
+	if (cmd == NL80211_CMD_COLOR_CHANGE_ANNOUNCEMENT_STARTED &&
+	    nla_put_u32(msg, NL80211_ATTR_COLOR_CHANGE_ANNOUNCEMENT_COUNT, count))
+		goto nla_put_failure;
+
+	if (cmd == NL80211_CMD_OBSS_COLOR_COLLISION &&
+	    nla_put_u64_64bit(msg, NL80211_ATTR_OBSS_COLOR_BITMAP,
+			      color_bitmap, NL80211_ATTR_PAD))
+		goto nla_put_failure;
+
+	genlmsg_end(msg, hdr);
+
+	genlmsg_multicast_netns(&nl80211_fam, wiphy_net(&rdev->wiphy), msg, 0,
+				NL80211_MCGRP_MLME, gfp);
+	return;
+
+ nla_put_failure:
+	nlmsg_free(msg);
+}
+EXPORT_SYMBOL(cfg80211_bss_color_notify);
+
 void
 nl80211_radar_notify(struct cfg80211_registered_device *rdev,
 		     const struct cfg80211_chan_def *chandef,
--- a/net/wireless/rdev-ops.h
+++ b/net/wireless/rdev-ops.h
@@ -1382,4 +1382,16 @@ static inline int rdev_set_unsol_bcast_p
 	return ret;
 }
 
+static inline int rdev_color_change(struct cfg80211_registered_device *rdev,
+				    struct net_device *dev,
+				    struct cfg80211_color_change_settings *params)
+{
+	int ret;
+
+	trace_rdev_color_change(&rdev->wiphy, dev, params);
+	ret = rdev->ops->color_change(&rdev->wiphy, dev, params);
+	trace_rdev_return_int(&rdev->wiphy, ret);
+	return ret;
+}
+
 #endif /* __CFG80211_RDEV_OPS */
--- a/net/wireless/trace.h
+++ b/net/wireless/trace.h
@@ -3612,6 +3612,52 @@ TRACE_EVENT(rdev_set_unsol_bcast_probe_r
 		  __entry->interval, __entry->tmpl_len)
 );
 
+TRACE_EVENT(rdev_color_change,
+	TP_PROTO(struct wiphy *wiphy, struct net_device *netdev,
+		 struct cfg80211_color_change_settings *params),
+	TP_ARGS(wiphy, netdev, params),
+	TP_STRUCT__entry(
+		WIPHY_ENTRY
+		NETDEV_ENTRY
+		__field(u8, count)
+		__field(u16, bcn_ofs)
+		__field(u16, pres_ofs)
+	),
+	TP_fast_assign(
+		WIPHY_ASSIGN;
+		NETDEV_ASSIGN;
+		__entry->count = params->count;
+		__entry->bcn_ofs = params->counter_offset_beacon;
+		__entry->pres_ofs = params->counter_offset_presp;
+	),
+	TP_printk(WIPHY_PR_FMT ", " NETDEV_PR_FMT
+		  ", count: %u",
+		  WIPHY_PR_ARG, NETDEV_PR_ARG,
+		  __entry->count)
+);
+
+TRACE_EVENT(cfg80211_bss_color_notify,
+	TP_PROTO(struct net_device *netdev,
+		 enum nl80211_commands cmd,
+		 u8 count, u64 color_bitmap),
+	TP_ARGS(netdev, cmd, count, color_bitmap),
+	TP_STRUCT__entry(
+		NETDEV_ENTRY
+		__field(enum nl80211_bss_scan_width, cmd)
+		__field(u8, count)
+		__field(u64, color_bitmap)
+	),
+	TP_fast_assign(
+		NETDEV_ASSIGN;
+		__entry->cmd = cmd;
+		__entry->count = count;
+		__entry->color_bitmap = color_bitmap;
+	),
+	TP_printk(NETDEV_PR_FMT ", cmd: %x, count: %u, bitmap: %llx",
+		  NETDEV_PR_ARG, __entry->cmd, __entry->count,
+		  __entry->color_bitmap)
+);
+
 #endif /* !__RDEV_OPS_TRACE || TRACE_HEADER_MULTI_READ */
 
 #undef TRACE_INCLUDE_PATH
