mt7603: fix configuration for HT40

Signed-off-by: Felix Fietkau <nbd@nbd.name>
openwrt · Dec 14, 2017 · 00f7e60 · 00f7e60
1 parent 4ca8bba
commit 00f7e60
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 3 deletions.
diff --git a/mt7603_main.c b/mt7603_main.c
@@ -127,11 +127,13 @@ mt7603_set_channel(struct mt7603_dev *dev, struct cfg80211_chan_def *def)
 {
 	u8 *rssi_data = (u8 *) dev->mt76.eeprom.data;
 	int idx, ret;
-
 	u8 bw = MT_BW_20;
 
 	mt7603_mac_stop(dev);
 
+	if (def->width == NL80211_CHAN_WIDTH_40)
+		bw = MT_BW_40;
+
 	dev->mt76.chandef = *def;
 	mt76_rmw_field(dev, MT_AGG_BWCR, MT_AGG_BWCR_BW, bw);
 	ret = mt7603_mcu_set_channel(dev);

diff --git a/mt7603_mcu.c b/mt7603_mcu.c
@@ -470,6 +470,7 @@ static int mt7603_mcu_set_tx_power(struct mt7603_dev *dev)
 
 int mt7603_mcu_set_channel(struct mt7603_dev *dev)
 {
+	struct cfg80211_chan_def *chandef = &dev->mt76.chandef;
 	struct {
 		u8 control_chan;
 		u8 center_chan;
@@ -480,15 +481,23 @@ int mt7603_mcu_set_channel(struct mt7603_dev *dev)
 		u8 txpower[21];
 		u8 _res1[3];
 	} req = {
-		.control_chan = dev->mt76.chandef.chan->hw_value,
-		.center_chan = dev->mt76.chandef.chan->hw_value,
+		.control_chan = chandef->chan->hw_value,
+		.center_chan = chandef->chan->hw_value,
 		.bw = MT_BW_20,
 		.tx_streams = dev->tx_chains,
 		.rx_streams = dev->rx_chains,
 	};
 	struct sk_buff *skb;
 	int ret;
 
+	if (dev->mt76.chandef.width == NL80211_CHAN_WIDTH_40) {
+		req.bw = MT_BW_40;
+		if (chandef->center_freq1 > chandef->chan->center_freq)
+			req.center_chan += 2;
+		else
+			req.center_chan -= 2;
+	}
+
 	memset(req.txpower, 0xff, sizeof(req.txpower));
 	skb = mt7603_mcu_msg_alloc(&req, sizeof(req));
 	ret = mt7603_mcu_msg_send(dev, skb, MCU_EXT_CMD_CHANNEL_SWITCH, MCU_Q_SET, NULL);