1 files changed, 170 insertions, 5 deletions

diff --git a/drivers/leds/leds-qnap-mcu.c b/drivers/leds/leds-qnap-mcu.c
index 4e4709456261..6df110e33ac9 100644
--- a/drivers/leds/leds-qnap-mcu.c
+++ b/drivers/leds/leds-qnap-mcu.c
@@ -104,9 +104,9 @@ static int qnap_mcu_register_err_led(struct device *dev, struct qnap_mcu *mcu, i
 }
 
 enum qnap_mcu_usb_led_mode {
-	QNAP_MCU_USB_LED_ON = 1,
-	QNAP_MCU_USB_LED_OFF = 3,
-	QNAP_MCU_USB_LED_BLINK = 2,
+	QNAP_MCU_USB_LED_ON = 0,
+	QNAP_MCU_USB_LED_OFF = 2,
+	QNAP_MCU_USB_LED_BLINK = 1,
 };
 
 struct qnap_mcu_usb_led {
@@ -137,7 +137,7 @@ static int qnap_mcu_usb_led_set(struct led_classdev *led_cdev,
 	 * Byte 3 is shared between the usb led target on/off/blink
 	 * and also the buzzer control (in the input driver)
 	 */
-	cmd[2] = 'D' + usb_led->mode;
+	cmd[2] = 'E' + usb_led->mode;
 
 	return qnap_mcu_exec_with_ack(usb_led->mcu, cmd, sizeof(cmd));
 }
@@ -161,7 +161,7 @@ static int qnap_mcu_usb_led_blink_set(struct led_classdev *led_cdev,
 	 * Byte 3 is shared between the USB LED target on/off/blink
 	 * and also the buzzer control (in the input driver)
 	 */
-	cmd[2] = 'D' + usb_led->mode;
+	cmd[2] = 'E' + usb_led->mode;
 
 	return qnap_mcu_exec_with_ack(usb_led->mcu, cmd, sizeof(cmd));
 }
@@ -190,6 +190,166 @@ static int qnap_mcu_register_usb_led(struct device *dev, struct qnap_mcu *mcu)
 	return qnap_mcu_usb_led_set(&usb_led->cdev, 0);
 }
 
