From 6203d46fcc4577065209ea0ed9334d89df4f63f7 Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@nbd.name>
Date: Thu, 6 Dec 2018 14:38:03 +0100
Subject: [PATCH] mt7603: add support for setting transmit power

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 mt7603/init.c   |  7 ++++++-
 mt7603/main.c   | 11 +++--------
 mt7603/mcu.c    | 14 +++++++++++++-
 mt7603/mt7603.h |  2 ++
 4 files changed, 24 insertions(+), 10 deletions(-)

diff --git a/mt7603/init.c b/mt7603/init.c
index c7e7d9b4..b6c2832c 100644
--- a/mt7603/init.c
+++ b/mt7603/init.c
@@ -503,7 +503,12 @@ mt7603_init_txpower(struct mt7603_dev *dev,
 		max_offset = max(max_offset, cur_offset);
 	}
 
-	target_power = DIV_ROUND_UP(target_power + max_offset, 2);
+	target_power += max_offset;
+
+	dev->tx_power_limit = target_power;
+	dev->mt76.txpower_cur = target_power;
+
+	target_power = DIV_ROUND_UP(target_power, 2);
 
 	/* add 3 dBm for 2SS devices (combined output) */
 	if (dev->mt76.antenna_mask & BIT(1))
diff --git a/mt7603/main.c b/mt7603/main.c
index 14112ce7..7cfe212a 100644
--- a/mt7603/main.c
+++ b/mt7603/main.c
@@ -219,7 +219,8 @@ mt7603_config(struct ieee80211_hw *hw, u32 changed)
 	struct mt7603_dev *dev = hw->priv;
 	int ret = 0;
 
-	if (changed & IEEE80211_CONF_CHANGE_CHANNEL)
+	if (changed & (IEEE80211_CONF_CHANGE_CHANNEL |
+		       IEEE80211_CONF_CHANGE_POWER))
 		ret = mt7603_set_channel(dev, &hw->conf.chandef);
 
 	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
@@ -541,12 +542,6 @@ mt7603_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 {
 }
 
-static int
-mt7603_get_txpower(struct ieee80211_hw *hw, struct ieee80211_vif *vif, int *dbm)
-{
-	return -EINVAL;
-}
-
 static int
 mt7603_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		    struct ieee80211_ampdu_params *params)
@@ -685,7 +680,7 @@ const struct ieee80211_ops mt7603_ops = {
 	.sw_scan_complete = mt7603_sw_scan_complete,
 	.flush = mt7603_flush,
 	.ampdu_action = mt7603_ampdu_action,
-	.get_txpower = mt7603_get_txpower,
+	.get_txpower = mt76_get_txpower,
 	.wake_tx_queue = mt76_wake_tx_queue,
 	.sta_rate_tbl_update = mt7603_sta_rate_tbl_update,
 	.release_buffered_frames = mt7603_release_buffered_frames,
diff --git a/mt7603/mcu.c b/mt7603/mcu.c
index 14ed7221..b611df7c 100644
--- a/mt7603/mcu.c
+++ b/mt7603/mcu.c
@@ -481,6 +481,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 ieee80211_hw *hw = mt76_hw(dev);
 	int n_chains = __sw_hweight8(dev->mt76.antenna_mask);
 	struct {
 		u8 control_chan;
@@ -499,7 +500,9 @@ int mt7603_mcu_set_channel(struct mt7603_dev *dev)
 		.rx_streams = n_chains,
 	};
 	struct sk_buff *skb;
+	s8 tx_power;
 	int ret;
+	int i;
 
 	if (dev->mt76.chandef.width == NL80211_CHAN_WIDTH_40) {
 		req.bw = MT_BW_40;
@@ -509,7 +512,16 @@ int mt7603_mcu_set_channel(struct mt7603_dev *dev)
 			req.center_chan -= 2;
 	}
 
-	memset(req.txpower, 0xff, sizeof(req.txpower));
+	tx_power = hw->conf.power_level * 2;
+	if (dev->mt76.antenna_mask == 3)
+		tx_power -= 6;
+	tx_power = min(tx_power, dev->tx_power_limit);
+
+	dev->mt76.txpower_cur = tx_power;
+
+	for (i = 0; i < ARRAY_SIZE(req.txpower); i++)
+		req.txpower[i] = tx_power;
+
 	skb = mt7603_mcu_msg_alloc(&req, sizeof(req));
 	ret = mt7603_mcu_msg_send(dev, skb, MCU_EXT_CMD_CHANNEL_SWITCH,
 				  MCU_Q_SET);
diff --git a/mt7603/mt7603.h b/mt7603/mt7603.h
index 6dfc7bff..d19d6783 100644
--- a/mt7603/mt7603.h
+++ b/mt7603/mt7603.h
@@ -120,6 +120,8 @@ struct mt7603_dev {
 	u8 slottime;
 	s16 coverage_class;
 
+	s8 tx_power_limit;
+
 	ktime_t survey_time;
 	ktime_t ed_time;
 	int beacon_int;