[RFC 0/2] mac80211: add support for ht_caps mcs rxmask override

@@ -2500,6 +2500,8 @@ struct cfg80211_qos_map {
  *	and returning to the base channel for communication with the AP.
  * @tdls_cancel_channel_switch: Stop channel-switching with a TDLS peer. Both
  *	peers must be on the base channel when the call completes.
+ *
+ * @set_htcap_rxmask: Override hardware capabilities for ht_caps mcs rxmask.
  */
 struct cfg80211_ops {
 	int	(*suspend)(struct wiphy *wiphy, struct cfg80211_wowlan *wow);

@@ -2765,6 +2767,8 @@ struct cfg80211_ops {
 	void	(*tdls_cancel_channel_switch)(struct wiphy *wiphy,
 					      struct net_device *dev,
 					      const u8 *addr);
+
+	int (*set_htcap_rxmask)(struct wiphy *wiphy, uint8_t *rxmask);
 };
 
 /*

@@ -3121,6 +3125,8 @@ struct wiphy_vendor_command {
  *	wiphy is theirs, e.g. in global notifiers
  * @bands: information about bands/channels supported by this device
  *
+ * @init_bands: save of the originals information about bands.
+ *
  * @mgmt_stypes: bitmasks of frame subtypes that can be subscribed to or
  *	transmitted through nl80211, points to an array indexed by interface
  *	type

@@ -3266,6 +3272,7 @@ struct wiphy {
 	const void *privid;
 
 	struct ieee80211_supported_band *bands[IEEE80211_NUM_BANDS];
+	struct ieee80211_supported_band *init_bands[IEEE80211_NUM_BANDS];
 
 	/* Lets us get back the wiphy on the callback */
 	void (*reg_notifier)(struct wiphy *wiphy,

@@ -3350,6 +3350,53 @@ static int ieee80211_del_tx_ts(struct wiphy *wiphy, struct net_device *dev,
 	return -ENOENT;
 }
 
+static int ieee80211_set_htcap_rxmask(struct wiphy *wiphy, uint8_t *rxmask)
+{
+	struct ieee80211_local *local = wiphy_priv(wiphy);
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_supported_band *iband;
+	int blank = 1;
+	int empty;
+	int i;
+
+	if (ieee80211_hw_check(&local->hw, HAS_RATE_CONTROL))
+		return -EINVAL;
+
+	mutex_lock(&local->iflist_mtx);
+	empty = list_empty(&local->interfaces);
+	mutex_unlock(&local->iflist_mtx);
+
+	if (!empty)
+		return -EBUSY;
+
+	for (i = 0; i < IEEE80211_HT_MCS_MASK_LEN; i++)
+		if (rxmask[i]) {
+			blank = 0;
+			break;
+		}
+
+	for (i = 0; i < IEEE80211_NUM_BANDS; i++) {
+		int j;
+
+		sband = wiphy->bands[i];
+		iband = wiphy->init_bands[i];
+
+		if (!iband)
+			continue;
+
+		for (j = 0; j < IEEE80211_HT_MCS_MASK_LEN; j++) {
+			if (blank)
+				sband->ht_cap.mcs.rx_mask[j] =
+						iband->ht_cap.mcs.rx_mask[j];
+			else
+				sband->ht_cap.mcs.rx_mask[j] = rxmask[j] &
+						iband->ht_cap.mcs.rx_mask[j];
+		}
+	}
+
+	return 0;
+}
+
 const struct cfg80211_ops mac80211_config_ops = {
 	.add_virtual_intf = ieee80211_add_iface,
 	.del_virtual_intf = ieee80211_del_iface,

@@ -3435,4 +3482,5 @@ const struct cfg80211_ops mac80211_config_ops = {
 	.set_ap_chanwidth = ieee80211_set_ap_chanwidth,
 	.add_tx_ts = ieee80211_add_tx_ts,
 	.del_tx_ts = ieee80211_del_tx_ts,
+	.set_htcap_rxmask = ieee80211_set_htcap_rxmask,
 };

@@ -789,15 +789,37 @@ static int ieee80211_init_cipher_suites(struct ieee80211_local *local)
 	return 0;
 }
 
+static int ieee80211_alloc_init_bands(struct wiphy *wiphy)
+{
+	int i;
+
+	memset(wiphy->init_bands, 0, IEEE80211_NUM_BANDS);
+	for (i = 0; i < IEEE80211_NUM_BANDS; i++) {
+		if (!wiphy->bands[i])
+			continue;
+
+		wiphy->init_bands[i] = kzalloc(sizeof(*wiphy->init_bands[i]),
+					       GFP_KERNEL);
+		if (!wiphy->init_bands[i])
+			return -ENOMEM;
+
+		memcpy(wiphy->init_bands[i], wiphy->bands[i],
+		       sizeof(struct ieee80211_supported_band));
+	}
+
+	return 0;
+}
+
 int ieee80211_register_hw(struct ieee80211_hw *hw)
 {
 	struct ieee80211_local *local = hw_to_local(hw);
-	int result, i;
-	enum ieee80211_band band;
+	struct cfg80211_chan_def dflt_chandef = {};
+	netdev_features_t feature_whitelist;
 	int channels, max_bitrates;
+	enum ieee80211_band band;
 	bool supp_ht, supp_vht;
-	netdev_features_t feature_whitelist;
-	struct cfg80211_chan_def dflt_chandef = {};
+	int result = 0;
+	int i;
 
 	if (ieee80211_hw_check(hw, QUEUE_CONTROL) &&
 	    (local->hw.offchannel_tx_hw_queue == IEEE80211_INVAL_HW_QUEUE ||

@@ -857,6 +879,11 @@ int ieee80211_register_hw(struct ieee80211_hw *hw)
 
 	local->rx_chains = 1;
 
+	if (ieee80211_alloc_init_bands(hw->wiphy)) {
+		result = -ENOMEM;
+		goto fail_sband_init;
+	}
+
 	/*
 	 * generic code guarantees at least one band,
 	 * set this very early because much code assumes

@@ -919,14 +946,18 @@ int ieee80211_register_hw(struct ieee80211_hw *hw)
 
 		for (j = 0; j < c->n_limits; j++)
 			if ((c->limits[j].types & BIT(NL80211_IFTYPE_ADHOC)) &&
-			    c->limits[j].max > 1)
-				return -EINVAL;
+			    c->limits[j].max > 1) {
+				result = -EINVAL;
+				goto fail_sband_init;
+			}
 	}
 
 	local->int_scan_req = kzalloc(sizeof(*local->int_scan_req) +
 				      sizeof(void *) * channels, GFP_KERNEL);
-	if (!local->int_scan_req)
-		return -ENOMEM;
+	if (!local->int_scan_req) {
+		result = -ENOMEM;
+		goto fail_sband_init;
+	}
 
 	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
 		if (!local->hw.wiphy->bands[band])

@@ -1120,6 +1151,9 @@ int ieee80211_register_hw(struct ieee80211_hw *hw)
 	if (local->wiphy_ciphers_allocated)
 		kfree(local->hw.wiphy->cipher_suites);
 	kfree(local->int_scan_req);
+ fail_sband_init:
+	for (i = 0; i < IEEE80211_NUM_BANDS; i++)
+		kfree(hw->wiphy->init_bands[i]);
 	return result;
 }
 EXPORT_SYMBOL(ieee80211_register_hw);

@@ -1127,6 +1161,7 @@ EXPORT_SYMBOL(ieee80211_register_hw);
 void ieee80211_unregister_hw(struct ieee80211_hw *hw)
 {
 	struct ieee80211_local *local = hw_to_local(hw);
+	int i;
 
 	tasklet_kill(&local->tx_pending_tasklet);
 	tasklet_kill(&local->tasklet);

@@ -1170,6 +1205,8 @@ void ieee80211_unregister_hw(struct ieee80211_hw *hw)
 	ieee80211_wep_free(local);
 	ieee80211_led_exit(local);
 	kfree(local->int_scan_req);
+	for (i = 0; i < IEEE80211_NUM_BANDS; i++)
+		kfree(hw->wiphy->init_bands[i]);
 }
 EXPORT_SYMBOL(ieee80211_unregister_hw);
 

@@ -1794,6 +1794,9 @@ enum nl80211_commands {
  *	connecting to a PCP, and in %NL80211_CMD_START_AP to start
  *	a PCP instead of AP. Relevant for DMG networks only.
  *
+ * @NL80211_ATTR_WIPHY_HTCAP_RXMASK: Override hardware capabilities for ht_caps
+ *	mcs rxmask.
+ *
  * @NUM_NL80211_ATTR: total number of nl80211_attrs available
  * @NL80211_ATTR_MAX: highest attribute number currently defined
  * @__NL80211_ATTR_AFTER_LAST: internal use

@@ -2170,6 +2173,8 @@ enum nl80211_attrs {
 
 	NL80211_ATTR_PBSS,
 
+	NL80211_ATTR_WIPHY_HTCAP_RXMASK,
+
 	/* add attributes here, update the policy in nl80211.c */
 
 	__NL80211_ATTR_AFTER_LAST,

@@ -402,6 +402,9 @@ static const struct nla_policy nl80211_policy[NUM_NL80211_ATTR] = {
 	[NL80211_ATTR_SCHED_SCAN_DELAY] = { .type = NLA_U32 },
 	[NL80211_ATTR_REG_INDOOR] = { .type = NLA_FLAG },
 	[NL80211_ATTR_PBSS] = { .type = NLA_FLAG },
+	[NL80211_ATTR_WIPHY_HTCAP_RXMASK] = { .type = NLA_BINARY,
+					      .len = IEEE80211_HT_MCS_MASK_LEN
+	},
 };
 
 /* policy for the key attributes */

@@ -2243,6 +2246,21 @@ static int nl80211_set_wiphy(struct sk_buff *skb, struct genl_info *info)
 			return result;
 	}
 
+	if (info->attrs[NL80211_ATTR_WIPHY_HTCAP_RXMASK]) {
+		u8 rxmask[IEEE80211_HT_MCS_MASK_LEN];
+
+		if (wdev)
+			return -EOPNOTSUPP;
+
+		memcpy(rxmask,
+		       nla_data(info->attrs[NL80211_ATTR_WIPHY_HTCAP_RXMASK]),
+		       IEEE80211_HT_MCS_MASK_LEN);
+
+		result = rdev_set_htcap_rxmask(rdev, rxmask);
+		if (result)
+			return result;
+	}
+
 	changed = 0;
 
 	if (info->attrs[NL80211_ATTR_WIPHY_RETRY_SHORT]) {

@@ -1071,4 +1071,15 @@ rdev_set_coalesce(struct cfg80211_registered_device *rdev,
 	trace_rdev_return_int(&rdev->wiphy, ret);
 	return ret;
 }
+
+static inline int
+rdev_set_htcap_rxmask(struct cfg80211_registered_device *rdev, uint8_t *rxmask)
+{
+	int ret;
+
+	trace_rdev_set_htcap_rxmask(&rdev->wiphy, rxmask);
+	ret = rdev->ops->set_htcap_rxmask(&rdev->wiphy, rxmask);
+	trace_rdev_return_int(&rdev->wiphy, ret);
+	return ret;
+}
 #endif /* __CFG80211_RDEV_OPS */

@@ -2120,6 +2120,21 @@ TRACE_EVENT(rdev_tdls_cancel_channel_switch,
 		  WIPHY_PR_ARG, NETDEV_PR_ARG, MAC_PR_ARG(addr))
 );
 
+TRACE_EVENT(rdev_set_htcap_rxmask,
+	TP_PROTO(struct wiphy *wiphy, uint8_t *rxmask),
+	TP_ARGS(wiphy, rxmask),
+	TP_STRUCT__entry(
+		WIPHY_ENTRY
+		__array(uint8_t, rxmask, IEEE80211_HT_MCS_MASK_LEN)
+	),
+	TP_fast_assign(
+		WIPHY_ASSIGN;
+		memcpy(__entry->rxmask, rxmask, IEEE80211_HT_MCS_MASK_LEN);
+	),
+	TP_printk(WIPHY_PR_FMT ", %*ph",
+		  WIPHY_PR_ARG, IEEE80211_HT_MCS_MASK_LEN, &__entry->rxmask[0])
+);
+
 /*************************************************************
  *	     cfg80211 exported functions traces		     *
  *************************************************************/