canbus: canopen: add object dictionary storage

Add support for storing the CANopen object dictionary to non-volatile
storage.

This fixes #15278.

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

CODEOWNERS

@@ -267,6 +267,7 @@
 /include/sys/atomic.h                     @andrewboie @andyross
 /include/bluetooth/                       @joerchan @jhedberg @Vudentz
 /include/cache.h                          @andrewboie @andyross
+/include/canbus/                          @alexanderwachter
 /include/device.h                         @wentongwu @nashif
 /include/display/                         @vanwinkeljan
 /include/dt-bindings/clock/kinetis_mcg.h  @henrikbrixandersen

include/canbus/canopen.h

@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2019 Vestas Wind Systems A/S
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+/**
+ * @brief CANopen Network Stack
+ * @defgroup canopen CANopen Network Stack
+ * @ingroup CAN
+ * @{
+ */
+
+#ifndef ZEPHYR_INCLUDE_CANOPEN_H_
+#define ZEPHYR_INCLUDE_CANOPEN_H_
+
+#include <CANopen.h>
+#include <CO_Emergency.h>
+#include <CO_SDO.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief CANopen object dictionary storage types.
+ */
+enum canopen_storage {
+	CANOPEN_STORAGE_RAM,
+	CANOPEN_STORAGE_ROM,
+	CANOPEN_STORAGE_EEPROM,
+};
+
+/**
+ * @brief Attach CANopen object dictionary storage handlers.
+ *
+ * Attach CANopen storage handler functions to object dictionary
+ * indexes 0x1010 (Store parameters) and 0x1011 (Restore default
+ * parameters). This function must be called after calling CANopenNode
+ * `CO_init()`.
+ *
+ * The handlers will save object dictionary entries of type @ref
+ * CANOPEN_STORAGE_ROM to non-volatile storage when a CANopen SDO
+ * client writes 0x65766173 ('s', 'a', 'v', 'e' from LSB to MSB) to
+ * object dictionary index 0x1010 sub-index 1.
+ *
+ * Object dictionary entries of types @ref CANOPEN_STORAGE_ROM (and
+ * optionally @ref CANOPEN_STORAGE_EEPROM) will be deleted from
+ * non-volatile storage when a CANopen SDO client writes 0x64616F6C
+ * ('l', 'o', 'a', 'd' from LSB to MSB) to object dictionary index
+ * 0x1011 sub-index 1.
+ *
+ * Object dictionary entries of type @ref CANOPEN_STORAGE_EEPROM may be
+ * saved by the application by periodically calling @ref
+ * canopen_storage_save().
+ *
+ * Object dictionary entries of type @ref CANOPEN_STORAGE_RAM are
+ * never saved to non-volatile storage.
+ *
+ * @param sdo CANopenNode SDO server object
+ * @param em  CANopenNode Emergency object
+ */
+void canopen_storage_attach(CO_SDO_t *sdo, CO_EM_t *em);
+
+/**
+ * @brief Save CANopen object dictionary entries to non-volatile storage.
+ *
+ * Save object dictionary entries of a given type to non-volatile
+ * storage.
+ *
+ * @param storage CANopen object dictionary entry type
+ *
+ * @return 0 if successful, negative errno code if failure
+ */
+int canopen_storage_save(enum canopen_storage storage);
+
+/**
+ * @brief Erase CANopen object dictionary entries from non-volatile storage.
+ *
+ * Erase object dictionary entries of a given type from non-volatile
+ * storage.
+ *
+ * @param storage CANopen object dictionary entry type
+ *
+ * @return 0 if successful, negative errno code if failure
+ */
+int canopen_storage_erase(enum canopen_storage storage);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* ZEPHYR_INCLUDE_CANOPEN_H_ */

subsys/canbus/canopen/CMakeLists.txt

@@ -3,4 +3,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_include_directories(.)

subsys/canbus/canopen/Kconfig

@@ -49,6 +49,22 @@ config CANOPEN_TX_WORKQUEUE_PRIORITY
 	help
 	  Priority level of the internal CANopen transmit workqueue.
 
