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>
This commit is contained in:
Henrik Brix Andersen
Carles Cufí
parent
56f74da757
commit
2dcbd8bbc1
|
|
@ -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
|
||||
|
|
|
|||
97
include/canbus/canopen.h
Normal file
97
include/canbus/canopen.h
Normal file
|
|
@ -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_ */
|
||||
|
|
@ -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(.)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
231
subsys/canbus/canopen/canopen_storage.c
Normal file
231
subsys/canbus/canopen/canopen_storage.c
Normal file
|
|
@ -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;
|
||||
}
|
||||
Loading…
Reference in a new issue