From 2dcbd8bbc1554aa073adde1b73b1a5ee7ae093ff Mon Sep 17 00:00:00 2001 From: Henrik Brix Andersen Date: Thu, 26 Sep 2019 15:06:55 +0200 Subject: [PATCH] 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 --- CODEOWNERS | 1 + include/canbus/canopen.h | 97 ++++++++++ subsys/canbus/canopen/CMakeLists.txt | 1 + subsys/canbus/canopen/Kconfig | 16 ++ subsys/canbus/canopen/canopen_storage.c | 231 ++++++++++++++++++++++++ 5 files changed, 346 insertions(+) create mode 100644 include/canbus/canopen.h create mode 100644 subsys/canbus/canopen/canopen_storage.c diff --git a/CODEOWNERS b/CODEOWNERS index 18563f29efb..26205f82411 100644 --- a/CODEOWNERS +++ b/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 diff --git a/include/canbus/canopen.h b/include/canbus/canopen.h new file mode 100644 index 00000000000..b8d5051f91d --- /dev/null +++ b/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 +#include +#include + +#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_ */ diff --git a/subsys/canbus/canopen/CMakeLists.txt b/subsys/canbus/canopen/CMakeLists.txt index d3a172ccaf6..64e021b9629 100644 --- a/subsys/canbus/canopen/CMakeLists.txt +++ b/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(.) diff --git a/subsys/canbus/canopen/Kconfig b/subsys/canbus/canopen/Kconfig index daab1b861b1..3c7cf09c139 100644 --- a/subsys/canbus/canopen/Kconfig +++ b/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 diff --git a/subsys/canbus/canopen/canopen_storage.c b/subsys/canbus/canopen/canopen_storage.c new file mode 100644 index 00000000000..be5da48344b --- /dev/null +++ b/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 + +#include +#include +#include + +#include + +#define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL +#include +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; +}