From 58e9138323368582efffdf9713fec931663e06af Mon Sep 17 00:00:00 2001
From: Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
Date: Mon, 30 Oct 2017 23:41:41 +0100
Subject: [PATCH] mt76x2: init: add ma80211 led callbacks

Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
---
 mt76x2_init.c | 52 +++++++++++++++++++++++++++++++++++++++++++++++++++
 mt76x2_mcu.h  | 19 ++++++++++++++-----
 2 files changed, 66 insertions(+), 5 deletions(-)

diff --git a/mt76x2_init.c b/mt76x2_init.c
index fd7328d1..445441ee 100644
--- a/mt76x2_init.c
+++ b/mt76x2_init.c
@@ -709,6 +709,54 @@ static const struct ieee80211_iface_combination if_comb[] = {
 	}
 };
 
+static void mt76x2_led_set_config(struct mt76_dev *mt76, u8 delay_on,
+				  u8 delay_off)
+{
+	struct mt76x2_dev *dev = container_of(mt76, struct mt76x2_dev,
+					      mt76);
+	u32 val;
+
+	val = MT_LED_STATUS_DURATION(0xff) |
+	      MT_LED_STATUS_OFF(delay_off) |
+	      MT_LED_STATUS_ON(delay_on);
+
+	mt76_wr(dev, MT_LED_S0(mt76->led_pin), val);
+	mt76_wr(dev, MT_LED_S1(mt76->led_pin), val);
+
+	val = MT_LED_CTRL_REPLAY(mt76->led_pin) |
+	      MT_LED_CTRL_KICK(mt76->led_pin);
+	if (mt76->led_al)
+		val |= MT_LED_CTRL_POLARITY(mt76->led_pin);
+	mt76_wr(dev, MT_LED_CTRL, val);
+}
+
+static int mt76x2_led_set_blink(struct led_classdev *led_cdev,
+				unsigned long *delay_on,
+				unsigned long *delay_off)
+{
+	struct mt76_dev *mt76 = container_of(led_cdev, struct mt76_dev,
+					     led_cdev);
+	u8 delta_on, delta_off;
+
+	delta_off = max_t(u8, *delay_off / 10, 1);
+	delta_on = max_t(u8, *delay_on / 10, 1);
+
+	mt76x2_led_set_config(mt76, delta_on, delta_off);
+	return 0;
+}
+
+static void mt76x2_led_set_brightness(struct led_classdev *led_cdev,
+				      enum led_brightness brightness)
+{
+	struct mt76_dev *mt76 = container_of(led_cdev, struct mt76_dev,
+					     led_cdev);
+
+	if (!brightness)
+		mt76x2_led_set_config(mt76, 0, 0xff);
+	else
+		mt76x2_led_set_config(mt76, 0xff, 0);
+}
+
 int mt76x2_register_device(struct mt76x2_dev *dev)
 {
 	struct ieee80211_hw *hw = mt76_hw(dev);
@@ -767,6 +815,10 @@ int mt76x2_register_device(struct mt76x2_dev *dev)
 
 	mt76x2_dfs_init_detector(dev);
 
+	/* init led callbacks */
+	dev->mt76.led_cdev.brightness_set = mt76x2_led_set_brightness;
+	dev->mt76.led_cdev.blink_set = mt76x2_led_set_blink;
+
 	ret = mt76_register_device(&dev->mt76, true, mt76x2_rates,
 				   ARRAY_SIZE(mt76x2_rates));
 	if (ret)
diff --git a/mt76x2_mcu.h b/mt76x2_mcu.h
index de4d958d..d7a7e832 100644
--- a/mt76x2_mcu.h
+++ b/mt76x2_mcu.h
@@ -39,11 +39,20 @@
 
 #define MT_LED_TX_BLINK_0		0x0774
 #define MT_LED_TX_BLINK_1		0x0778
-#define MT_LED0_S0			0x077C
-#define MT_LED0_S1			0x0780
-#define MT_LED0_STATUS_OFF		GENMASK(31, 24)
-#define MT_LED0_STATUS_ON		GENMASK(23, 16)
-#define MT_LED0_STATUS_DURATION		GENMASK(15, 8)
+
+#define MT_LED_S0_BASE			0x077C
+#define MT_LED_S0(_n)			(MT_LED_S0_BASE + 8 * (_n))
+#define MT_LED_S1_BASE			0x0780
+#define MT_LED_S1(_n)			(MT_LED_S1_BASE + 8 * (_n))
+#define MT_LED_STATUS_OFF_MASK		GENMASK(31, 24)
+#define MT_LED_STATUS_OFF(_v)		(((_v) << __ffs(MT_LED_STATUS_OFF_MASK)) & \
+					 MT_LED_STATUS_OFF_MASK)
+#define MT_LED_STATUS_ON_MASK		GENMASK(23, 16)
+#define MT_LED_STATUS_ON(_v)		(((_v) << __ffs(MT_LED_STATUS_ON_MASK)) & \
+					 MT_LED_STATUS_ON_MASK)
+#define MT_LED_STATUS_DURATION_MASK	GENMASK(15, 8)
+#define MT_LED_STATUS_DURATION(_v)	(((_v) << __ffs(MT_LED_STATUS_DURATION_MASK)) & \
+					 MT_LED_STATUS_DURATION_MASK)
 
 #define MT_MCU_SEMAPHORE_00		0x07B0
 #define MT_MCU_SEMAPHORE_01		0x07B4