From 030159528eff349db01b6b47b6fff8112b4282a4 Mon Sep 17 00:00:00 2001
From: mtk25577 <jen-hao.cheng@mediatek.com>
Date: Tue, 28 Mar 2023 18:23:00 +0800
Subject: [PATCH 26/39] wifi: mt76: mt7996: add led feature support

Signed-off-by: mtk25577 <jen-hao.cheng@mediatek.com>
---
 mt7996/Makefile |  1 +
 mt7996/init.c   | 33 +++++++++++++++++++++------------
 mt7996/regs.h   |  1 +
 3 files changed, 23 insertions(+), 12 deletions(-)

diff --git a/mt7996/Makefile b/mt7996/Makefile
index bed9efd..7c2514a 100644
--- a/mt7996/Makefile
+++ b/mt7996/Makefile
@@ -1,4 +1,5 @@
 # SPDX-License-Identifier: ISC
+EXTRA_CFLAGS += -DCONFIG_MT76_LEDS
 
 obj-$(CONFIG_MT7996E) += mt7996e.o
 
diff --git a/mt7996/init.c b/mt7996/init.c
index fecd0d3..192af3f 100644
--- a/mt7996/init.c
+++ b/mt7996/init.c
@@ -232,23 +232,31 @@ static void mt7996_led_set_config(struct led_classdev *led_cdev,
 	dev = container_of(mphy->dev, struct mt7996_dev, mt76);
 
 	/* select TX blink mode, 2: only data frames */
-	mt76_rmw_field(dev, MT_TMAC_TCR0(0), MT_TMAC_TCR0_TX_BLINK, 2);
+	mt76_rmw_field(dev, MT_TMAC_TCR0(mphy->band_idx), MT_TMAC_TCR0_TX_BLINK, 2);
 
 	/* enable LED */
-	mt76_wr(dev, MT_LED_EN(0), 1);
+	mt76_wr(dev, MT_LED_EN(mphy->band_idx), 1);
 
 	/* set LED Tx blink on/off time */
 	val = FIELD_PREP(MT_LED_TX_BLINK_ON_MASK, delay_on) |
 	      FIELD_PREP(MT_LED_TX_BLINK_OFF_MASK, delay_off);
-	mt76_wr(dev, MT_LED_TX_BLINK(0), val);
+	mt76_wr(dev, MT_LED_TX_BLINK(mphy->band_idx), val);
+
+	/* turn LED off */
+	if (delay_off == 0xff && delay_on == 0x0)
+		val = MT_LED_CTRL_POLARITY | MT_LED_CTRL_KICK;
+	else {
+		/* control LED */
+		val = MT_LED_CTRL_BLINK_MODE | MT_LED_CTRL_KICK;
+		if (mphy->band_idx == MT_BAND1)
+			val |= MT_LED_CTRL_BLINK_BAND_SEL;
+	}
 
-	/* control LED */
-	val = MT_LED_CTRL_BLINK_MODE | MT_LED_CTRL_KICK;
 	if (mphy->leds.al)
 		val |= MT_LED_CTRL_POLARITY;
 
-	mt76_wr(dev, MT_LED_CTRL(0), val);
-	mt76_clear(dev, MT_LED_CTRL(0), MT_LED_CTRL_KICK);
+	mt76_wr(dev, MT_LED_CTRL(mphy->band_idx), val);
+	mt76_clear(dev, MT_LED_CTRL(mphy->band_idx), MT_LED_CTRL_KICK);
 }
 
 static int mt7996_led_set_blink(struct led_classdev *led_cdev,
@@ -400,6 +408,12 @@ mt7996_init_wiphy(struct ieee80211_hw *hw)
 		ieee80211_hw_set(hw, SUPPORTS_VHT_EXT_NSS_BW);
 	}
 
+	/* init led callbacks */
+	if (IS_ENABLED(CONFIG_MT76_LEDS)) {
+		phy->mt76->leds.cdev.brightness_set = mt7996_led_set_brightness;
+		phy->mt76->leds.cdev.blink_set = mt7996_led_set_blink;
+	}
+
 	mt76_set_stream_caps(phy->mt76, true);
 	mt7996_set_stream_vht_txbf_caps(phy);
 	mt7996_set_stream_he_eht_caps(phy);
@@ -1063,11 +1077,6 @@ int mt7996_register_device(struct mt7996_dev *dev)
 #ifdef CONFIG_NL80211_TESTMODE
 	dev->mt76.test_ops = &mt7996_testmode_ops;
 #endif
-	/* init led callbacks */
-	if (IS_ENABLED(CONFIG_MT76_LEDS)) {
-		dev->mphy.leds.cdev.brightness_set = mt7996_led_set_brightness;
-		dev->mphy.leds.cdev.blink_set = mt7996_led_set_blink;
-	}
 
 	ret = mt76_register_device(&dev->mt76, true, mt76_rates,
 				   ARRAY_SIZE(mt76_rates));
diff --git a/mt7996/regs.h b/mt7996/regs.h
index d1d3d15..86da1bf 100644
--- a/mt7996/regs.h
+++ b/mt7996/regs.h
@@ -509,6 +509,7 @@ enum base_rev {
 
 #define MT_LED_CTRL(_n)				MT_LED_PHYS(0x00 + ((_n) * 4))
 #define MT_LED_CTRL_KICK			BIT(7)
+#define MT_LED_CTRL_BLINK_BAND_SEL		BIT(4)
 #define MT_LED_CTRL_BLINK_MODE			BIT(2)
 #define MT_LED_CTRL_POLARITY			BIT(1)
 
-- 
2.18.0