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 <lorenzo@kernel.org>
Signed-off-by: Felix Fietkau <nbd@nbd.name>

diff --git a/drivers/net/wireless/mediatek/mt76/mt76.h b/drivers/net/wireless/mediatek/mt76/mt76.h
index 598c09b..7a7776f 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt76.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76.h
@@ -656,6 +656,7 @@ struct mt76_dev {
 
        struct mt76_rate_power rate_power;
 
+       char alpha2[3];
        enum nl80211_dfs_regions region;
 
        u32 debugfs_reg;

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/init.c b/drivers/net/wireless/mediatek/mt76/mt7615/init.c
index 8151c1d..7e81ec5 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7615/init.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/init.c
@@ -296,13 +296,15 @@ mt7615_regd_notifier(struct wiphy *wiphy,
        struct mt7615_phy *phy = mphy->priv;
        struct cfg80211_chan_def *chandef = &mphy->chandef;
 
+       memcpy(dev->mt76.alpha2, request->alpha2, sizeof(dev->mt76.alpha2));
        dev->mt76.region = request->dfs_region;
 
-       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/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
index c1cd7d5..a035bfa 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
@@ -2982,7 +2982,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
@@ -2997,43 +2997,61 @@ 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;
+       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;
 
-               if (i < hdr.n_2ch)
-                       chan = &mphy->sband_2g.sband.channels[i];
-               else
-                       chan = &mphy->sband_5g.sband.channels[i - hdr.n_2ch];
+               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);
 }