+config CANOPEN_STORAGE
+	bool "CANopen object dictionary storage"
+	depends on SETTINGS
+	default y
+	help
+	  Enable support for storing the CANopen object dictionary to
+	  non-volatile storage.
+
+config CANOPEN_STORAGE_HANDLER_ERASES_EEPROM
+	bool "Erase CANopen object dictionary EEPROM entries in storage handler"
+	depends on CANOPEN_STORAGE
+	default n
+	help
+	  Erase CANopen object dictionary EEPROM entries upon write to
+	  object dictionary index 0x1011 subindex 1.
+
 config CANOPEN_SYNC_THREAD
 	bool "CANopen SYNC thread"
 	default y

subsys/canbus/canopen/canopen_storage.c

@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2019 Vestas Wind Systems A/S
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <settings/settings.h>
+
+#include <CANopen.h>
+#include <CO_Emergency.h>
+#include <CO_SDO.h>
+
+#include <canbus/canopen.h>
+
+#define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
+#include <logging/log.h>
+LOG_MODULE_REGISTER(canopen_storage);
+
+/* 's', 'a', 'v', 'e' from LSB to MSB */
+#define STORE_PARAM_MAGIC   0x65766173UL
+
+/* 'l', 'o', 'a', 'd' from LSB to MSB */
+#define RESTORE_PARAM_MAGIC 0x64616F6CUL
+
+/* Variables for reporing errors through CANopen once the stack is up */
+static int canopen_storage_rom_error;
+static int canopen_storage_eeprom_error;
+
+static CO_SDO_abortCode_t canopen_odf_1010(CO_ODF_arg_t *odf_arg)
+{
+	CO_EM_t *em = odf_arg->object;
+	u32_t value;
+	int err;
+
+	value = CO_getUint32(odf_arg->data);
+
+	if (odf_arg->reading) {
+		return CO_SDO_AB_NONE;
+	}
+
+	/* Preserve old value */
+	memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(u32_t));
+
+	if (odf_arg->subIndex != 1U) {
+		return CO_SDO_AB_NONE;
+	}
+
+	if (value != STORE_PARAM_MAGIC) {
+		return CO_SDO_AB_DATA_TRANSF;
+	}
+
+	err = canopen_storage_save(CANOPEN_STORAGE_ROM);
+	if (err) {
+		LOG_ERR("failed to save object dictionary ROM entries (err %d)",
+			err);
+		CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
+			       err);
+		return CO_SDO_AB_HW;
+	} else {
+		LOG_DBG("saved object dictionary ROM entries");
+	}
+
+	return CO_SDO_AB_NONE;
+}
+
+static CO_SDO_abortCode_t canopen_odf_1011(CO_ODF_arg_t *odf_arg)
+{
+	CO_EM_t *em = odf_arg->object;
+	bool failed = false;
+	u32_t value;
+	int err;
+
+	value = CO_getUint32(odf_arg->data);
+
+	if (odf_arg->reading) {
+		return CO_SDO_AB_NONE;
+	}
+
+	/* Preserve old value */
+	memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(u32_t));
+
+	if (odf_arg->subIndex < 1U) {
+		return CO_SDO_AB_NONE;
+	}
+
+	if (value != RESTORE_PARAM_MAGIC) {
+		return CO_SDO_AB_DATA_TRANSF;
+	}
+
+	err = canopen_storage_erase(CANOPEN_STORAGE_ROM);
+	if (err == -ENOENT) {
+		LOG_DBG("no object dictionary ROM entries to delete");
+	} else if (err) {
+		LOG_ERR("failed to delete object dictionary ROM entries"
+			" (err %d)", err);
+		CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
+			       err);
+		failed = true;
+	} else {
+		LOG_DBG("deleted object dictionary ROM entries");
+	}
+
+#ifdef CANOPEN_STORAGE_HANDLER_ERASES_EEPROM
+	err = canopen_storage_erase(CANOPEN_STORAGE_EEPROM);
+	if (err == -ENOENT) {
+		LOG_DBG("no object dictionary EEPROM entries to delete");
+	} else if (err) {
+		LOG_ERR("failed to delete object dictionary EEPROM entries"
+			" (err %d)", err);
+		CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
+			       err);
+		failed = true;
+	} else {
+		LOG_DBG("deleted object dictionary EEPROM entries");
+	}
+#endif
+
+	if (failed) {
+		return CO_SDO_AB_HW;
+	}
+
+	return CO_SDO_AB_NONE;
+}
+
+static int canopen_settings_set(const char *key, size_t len_rd,
+				settings_read_cb read_cb, void *cb_arg)
+{
+	const char *next;
+	int nlen;
+	int len;
+
+	nlen = settings_name_next(key, &next);
+
+	if (!strncmp(key, "eeprom", nlen)) {
+		struct sCO_OD_EEPROM eeprom;
+
+		len = read_cb(cb_arg, &eeprom, sizeof(eeprom));
+		if (len < 0) {
+			LOG_ERR("failed to restore object dictionary EEPROM"
+				" entries (err %d)", len);
+			canopen_storage_eeprom_error = len;
+		} else {
+			if ((eeprom.FirstWord == CO_OD_FIRST_LAST_WORD) &&
+			    (eeprom.LastWord == CO_OD_FIRST_LAST_WORD)) {
+				memcpy(&CO_OD_EEPROM, &eeprom,
+				       sizeof(CO_OD_EEPROM));
+				LOG_DBG("restored object dictionary EEPROM"
+					" entries");
+			} else {
+				LOG_WRN("object dictionary EEPROM entries"
+					" signature mismatch, skipping"
+					" restore");
+			}
+		}
+
+		return 0;
+	} else if (!strncmp(key, "rom", nlen)) {
+		struct sCO_OD_ROM rom;
+
+		len = read_cb(cb_arg, &rom, sizeof(rom));
+		if (len < 0) {
+			LOG_ERR("failed to restore object dictionary ROM"
+				" entries (err %d)", len);
+			canopen_storage_rom_error = len;
+		} else {
+			if ((rom.FirstWord == CO_OD_FIRST_LAST_WORD) &&
+			    (rom.LastWord == CO_OD_FIRST_LAST_WORD)) {
+				memcpy(&CO_OD_ROM, &rom, sizeof(CO_OD_ROM));
+				LOG_DBG("restored object dictionary ROM"
+					" entries");
+			} else {
+				LOG_WRN("object dictionary ROM entries"
+					" signature mismatch, skipping"
+					" restore");
+			}
+		}
+
+		return 0;
+	}
+
+	return 0;
+}
+
+SETTINGS_STATIC_HANDLER_DEFINE(canopen, "canopen", NULL,
+			       canopen_settings_set, NULL, NULL);
+
+void canopen_storage_attach(CO_SDO_t *sdo, CO_EM_t *em)
+{
+	CO_OD_configure(sdo, OD_H1010_STORE_PARAM_FUNC, canopen_odf_1010,
+			em, 0U, 0U);
+	CO_OD_configure(sdo, OD_H1011_REST_PARAM_FUNC, canopen_odf_1011,
+			em, 0U, 0U);
+
+	if (canopen_storage_eeprom_error) {
+		CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
+			       canopen_storage_eeprom_error);
+	}
+
+	if (canopen_storage_rom_error) {
+		CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE,
+			       canopen_storage_rom_error);
+	}
+}
+
+int canopen_storage_save(enum canopen_storage storage)
+{
+	int ret = 0;
+
+	if (storage == CANOPEN_STORAGE_ROM) {
+		ret = settings_save_one("canopen/rom", &CO_OD_ROM,
+					sizeof(CO_OD_ROM));
+	} else if (storage == CANOPEN_STORAGE_EEPROM) {
+		ret = settings_save_one("canopen/eeprom", &CO_OD_EEPROM,
+					sizeof(CO_OD_EEPROM));
+	}
+
+	return ret;
+}
+
+int canopen_storage_erase(enum canopen_storage storage)
+{
+	int ret = 0;
+
+	if (storage == CANOPEN_STORAGE_ROM) {
+		ret = settings_delete("canopen/rom");
+	} else if (storage == CANOPEN_STORAGE_EEPROM) {
+		ret = settings_delete("canopen/eeprom");
+	}
+
+	return ret;
+}