[PATCH 2/2] mt76x0: phy: fix mt76x0_temp_sensor routine

[Lorenzo Bianconi <lorenzo.bianconi@xxxxxxxxxx>]

Fix configuration of register 67 on bank0 during temperature
reading and use mt76_poll utility routine to poll core34
register. Moreover temperature compensation need to be disabled
if the device supports tssi compensation.
This issue has never been hit since temperature reading is not
actually used by usb code.

Fixes: 10de7a8b4ab9 ("mt76x0: phy files")
Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@xxxxxxxxxx>
---
 .../net/wireless/mediatek/mt76/mt76x0/phy.c   | 43 +++++++++----------
 1 file changed, 20 insertions(+), 23 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt76x0/phy.c b/drivers/net/wireless/mediatek/mt76/mt76x0/phy.c
index 29bc4e4623cd..e9254452c7f5 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76x0/phy.c
+++ b/drivers/net/wireless/mediatek/mt76/mt76x0/phy.c
@@ -763,48 +763,41 @@ void mt76x0_agc_restore(struct mt76x02_dev *dev)
 	mt76_rmw_field(dev, MT_BBP(AGC, 8), MT_BBP_AGC_GAIN, dev->agc_save);
 }
 
-static void mt76x0_temp_sensor(struct mt76x02_dev *dev)
+static int mt76x0_temp_sensor(struct mt76x02_dev *dev, int *temp)
 {
 	u8 rf_b7_73, rf_b0_66, rf_b0_67;
-	int cycle, temp;
-	u32 val;
-	s32 sval;
+	int val, err = 0;
 
 	rf_b7_73 = rf_rr(dev, MT_RF(7, 73));
 	rf_b0_66 = rf_rr(dev, MT_RF(0, 66));
-	rf_b0_67 = rf_rr(dev, MT_RF(0, 73));
+	rf_b0_67 = rf_rr(dev, MT_RF(0, 67));
 
 	rf_wr(dev, MT_RF(7, 73), 0x02);
 	rf_wr(dev, MT_RF(0, 66), 0x23);
-	rf_wr(dev, MT_RF(0, 73), 0x01);
+	rf_wr(dev, MT_RF(0, 67), 0x01);
 
 	mt76_wr(dev, MT_BBP(CORE, 34), 0x00080055);
 
-	for (cycle = 0; cycle < 2000; cycle++) {
-		val = mt76_rr(dev, MT_BBP(CORE, 34));
-		if (!(val & 0x10))
-			break;
-		udelay(3);
-	}
-
-	if (cycle >= 2000) {
-		val &= 0x10;
-		mt76_wr(dev, MT_BBP(CORE, 34), val);
+	if (!mt76_poll(dev, MT_BBP(CORE, 34), BIT(4), 0, 2000)) {
+		mt76_clear(dev, MT_BBP(CORE, 34), BIT(4));
+		err = -ETIMEDOUT;
 		goto done;
 	}
 
-	sval = mt76_rr(dev, MT_BBP(CORE, 35)) & 0xff;
-	if (!(sval & 0x80))
-		sval &= 0x7f; /* Positive */
+	val = mt76_rr(dev, MT_BBP(CORE, 35)) & 0xff;
+	if (!(val & BIT(7)))
+		val &= 0x7f; /* Positive */
 	else
-		sval |= 0xffffff00; /* Negative */
+		val |= 0xffffff00; /* Negative */
 
-	temp = (35 * (sval - dev->cal.rx.temp_offset)) / 10 + 25;
+	*temp = (35 * (val - dev->cal.rx.temp_offset)) / 10 + 25;
 
 done:
 	rf_wr(dev, MT_RF(7, 73), rf_b7_73);
 	rf_wr(dev, MT_RF(0, 66), rf_b0_66);
-	rf_wr(dev, MT_RF(0, 73), rf_b0_67);
+	rf_wr(dev, MT_RF(0, 67), rf_b0_67);
+
+	return err;
 }
 
 static void mt76x0_dynamic_vga_tuning(struct mt76x02_dev *dev)
@@ -832,7 +825,11 @@ static void mt76x0_phy_calibrate(struct work_struct *work)
 					       cal_work.work);
 
 	mt76x0_dynamic_vga_tuning(dev);
-	mt76x0_temp_sensor(dev);
+	if (!mt76x0_tssi_enabled(dev)) {
+		int temp;
+
+		mt76x0_temp_sensor(dev, &temp);
+	}
 
 	ieee80211_queue_delayed_work(dev->mt76.hw, &dev->cal_work,
 				     MT_CALIBRATE_INTERVAL);
-- 
2.17.1