canbus: canopen: add LED indicator support

Add support for CANopen LED indicators according to the CAN in
Automation (CiA) 303-3 specification.

This fixes #15278.

Signed-off-by: Henrik Brix Andersen <hebad@vestas.com>

include/canbus/canopen.h

@@ -86,6 +86,32 @@ int canopen_storage_save(enum canopen_storage storage);
  */
 int canopen_storage_erase(enum canopen_storage storage);
 
+/**
+ * @typedef canopen_led_callback_t
+ * @brief CANopen LED indicator callback function signature.
+ *
+ * @param value true if the LED indicator shall be turned on, false otherwise.
+ * @param arg argument that was passed when LEDs were initialized.
+ */
+typedef void (*canopen_led_callback_t)(bool value, void *arg);
+
+/**
+ * @brief Initialize CANopen LED indicators.
+ *
+ * Initialize CANopen LED indicators and attach callbacks for setting
+ * their state. Two LED indicators, a red and a green, are supported
+ * according to CiA 303-3.
+ *
+ * @param nmt CANopenNode NMT object.
+ * @param green_cb callback for changing state on the green LED indicator.
+ * @param green_arg argument to pass to the green LED indicator callback.
+ * @param red_cb callback for changing state on the red LED indicator.
+ * @param red_arg argument to pass to the red LED indicator callback.
+ */
+void canopen_leds_init(CO_NMT_t *nmt,
+		       canopen_led_callback_t green_cb, void *green_arg,
+		       canopen_led_callback_t red_cb, void *red_arg);
+
 #ifdef __cplusplus
 }
 #endif

subsys/canbus/canopen/CMakeLists.txt

@@ -4,4 +4,5 @@ zephyr_library()
 zephyr_library_sources_ifdef(CONFIG_CANOPEN CO_driver.c)
 zephyr_library_sources_ifdef(CONFIG_CANOPEN_SYNC_THREAD canopen_sync.c)
 zephyr_library_sources_ifdef(CONFIG_CANOPEN_STORAGE canopen_storage.c)
+zephyr_library_sources_if_kconfig(canopen_leds.c)
 zephyr_include_directories(.)

subsys/canbus/canopen/Kconfig

@@ -65,6 +65,21 @@ config CANOPEN_STORAGE_HANDLER_ERASES_EEPROM
 	  Erase CANopen object dictionary EEPROM entries upon write to
 	  object dictionary index 0x1011 subindex 1.
 
+config CANOPEN_LEDS
+	bool "CANopen LED indicators"
+	default y
+	help
+	  Enable support for CANopen LED indicators according to the CiA
+	  303-3 specification.
+
+config CANOPEN_LEDS_BICOLOR
+	bool "CANopen bicolor LED indicator"
+	depends on CANOPEN_LEDS
+	help
+	  Handle CANopen LEDs as one bicolor LED, favoring the red LED
+	  over the green LED in accordance with the CiA 303-3
+	  specification.
+
 config CANOPEN_SYNC_THREAD
 	bool "CANopen SYNC thread"
 	default y

subsys/canbus/canopen/canopen_leds.c

@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2019 Vestas Wind Systems A/S
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <CANopen.h>
+#include <canbus/canopen.h>
+
+struct canopen_leds_state {
+	CO_NMT_t *nmt;
+	canopen_led_callback_t green_cb;
+	void *green_arg;
+	canopen_led_callback_t red_cb;
+	void *red_arg;
+	bool green : 1;
+	bool red : 1;
+};
+
+static struct canopen_leds_state canopen_leds;
+
+static void canopen_leds_update(struct k_timer *timer_id)
+{
+	bool green = false;
+	bool red = false;
+
+	ARG_UNUSED(timer_id);
+
+	CO_NMT_blinkingProcess50ms(canopen_leds.nmt);
+
+	green = LED_GREEN_RUN(canopen_leds.nmt);
+	red = LED_RED_ERROR(canopen_leds.nmt);
+
+#ifdef CONFIG_CANOPEN_LEDS_BICOLOR
+	if (red && canopen_leds.red_cb) {
+		green = false;
+	}
+#endif
+
+	if (canopen_leds.green_cb) {
+		if (green != canopen_leds.green) {
+			canopen_leds.green_cb(green, canopen_leds.green_arg);
+			canopen_leds.green = green;
+		}
+	}
+
+	if (canopen_leds.red_cb) {
+		if (red != canopen_leds.red) {
+			canopen_leds.red_cb(red, canopen_leds.red_arg);
+			canopen_leds.red = red;
+		}
+	}
+}
+
+K_TIMER_DEFINE(canopen_leds_timer, canopen_leds_update, NULL);
+
+void canopen_leds_init(CO_NMT_t *nmt,
+		       canopen_led_callback_t green_cb, void *green_arg,
+		       canopen_led_callback_t red_cb, void *red_arg)
+{
+	k_timer_stop(&canopen_leds_timer);
+
+	canopen_leds.nmt = nmt;
+
+	/* Call existing callbacks to turn off LEDs */
+	if (canopen_leds.green_cb) {
+		canopen_leds.green_cb(false, canopen_leds.green_arg);
+	}
+	if (canopen_leds.red_cb) {
+		canopen_leds.red_cb(false, canopen_leds.red_arg);
+	}
+
+	canopen_leds.green_cb = green_cb;
+	canopen_leds.green_arg = green_arg;
+	canopen_leds.green = false;
+	canopen_leds.red_cb = red_cb;
+	canopen_leds.red_arg = red_arg;
+	canopen_leds.red = false;
+
+	/* Call new callbacks to turn off LEDs */
+	if (canopen_leds.green_cb) {
+		canopen_leds.green_cb(false, canopen_leds.green_arg);
+	}
+	if (canopen_leds.red_cb) {
+		canopen_leds.red_cb(false, canopen_leds.red_arg);
+	}
+
+	if (nmt && (green_cb || red_cb)) {
+		k_timer_start(&canopen_leds_timer, K_MSEC(50), K_MSEC(50));
+	}
+}