[PATCH] drivers: led: bcm6858: Set default brightness when setting LED state

[Paul HENRYS]

When setting the LED state (OFF, ON or blinking), the default
maximum brightness is set for ON and blinking state and the
brightness is set to 0 for OFF state.
This should make sure the LED controller's registers affecting the
brightness are correctly initialized and should give consistent
behaviour.

Signed-off-by: Paul HENRYS <paul.henrys_ext at softathome.com>
---
 drivers/led/led_bcm6858.c | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/drivers/led/led_bcm6858.c b/drivers/led/led_bcm6858.c
index fbf46a114c..02a8308611 100644
--- a/drivers/led/led_bcm6858.c
+++ b/drivers/led/led_bcm6858.c
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0+
 /*
  * Copyright (C) 2019 Philippe Reynes <philippe.reynes at softathome.com>
+ * Copyright (C) 2022 Paul HENRYS <paul.henrys_ext at softathome.com>
  *
  * based on:
  * drivers/led/led_bcm6328.c
@@ -38,6 +39,8 @@
 #define LED_HW_LED_EN_REG		0x08
 /* LED Flash control register0 */
 #define LED_FLASH_RATE_CONTROL_REG0	0x10
+/* LED Brightness control register0 */
+#define LED_BRIGHTNESS_CONTROL_REG0	0x20
 /* Soft LED input register */
 #define LED_SW_LED_IP_REG		0xb8
 /* Parallel LED Output Polarity Register */
@@ -96,6 +99,24 @@ static int bcm6858_led_set_period(struct udevice *dev, int period_ms)
 }
 #endif
 
+static int led_set_brightness(struct udevice *dev, unsigned int brightness)
+{
+	struct bcm6858_led_priv *priv = dev_get_priv(dev);
+	u32 offset, shift, mask, value;
+
+	offset = (priv->pin / 8) * 4;
+	shift  = (priv->pin % 8) * 4;
+	mask   = 0xf << shift;
+
+        /* 8 levels of brightness achieved through PWM */
+	value = (brightness > 7 ? 7 : brightness) << shift;
+
+	clrbits_32(priv->regs + LED_BRIGHTNESS_CONTROL_REG0 + offset, mask);
+	setbits_32(priv->regs + LED_BRIGHTNESS_CONTROL_REG0 + offset, value);
+
+	return 0;
+}
+
 static enum led_state_t bcm6858_led_get_state(struct udevice *dev)
 {
 	struct bcm6858_led_priv *priv = dev_get_priv(dev);
@@ -116,12 +137,15 @@ static int bcm6858_led_set_state(struct udevice *dev, enum led_state_t state)
 	switch (state) {
 	case LEDST_OFF:
 		clrbits_32(priv->regs + LED_SW_LED_IP_REG, (1 << priv->pin));
+		led_set_brightness(dev, 0);
 #ifdef CONFIG_LED_BLINK
 		bcm6858_led_set_period(dev, 0);
 #endif
 		break;
 	case LEDST_ON:
 		setbits_32(priv->regs + LED_SW_LED_IP_REG, (1 << priv->pin));
+		/* Set maximum brightness by default */
+		led_set_brightness(dev, 7);
 #ifdef CONFIG_LED_BLINK
 		bcm6858_led_set_period(dev, 0);
 #endif
@@ -135,6 +159,8 @@ static int bcm6858_led_set_state(struct udevice *dev, enum led_state_t state)
 #ifdef CONFIG_LED_BLINK
 	case LEDST_BLINK:
 		setbits_32(priv->regs + LED_SW_LED_IP_REG, (1 << priv->pin));
+		/* Set maximum brightness by default */
+		led_set_brightness(dev, 7);
 		break;
 #endif
 	default:
-- 
2.25.1