From 3199ef5fa35e44f95e4acd2b6a45e14435fe5f10 Mon Sep 17 00:00:00 2001 From: Lorenzo Bianconi Date: Thu, 14 Jan 2021 16:38:55 +0100 Subject: [PATCH] mt76: mt7615: set mcu country code in mt7615_mcu_set_channel_domain() Update mcu country code running mt7615_mcu_set_channel_domain routine in mt7615_regd_notifier(). Filter out disabled channels in mt7615_mcu_set_channel_domain(). Signed-off-by: Lorenzo Bianconi Signed-off-by: Felix Fietkau --- mt7615/init.c | 9 +++++---- mt7615/mcu.c | 48 +++++++++++++++++++++++++++++++++--------------- 2 files changed, 38 insertions(+), 19 deletions(-) diff --git a/mt7615/init.c b/mt7615/init.c index 8c97ef8e..2f0b54c5 100644 --- a/mt7615/init.c +++ b/mt7615/init.c @@ -307,11 +307,12 @@ mt7615_regd_notifier(struct wiphy *wiphy, mt7615_init_txpower(dev, &mphy->sband_2g.sband); mt7615_init_txpower(dev, &mphy->sband_5g.sband); - if (!(chandef->chan->flags & IEEE80211_CHAN_RADAR)) - return; - mt7615_mutex_acquire(dev); - mt7615_dfs_init_radar_detector(phy); + + if (chandef->chan->flags & IEEE80211_CHAN_RADAR) + mt7615_dfs_init_radar_detector(phy); + mt7615_mcu_set_channel_domain(phy); + mt7615_mutex_release(dev); } diff --git a/mt7615/mcu.c b/mt7615/mcu.c index 834295bd..45581988 100644 --- a/mt7615/mcu.c +++ b/mt7615/mcu.c @@ -3041,7 +3041,7 @@ int mt7615_mcu_set_channel_domain(struct mt7615_phy *phy) struct mt76_phy *mphy = phy->mt76; struct mt7615_dev *dev = phy->dev; struct mt7615_mcu_channel_domain { - __le32 country_code; /* regulatory_request.alpha2 */ + u8 alpha2[4]; /* regulatory_request.alpha2 */ u8 bw_2g; /* BW_20_40M 0 * BW_20M 1 * BW_20_40_80M 2 @@ -3056,42 +3056,60 @@ int mt7615_mcu_set_channel_domain(struct mt7615_phy *phy) } __packed hdr = { .bw_2g = 0, .bw_5g = 3, - .n_2ch = mphy->sband_2g.sband.n_channels, - .n_5ch = mphy->sband_5g.sband.n_channels, }; struct mt7615_mcu_chan { __le16 hw_value; __le16 pad; __le32 flags; - } __packed; - int i, n_channels = hdr.n_2ch + hdr.n_5ch; - int len = sizeof(hdr) + n_channels * sizeof(struct mt7615_mcu_chan); + } __packed channel; + int len, i, n_max_channels, n_2ch = 0, n_5ch = 0; + struct ieee80211_channel *chan; struct sk_buff *skb; if (!mt7615_firmware_offload(dev)) return 0; + n_max_channels = mphy->sband_2g.sband.n_channels + + mphy->sband_5g.sband.n_channels; + len = sizeof(hdr) + n_max_channels * sizeof(channel); + skb = mt76_mcu_msg_alloc(&dev->mt76, NULL, len); if (!skb) return -ENOMEM; - skb_put_data(skb, &hdr, sizeof(hdr)); + skb_reserve(skb, sizeof(hdr)); - for (i = 0; i < n_channels; i++) { - struct ieee80211_channel *chan; - struct mt7615_mcu_chan channel; - - if (i < hdr.n_2ch) - chan = &mphy->sband_2g.sband.channels[i]; - else - chan = &mphy->sband_5g.sband.channels[i - hdr.n_2ch]; + for (i = 0; i < mphy->sband_2g.sband.n_channels; i++) { + chan = &mphy->sband_2g.sband.channels[i]; + if (chan->flags & IEEE80211_CHAN_DISABLED) + continue; channel.hw_value = cpu_to_le16(chan->hw_value); channel.flags = cpu_to_le32(chan->flags); channel.pad = 0; skb_put_data(skb, &channel, sizeof(channel)); + n_2ch++; } + for (i = 0; i < mphy->sband_5g.sband.n_channels; i++) { + chan = &mphy->sband_5g.sband.channels[i]; + if (chan->flags & IEEE80211_CHAN_DISABLED) + continue; + + channel.hw_value = cpu_to_le16(chan->hw_value); + channel.flags = cpu_to_le32(chan->flags); + channel.pad = 0; + + skb_put_data(skb, &channel, sizeof(channel)); + n_5ch++; + } + + BUILD_BUG_ON(sizeof(dev->mt76.alpha2) > sizeof(hdr.alpha2)); + memcpy(hdr.alpha2, dev->mt76.alpha2, sizeof(dev->mt76.alpha2)); + hdr.n_2ch = n_2ch; + hdr.n_5ch = n_5ch; + + memcpy(__skb_push(skb, sizeof(hdr)), &hdr, sizeof(hdr)); return mt76_mcu_skb_send_msg(&dev->mt76, skb, MCU_CMD_SET_CHAN_DOMAIN, false);