+enum qnap_mcu_status_led_mode {
+	QNAP_MCU_STATUS_LED_OFF = 0,
+	QNAP_MCU_STATUS_LED_ON = 1,
+	QNAP_MCU_STATUS_LED_BLINK_FAST = 2, /* 500ms / 500ms */
+	QNAP_MCU_STATUS_LED_BLINK_SLOW = 3, /* 1s / 1s */
+};
+
+struct qnap_mcu_status_led {
+	struct led_classdev cdev;
+	struct qnap_mcu_status_led *red;
+	u8 mode;
+};
+
+struct qnap_mcu_status {
+	struct qnap_mcu *mcu;
+	struct qnap_mcu_status_led red;
+	struct qnap_mcu_status_led green;
+};
+
+static inline struct qnap_mcu_status_led *cdev_to_qnap_mcu_status_led(struct led_classdev *led_cdev)
+{
+	return container_of(led_cdev, struct qnap_mcu_status_led, cdev);
+}
+
+static inline struct qnap_mcu_status *statusled_to_qnap_mcu_status(struct qnap_mcu_status_led *led)
+{
+	return container_of(led->red, struct qnap_mcu_status, red);
+}
+
+static u8 qnap_mcu_status_led_encode(struct qnap_mcu_status *status)
+{
+	if (status->red.mode == QNAP_MCU_STATUS_LED_OFF) {
+		switch (status->green.mode) {
+		case QNAP_MCU_STATUS_LED_OFF:
+			return '9';
+		case QNAP_MCU_STATUS_LED_ON:
+			return '6';
+		case QNAP_MCU_STATUS_LED_BLINK_FAST:
+			return '5';
+		case QNAP_MCU_STATUS_LED_BLINK_SLOW:
+			return 'A';
+		}
+	} else if (status->green.mode == QNAP_MCU_STATUS_LED_OFF) {
+		switch (status->red.mode) {
+		case QNAP_MCU_STATUS_LED_OFF:
+			return '9';
+		case QNAP_MCU_STATUS_LED_ON:
+			return '7';
+		case QNAP_MCU_STATUS_LED_BLINK_FAST:
+			return '4';
+		case QNAP_MCU_STATUS_LED_BLINK_SLOW:
+			return 'B';
+		}
+	} else if (status->green.mode == QNAP_MCU_STATUS_LED_ON &&
+		   status->red.mode == QNAP_MCU_STATUS_LED_ON) {
+		return 'D';
+	} else if (status->green.mode == QNAP_MCU_STATUS_LED_BLINK_SLOW &&
+		   status->red.mode == QNAP_MCU_STATUS_LED_BLINK_SLOW) {
+		return 'C';
+	}
+
+	/*
+	 * Here both LEDs are on in some fashion, either both blinking fast,
+	 * or in different speeds, so default to fast blinking for both.
+	 */
+	return '8';
+}
+
+static int qnap_mcu_status_led_update(struct qnap_mcu *mcu,
+				      struct qnap_mcu_status *status)
+{
+	u8 cmd[] = { '@', 'C', 0 };
+
+	cmd[2] = qnap_mcu_status_led_encode(status);
+
+	return qnap_mcu_exec_with_ack(mcu, cmd, sizeof(cmd));
+}
+
+static int qnap_mcu_status_led_set(struct led_classdev *led_cdev,
+				   enum led_brightness brightness)
+{
+	struct qnap_mcu_status_led *status_led = cdev_to_qnap_mcu_status_led(led_cdev);
+	struct qnap_mcu_status *base = statusled_to_qnap_mcu_status(status_led);
+
+	/* Don't disturb a possible set blink-mode if LED stays on */
+	if (brightness != 0 && status_led->mode >= QNAP_MCU_STATUS_LED_BLINK_FAST)
+		return 0;
+
+	status_led->mode = brightness ? QNAP_MCU_STATUS_LED_ON :
+					QNAP_MCU_STATUS_LED_OFF;
+
+	return qnap_mcu_status_led_update(base->mcu, base);
+}
+
+static int qnap_mcu_status_led_blink_set(struct led_classdev *led_cdev,
+					 unsigned long *delay_on,
+					 unsigned long *delay_off)
+{
+	struct qnap_mcu_status_led *status_led = cdev_to_qnap_mcu_status_led(led_cdev);
+	struct qnap_mcu_status *base = statusled_to_qnap_mcu_status(status_led);
+
+	if (status_led->mode == QNAP_MCU_STATUS_LED_OFF)
+		return 0;
+
+	if (*delay_on <= 500) {
+		*delay_on = 500;
+		*delay_off = 500;
+		status_led->mode = QNAP_MCU_STATUS_LED_BLINK_FAST;
+	} else {
+		*delay_on = 1000;
+		*delay_off = 1000;
+		status_led->mode = QNAP_MCU_STATUS_LED_BLINK_SLOW;
+	}
+
+	return qnap_mcu_status_led_update(base->mcu, base);
+}
+
+static int qnap_mcu_register_status_leds(struct device *dev, struct qnap_mcu *mcu)
+{
+	struct qnap_mcu_status *status;
+	int ret;
+
+	status = devm_kzalloc(dev, sizeof(*status), GFP_KERNEL);
+	if (!status)
+		return -ENOMEM;
+
+	status->mcu = mcu;
+
+	/*
+	 * point to the red led, so that statusled_to_qnap_mcu_status
+	 * can resolve the main status struct containing both leds
+	 */
+	status->red.red = &status->red;
+	status->green.red = &status->red;
+
+	status->red.mode = QNAP_MCU_STATUS_LED_OFF;
+	status->red.cdev.name = "red:status";
+	status->red.cdev.brightness_set_blocking = qnap_mcu_status_led_set;
+	status->red.cdev.blink_set = qnap_mcu_status_led_blink_set;
+	status->red.cdev.brightness = 0;
+	status->red.cdev.max_brightness = 1;
+
+	status->green.mode = QNAP_MCU_STATUS_LED_OFF;
+	status->green.cdev.name = "green:status";
+	status->green.cdev.brightness_set_blocking = qnap_mcu_status_led_set;
+	status->green.cdev.blink_set = qnap_mcu_status_led_blink_set;
+	status->green.cdev.brightness = 0;
+	status->green.cdev.max_brightness = 1;
+
+	ret = devm_led_classdev_register(dev, &status->red.cdev);
+	if (ret)
+		return ret;
+
+	ret = devm_led_classdev_register(dev, &status->green.cdev);
+	if (ret)
+		return ret;
+
+	return qnap_mcu_status_led_update(status->mcu, status);
+}
+
 static int qnap_mcu_leds_probe(struct platform_device *pdev)
 {
 	struct qnap_mcu *mcu = dev_get_drvdata(pdev->dev.parent);
@@ -210,6 +370,11 @@ static int qnap_mcu_leds_probe(struct platform_device *pdev)
 					"failed to register USB LED\n");
 	}
 
+	ret = qnap_mcu_register_status_leds(&pdev->dev, mcu);
+	if (ret)
+		return dev_err_probe(&pdev->dev, ret,
+				     "failed to register status LEDs\n");
+
 	return 0;
 }