[U-Boot] misc: led: Add PCA9551 LED driver

diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 36a8f0d..b8884ec 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -61,3 +61,17 @@  config CONFIG_FSL_SEC_MON
 	  system states.
 	  Security Monitor can be transitioned on any security failures,
 	  like software violations or hardware security violations.
+
+config PCA9551_LED
+	bool "Enable PCA9551 LED driver"
+	depends on TARGET_TQMA6
+	help
+	  Enable driver for PCA9551 LED controller. This controller
+	  is connected via I2C. So I2C needs to be enabled.
+
+config PCA9551_I2C_ADDR
+	hex "I2C address of PCA9551 LED controller"
+	depends on PCA9551_LED
+	default 0x60
+	help
+	  The I2C address of the PCA9551 LED controller.
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 6028cd4..149e686 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -29,3 +29,4 @@  obj-$(CONFIG_STATUS_LED) += status_led.o
 obj-$(CONFIG_TWL4030_LED) += twl4030_led.o
 obj-$(CONFIG_FSL_IFC) += fsl_ifc.o
 obj-$(CONFIG_FSL_SEC_MON) += fsl_sec_mon.o
+obj-$(CONFIG_PCA9551_LED) += pca9551_led.o
diff --git a/drivers/misc/pca9551_led.c b/drivers/misc/pca9551_led.c
new file mode 100644
index 0000000..273cc53
--- /dev/null
+++ b/drivers/misc/pca9551_led.c
@@ -0,0 +1,147 @@ 
+/*
+ * Copyright (C) 2015 Stefan Roese <sr@denx.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <i2c.h>
+
+#ifndef CONFIG_PCA9551_I2C_ADDR
+#error "CONFIG_PCA9551_I2C_ADDR not defined!"
+#endif
+
+#define PCA9551_REG_INPUT	0x00	/* Input register (read only) */
+#define PCA9551_REG_PSC0	0x01	/* Frequency prescaler 0 */
+#define PCA9551_REG_PWM0	0x02	/* PWM0 */
+#define PCA9551_REG_PSC1	0x03	/* Frequency prescaler 1 */
+#define PCA9551_REG_PWM1	0x04	/* PWM1 */
+#define PCA9551_REG_LS0		0x05	/* LED0 to LED3 selector */
+#define PCA9551_REG_LS1		0x06	/* LED4 to LED7 selector */
+
+#define PCA9551_CTRL_AI		(1 << 4)	/* Auto-increment flag */
+
+#define PCA9551_LED_STATE_ON		0x00
+#define PCA9551_LED_STATE_OFF		0x01
+#define PCA9551_LED_STATE_BLINK0	0x02
+#define PCA9551_LED_STATE_BLINK1	0x03
+
+struct pca9551_blink_rate {
+	u8 psc;	/* Frequency preescaler, see PCA9551_7.pdf p. 6 */
+	u8 pwm;	/* Pulse width modulation, see PCA9551_7.pdf p. 6 */
+} __packed;
+
+static int freq0, freq1;
+
+static int pca9551_led_get_state(int led, int *state)
+{
+	unsigned int reg;
+	u8 shift, buf;
+
+	if (led < 0 || led > 7) {
+		return -1;
+	} else if (led < 4) {
+		reg = PCA9551_REG_LS0;
+		shift = led << 1;
+	} else {
+		reg = PCA9551_REG_LS1;
+		shift = (led - 4) << 1;
+	}
+
+	if (i2c_read(CONFIG_PCA9551_I2C_ADDR, reg, 1, &buf, 1))
+		return -1;
+
+	*state = (buf >> shift) & 0x03;
+	return 0;
+}
+
+static int pca9551_led_set_state(int led, int state)
+{
+	unsigned int reg;
+	u8 shift, buf, mask;
+
+	if (led < 0 || led > 7) {
+		return -1;
+	} else if (led < 4) {
+		reg = PCA9551_REG_LS0;
+		shift = led << 1;
+	} else {
+		reg = PCA9551_REG_LS1;
+		shift = (led - 4) << 1;
+	}
+	mask = 0x03 << shift;
+
+	if (i2c_read(CONFIG_PCA9551_I2C_ADDR, reg, 1, &buf, 1))
+		return -1;
+
+	buf = (buf & ~mask) | ((state & 0x03) << shift);
+
+	if (i2c_write(CONFIG_PCA9551_I2C_ADDR, reg, 1, &buf, 1))
+		return -1;
+
+	return 0;
+}
+
+static int pca9551_led_set_blink_rate(int idx, struct pca9551_blink_rate rate)
+{
+	unsigned int reg;
+
+	switch (idx) {
+	case 0:
+		reg = PCA9551_REG_PSC0;
+		break;
+	case 1:
+		reg = PCA9551_REG_PSC1;
+		break;
+	default:
+		return -1;
+	}
+	reg |= PCA9551_CTRL_AI;
+
+	if (i2c_write(CONFIG_PCA9551_I2C_ADDR, reg, 1, (u8 *)&rate, 2))
+		return -1;
+
+	return 0;
+}
+
+/*
+ * Functions referenced by cmd_led.c
+ */
+void __led_set(led_id_t mask, int state)
+{
+	if (state == STATUS_LED_OFF)
+		pca9551_led_set_state(mask, PCA9551_LED_STATE_OFF);
+	else
+		pca9551_led_set_state(mask, PCA9551_LED_STATE_ON);
+}
+
+void __led_toggle(led_id_t mask)
+{
+	int state = 0;
+
+	pca9551_led_get_state(mask, &state);
+	pca9551_led_set_state(mask, !state);
+}
+
+void __led_blink(led_id_t mask, int freq)
+{
+	struct pca9551_blink_rate rate;
+	int mode;
+	int blink;
+
+	if ((freq0 == 0) || (freq == freq0)) {
+		blink = 0;
+		mode = PCA9551_LED_STATE_BLINK0;
+		freq0 = freq;
+	} else {
+		blink = 1;
+		mode = PCA9551_LED_STATE_BLINK1;
+		freq1 = freq;
+	}
+
+	rate.psc = ((freq * 38) / 1000) - 1;
+	rate.pwm = 128;		/* 50% duty cycle */
+
+	pca9551_led_set_blink_rate(blink, rate);
+	pca9551_led_set_state(mask, mode);
+}