michaelh/zephyr
1
0
Fork 0
Code Releases Activity

modules: canopennode: move glue code to modules directory

Browse source 
Move the Zephyr-specific interface and support code for CANopenNode into
the modules directory. Consolidate the CMakeLists.txt files into one.

Signed-off-by: Henrik Brix Andersen <hebad@vestas.com>
This commit is contained in:
Henrik Brix Andersen 2021-06-12 17:25:20 +02:00 • committed by Anas Nashif
commit d679037643
16 changed files with 72 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

36
modules/canopennode/CMakeLists.txt Normal file
View file

@ -0,0 +1,36 @@
# SPDX-License-Identifier: Apache-2.0
if(CONFIG_CANOPENNODE)
set(CANOPENNODE_DIR ${ZEPHYR_CURRENT_MODULE_DIR})
zephyr_library()
zephyr_include_directories(
${CANOPENNODE_DIR}
${CANOPENNODE_DIR}/stack
.
)
zephyr_library_sources(
${CANOPENNODE_DIR}/CANopen.c
${CANOPENNODE_DIR}/stack/CO_Emergency.c
${CANOPENNODE_DIR}/stack/CO_HBconsumer.c
${CANOPENNODE_DIR}/stack/CO_LSSmaster.c
${CANOPENNODE_DIR}/stack/CO_LSSslave.c
${CANOPENNODE_DIR}/stack/CO_NMT_Heartbeat.c
${CANOPENNODE_DIR}/stack/CO_PDO.c
${CANOPENNODE_DIR}/stack/CO_SDO.c
${CANOPENNODE_DIR}/stack/CO_SDOmaster.c
${CANOPENNODE_DIR}/stack/CO_SYNC.c
${CANOPENNODE_DIR}/stack/CO_TIME.c
${CANOPENNODE_DIR}/stack/CO_trace.c
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_ifdef(CONFIG_CANOPEN_LEDS canopen_leds.c)
zephyr_library_sources_ifdef(CONFIG_CANOPEN_PROGRAM_DOWNLOAD canopen_program.c)
endif()

484
modules/canopennode/CO_driver.c Normal file
View file

@ -0,0 +1,484 @@
/*
* Copyright (c) 2019 Vestas Wind Systems A/S
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr.h>
#include <drivers/can.h>
#include <init.h>
#include <sys/util.h>
#include <canbus/canopen.h>
#define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
#include <logging/log.h>
LOG_MODULE_REGISTER(canopen_driver);
K_KERNEL_STACK_DEFINE(canopen_tx_workq_stack,
CONFIG_CANOPEN_TX_WORKQUEUE_STACK_SIZE);
struct k_work_q canopen_tx_workq;
struct canopen_tx_work_container {
struct k_work work;
CO_CANmodule_t *CANmodule;
};
struct canopen_tx_work_container canopen_tx_queue;
K_MUTEX_DEFINE(canopen_send_mutex);
K_MUTEX_DEFINE(canopen_emcy_mutex);
K_MUTEX_DEFINE(canopen_co_mutex);
inline void canopen_send_lock(void)
{
k_mutex_lock(&canopen_send_mutex, K_FOREVER);
}
inline void canopen_send_unlock(void)
{
k_mutex_unlock(&canopen_send_mutex);
}
inline void canopen_emcy_lock(void)
{
k_mutex_lock(&canopen_emcy_mutex, K_FOREVER);
}
inline void canopen_emcy_unlock(void)
{
k_mutex_unlock(&canopen_emcy_mutex);
}
inline void canopen_od_lock(void)
{
k_mutex_lock(&canopen_co_mutex, K_FOREVER);
}
inline void canopen_od_unlock(void)
{
k_mutex_unlock(&canopen_co_mutex);
}
static void canopen_detach_all_rx_filters(CO_CANmodule_t *CANmodule)
{
uint16_t i;
if (!CANmodule || !CANmodule->rx_array || !CANmodule->configured) {
return;
}
for (i = 0U; i < CANmodule->rx_size; i++) {
if (CANmodule->rx_array[i].filter_id != CAN_NO_FREE_FILTER) {
can_detach(CANmodule->dev,
CANmodule->rx_array[i].filter_id);
CANmodule->rx_array[i].filter_id = CAN_NO_FREE_FILTER;
}
}
}
static void canopen_rx_isr_callback(struct zcan_frame *msg, void *arg)
{
CO_CANrx_t *buffer = (CO_CANrx_t *)arg;
CO_CANrxMsg_t rxMsg;
if (!buffer || !buffer->pFunct) {
LOG_ERR("failed to process CAN rx isr callback");
return;
}
rxMsg.ident = msg->id;
rxMsg.DLC = msg->dlc;
memcpy(rxMsg.data, msg->data, msg->dlc);
buffer->pFunct(buffer->object, &rxMsg);
}
static void canopen_tx_isr_callback(uint32_t error_flags, void *arg)
{
CO_CANmodule_t *CANmodule = arg;
if (!CANmodule) {
LOG_ERR("failed to process CAN tx isr callback");
return;
}
if (error_flags == CAN_TX_OK) {
CANmodule->first_tx_msg = false;
}
k_work_submit_to_queue(&canopen_tx_workq, &canopen_tx_queue.work);
}
static void canopen_tx_retry(struct k_work *item)
{
struct canopen_tx_work_container *container =
CONTAINER_OF(item, struct canopen_tx_work_container, work);
CO_CANmodule_t *CANmodule = container->CANmodule;
struct zcan_frame msg;
CO_CANtx_t *buffer;
int err;
uint16_t i;
CO_LOCK_CAN_SEND();
for (i = 0; i < CANmodule->tx_size; i++) {
buffer = &CANmodule->tx_array[i];
if (buffer->bufferFull) {
msg.id_type = CAN_STANDARD_IDENTIFIER;
msg.id = buffer->ident;
msg.dlc = buffer->DLC;
msg.rtr = (buffer->rtr ? 1 : 0);
memcpy(msg.data, buffer->data, buffer->DLC);
err = can_send(CANmodule->dev, &msg, K_NO_WAIT,
canopen_tx_isr_callback, CANmodule);
if (err == CAN_TIMEOUT) {
break;
} else if (err != CAN_TX_OK) {
LOG_ERR("failed to send CAN frame (err %d)",
err);
CO_errorReport(CANmodule->em,
CO_EM_GENERIC_SOFTWARE_ERROR,
CO_EMC_COMMUNICATION, 0);
}
buffer->bufferFull = false;
}
}
CO_UNLOCK_CAN_SEND();
}
void CO_CANsetConfigurationMode(void *CANdriverState)
{
/* No operation */
}
void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule)
{
CANmodule->CANnormal = true;
}
CO_ReturnError_t CO_CANmodule_init(CO_CANmodule_t *CANmodule,
void *CANdriverState,
CO_CANrx_t rxArray[], uint16_t rxSize,
CO_CANtx_t txArray[], uint16_t txSize,
uint16_t CANbitRate)
{
struct canopen_context *ctx = (struct canopen_context *)CANdriverState;
uint16_t i;
int err;
LOG_DBG("rxSize = %d, txSize = %d", rxSize, txSize);
if (!CANmodule || !rxArray || !txArray || !CANdriverState) {
LOG_ERR("failed to initialize CAN module");
return CO_ERROR_ILLEGAL_ARGUMENT;
}
if (rxSize > CONFIG_CAN_MAX_FILTER) {
LOG_ERR("insufficient number of concurrent CAN RX filters"
" (needs %d, %d available)", rxSize,
CONFIG_CAN_MAX_FILTER);
return CO_ERROR_OUT_OF_MEMORY;
} else if (rxSize < CONFIG_CAN_MAX_FILTER) {
LOG_DBG("excessive number of concurrent CAN RX filters enabled"
" (needs %d, %d available)", rxSize,
CONFIG_CAN_MAX_FILTER);
}
canopen_detach_all_rx_filters(CANmodule);
canopen_tx_queue.CANmodule = CANmodule;
CANmodule->dev = ctx->dev;
CANmodule->rx_array = rxArray;
CANmodule->rx_size = rxSize;
CANmodule->tx_array = txArray;
CANmodule->tx_size = txSize;
CANmodule->CANnormal = false;
CANmodule->first_tx_msg = true;
CANmodule->errors = 0;
CANmodule->em = NULL;
for (i = 0U; i < rxSize; i++) {
rxArray[i].ident = 0U;
rxArray[i].pFunct = NULL;
rxArray[i].filter_id = CAN_NO_FREE_FILTER;
}
for (i = 0U; i < txSize; i++) {
txArray[i].bufferFull = false;
}
err = can_set_bitrate(CANmodule->dev, KHZ(CANbitRate), 0);
if (err) {
LOG_ERR("failed to configure CAN bitrate (err %d)", err);
return CO_ERROR_ILLEGAL_ARGUMENT;
}
err = can_set_mode(CANmodule->dev, CAN_NORMAL_MODE);
if (err) {
LOG_ERR("failed to configure CAN interface (err %d)", err);
return CO_ERROR_ILLEGAL_ARGUMENT;
}
CANmodule->configured = true;
return CO_ERROR_NO;
}
void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
{
int err;
if (!CANmodule || !CANmodule->dev) {
return;
}
canopen_detach_all_rx_filters(CANmodule);
err = can_configure(CANmodule->dev, CAN_SILENT_MODE, 0);
if (err) {
LOG_ERR("failed to disable CAN interface (err %d)", err);
}
}
uint16_t CO_CANrxMsg_readIdent(const CO_CANrxMsg_t *rxMsg)
{
return rxMsg->ident;
}
CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index,
uint16_t ident, uint16_t mask, bool_t rtr,
void *object,
CO_CANrxBufferCallback_t pFunct)
{
struct zcan_filter filter;
CO_CANrx_t *buffer;
if (CANmodule == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
if (!pFunct || (index >= CANmodule->rx_size)) {
LOG_ERR("failed to initialize CAN rx buffer, illegal argument");
CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
CO_EMC_SOFTWARE_INTERNAL, 0);
return CO_ERROR_ILLEGAL_ARGUMENT;
}
buffer = &CANmodule->rx_array[index];
buffer->object = object;
buffer->pFunct = pFunct;
filter.id_type = CAN_STANDARD_IDENTIFIER;
filter.id = ident;
filter.id_mask = mask;
filter.rtr = (rtr ? 1 : 0);
filter.rtr_mask = 1;
if (buffer->filter_id != CAN_NO_FREE_FILTER) {
can_detach(CANmodule->dev, buffer->filter_id);
}
buffer->filter_id = can_attach_isr(CANmodule->dev,
canopen_rx_isr_callback,
buffer, &filter);
if (buffer->filter_id == CAN_NO_FREE_FILTER) {
LOG_ERR("failed to attach CAN rx isr, no free filter");
CO_errorReport(CANmodule->em, CO_EM_MEMORY_ALLOCATION_ERROR,
CO_EMC_SOFTWARE_INTERNAL, 0);
return CO_ERROR_OUT_OF_MEMORY;
}
return CO_ERROR_NO;
}
CO_CANtx_t *CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index,
uint16_t ident, bool_t rtr, uint8_t noOfBytes,
bool_t syncFlag)
{
CO_CANtx_t *buffer;
if (CANmodule == NULL) {
return NULL;
}
if (index >= CANmodule->tx_size) {
LOG_ERR("failed to initialize CAN rx buffer, illegal argument");
CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
CO_EMC_SOFTWARE_INTERNAL, 0);
return NULL;
}
buffer = &CANmodule->tx_array[index];
buffer->ident = ident;
buffer->rtr = rtr;
buffer->DLC = noOfBytes;
buffer->bufferFull = false;
buffer->syncFlag = syncFlag;
return buffer;
}
CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
{
CO_ReturnError_t ret = CO_ERROR_NO;
struct zcan_frame msg;
int err;
if (!CANmodule || !CANmodule->dev || !buffer) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
CO_LOCK_CAN_SEND();
if (buffer->bufferFull) {
if (!CANmodule->first_tx_msg) {
CO_errorReport(CANmodule->em, CO_EM_CAN_TX_OVERFLOW,
CO_EMC_CAN_OVERRUN, buffer->ident);
}
buffer->bufferFull = false;
ret = CO_ERROR_TX_OVERFLOW;
}
msg.id_type = CAN_STANDARD_IDENTIFIER;
msg.id = buffer->ident;
msg.dlc = buffer->DLC;
msg.rtr = (buffer->rtr ? 1 : 0);
memcpy(msg.data, buffer->data, buffer->DLC);
err = can_send(CANmodule->dev, &msg, K_NO_WAIT, canopen_tx_isr_callback,
CANmodule);
if (err == CAN_TIMEOUT) {
buffer->bufferFull = true;
} else if (err != CAN_TX_OK) {
LOG_ERR("failed to send CAN frame (err %d)", err);
CO_errorReport(CANmodule->em, CO_EM_GENERIC_SOFTWARE_ERROR,
CO_EMC_COMMUNICATION, 0);
ret = CO_ERROR_TX_UNCONFIGURED;
}
CO_UNLOCK_CAN_SEND();
return ret;
}
void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule)
{
bool_t tpdoDeleted = false;
CO_CANtx_t *buffer;
uint16_t i;
if (!CANmodule) {
return;
}
CO_LOCK_CAN_SEND();
for (i = 0; i < CANmodule->tx_size; i++) {
buffer = &CANmodule->tx_array[i];
if (buffer->bufferFull && buffer->syncFlag) {
buffer->bufferFull = false;
tpdoDeleted = true;
}
}
CO_UNLOCK_CAN_SEND();
if (tpdoDeleted) {
CO_errorReport(CANmodule->em, CO_EM_TPDO_OUTSIDE_WINDOW,
CO_EMC_COMMUNICATION, 0);
}
}
void CO_CANverifyErrors(CO_CANmodule_t *CANmodule)
{
CO_EM_t *em = (CO_EM_t *)CANmodule->em;
struct can_bus_err_cnt err_cnt;
enum can_state state;
uint8_t rx_overflows;
uint32_t errors;
/*
* TODO: Zephyr lacks an API for reading the rx mailbox
* overflow counter.
*/
rx_overflows = 0;
state = can_get_state(CANmodule->dev, &err_cnt);
errors = ((uint32_t)err_cnt.tx_err_cnt << 16) |
((uint32_t)err_cnt.rx_err_cnt << 8) |
rx_overflows;
if (errors != CANmodule->errors) {
CANmodule->errors = errors;
if (state == CAN_BUS_OFF) {
/* Bus off */
CO_errorReport(em, CO_EM_CAN_TX_BUS_OFF,
CO_EMC_BUS_OFF_RECOVERED, errors);
} else {
/* Bus not off */
CO_errorReset(em, CO_EM_CAN_TX_BUS_OFF, errors);
if ((err_cnt.rx_err_cnt >= 96U) ||
(err_cnt.tx_err_cnt >= 96U)) {
/* Bus warning */
CO_errorReport(em, CO_EM_CAN_BUS_WARNING,
CO_EMC_NO_ERROR, errors);
} else {
/* Bus not warning */
CO_errorReset(em, CO_EM_CAN_BUS_WARNING,
errors);
}
if (err_cnt.rx_err_cnt >= 128U) {
/* Bus rx passive */
CO_errorReport(em, CO_EM_CAN_RX_BUS_PASSIVE,
CO_EMC_CAN_PASSIVE, errors);
} else {
/* Bus not rx passive */
CO_errorReset(em, CO_EM_CAN_RX_BUS_PASSIVE,
errors);
}
if (err_cnt.tx_err_cnt >= 128U &&
!CANmodule->first_tx_msg) {
/* Bus tx passive */
CO_errorReport(em, CO_EM_CAN_TX_BUS_PASSIVE,
CO_EMC_CAN_PASSIVE, errors);
} else if (CO_isError(em, CO_EM_CAN_TX_BUS_PASSIVE)) {
/* Bus not tx passive */
CO_errorReset(em, CO_EM_CAN_TX_BUS_PASSIVE,
errors);
CO_errorReset(em, CO_EM_CAN_TX_OVERFLOW,
errors);
}
}
/* This code can be activated if we can read the overflows*/
if (false && rx_overflows != 0U) {
CO_errorReport(em, CO_EM_CAN_RXB_OVERFLOW,
CO_EMC_CAN_OVERRUN, errors);
}
}
}
static int canopen_init(const struct device *dev)
{
ARG_UNUSED(dev);
k_work_queue_start(&canopen_tx_workq, canopen_tx_workq_stack,
K_KERNEL_STACK_SIZEOF(canopen_tx_workq_stack),
CONFIG_CANOPEN_TX_WORKQUEUE_PRIORITY, NULL);
k_work_init(&canopen_tx_queue.work, canopen_tx_retry);
return 0;
}
SYS_INIT(canopen_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);

123
modules/canopennode/CO_driver_target.h Normal file
View file

@ -0,0 +1,123 @@
/*
* Copyright (c) 2019 Vestas Wind Systems A/S
*
* SPDX-License-Identifier: Apache-2.0
*/
#ifndef ZEPHYR_MODULES_CANOPENNODE_CO_DRIVER_H
#define ZEPHYR_MODULES_CANOPENNODE_CO_DRIVER_H
/*
* Zephyr RTOS CAN driver interface and configuration for CANopenNode
* CANopen protocol stack.
*
* See CANopenNode/stack/drvTemplate/CO_driver.h for API description.
*/
#ifdef __cplusplus
extern "C" {
#endif
#include <zephyr.h>
#include <zephyr/types.h>
#include <device.h>
#include <toolchain.h>
/* Use static variables instead of calloc() */
#define CO_USE_GLOBALS
/* Use Zephyr provided crc16 implementation */
#define CO_USE_OWN_CRC16
/* Use SDO buffer size from Kconfig */
#define CO_SDO_BUFFER_SIZE CONFIG_CANOPEN_SDO_BUFFER_SIZE
/* Use trace buffer size from Kconfig */
#define CO_TRACE_BUFFER_SIZE_FIXED CONFIG_CANOPEN_TRACE_BUFFER_SIZE
#ifdef CONFIG_CANOPEN_LEDS
#define CO_USE_LEDS 1
#endif
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
#define CO_LITTLE_ENDIAN
#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
#define CO_BIG_ENDIAN
#else
#error "Unsupported endianness"
#endif
typedef bool bool_t;
typedef float float32_t;
typedef long double float64_t;
typedef char char_t;
typedef unsigned char oChar_t;
typedef unsigned char domain_t;
typedef struct canopen_rx_msg {
uint8_t data[8];
uint16_t ident;
uint8_t DLC;
} CO_CANrxMsg_t;
typedef void (*CO_CANrxBufferCallback_t)(void *object,
const CO_CANrxMsg_t *message);
typedef struct canopen_rx {
int filter_id;
void *object;
CO_CANrxBufferCallback_t pFunct;
uint16_t ident;
} CO_CANrx_t;
typedef struct canopen_tx {
uint8_t data[8];
uint16_t ident;
uint8_t DLC;
bool_t rtr : 1;
bool_t bufferFull : 1;
bool_t syncFlag : 1;
} CO_CANtx_t;
typedef struct canopen_module {
const struct device *dev;
CO_CANrx_t *rx_array;
CO_CANtx_t *tx_array;
uint16_t rx_size;
uint16_t tx_size;
uint32_t errors;
void *em;
bool_t configured : 1;
bool_t CANnormal : 1;
bool_t first_tx_msg : 1;
} CO_CANmodule_t;
void canopen_send_lock(void);
void canopen_send_unlock(void);
#define CO_LOCK_CAN_SEND() canopen_send_lock()
#define CO_UNLOCK_CAN_SEND() canopen_send_unlock()
void canopen_emcy_lock(void);
void canopen_emcy_unlock(void);
#define CO_LOCK_EMCY() canopen_emcy_lock()
#define CO_UNLOCK_EMCY() canopen_emcy_unlock()
void canopen_od_lock(void);
void canopen_od_unlock(void);
#define CO_LOCK_OD() canopen_od_lock()
#define CO_UNLOCK_OD() canopen_od_unlock()
/*
* CANopenNode RX callbacks run in interrupt context, no memory
* barrier needed.
*/
#define CANrxMemoryBarrier()
#define IS_CANrxNew(rxNew) ((uintptr_t)rxNew)
#define SET_CANrxNew(rxNew) { CANrxMemoryBarrier(); rxNew = (void *)1L; }
#define CLEAR_CANrxNew(rxNew) { CANrxMemoryBarrier(); rxNew = (void *)0L; }
#ifdef __cplusplus
}
#endif
#endif /* ZEPHYR_MODULES_CANOPENNODE_CO_DRIVER_H */

112
modules/canopennode/Kconfig Normal file
View file

@ -0,0 +1,112 @@
# CANopenNode CANopen protocol stack configuration options
# Copyright (c) 2019 Vestas Wind Systems A/S
# SPDX-License-Identifier: Apache-2.0
config ZEPHYR_CANOPENNODE_MODULE
bool
config CANOPENNODE
bool "CANopenNode support"
depends on CAN
help
This option enables the CANopenNode library.
if CANOPENNODE
config CANOPEN_SDO_BUFFER_SIZE
int "CANopen SDO buffer size"
default 32
range 7 889
help
Size of the internal CANopen SDO buffer in bytes. Size must
be at least equal to the size of the largest variable in the
object dictionary. If data type is DOMAIN, data length is
not limited to the SDO buffer size. If block transfer is
implemented, value should be set to 889.
config CANOPEN_TRACE_BUFFER_SIZE
int "CANopen trace buffer size"
default 100
help
Size of the CANopen trace buffer in bytes.
config CANOPEN_TX_WORKQUEUE_STACK_SIZE
int "Stack size for the CANopen transmit workqueue"
default 320
help
Size of the stack used for the internal CANopen transmit
workqueue.
config CANOPEN_TX_WORKQUEUE_PRIORITY
int "Priority for CANopen transmit workqueue"
default 0 if !COOP_ENABLED
default -1
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
help
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
help
Enable internal thread for processing CANopen SYNC RPDOs and
TPDOs. Application layer must take care of SYNC RPDO and
TPDO processing on its own if this is disabled.
config CANOPEN_SYNC_THREAD_STACK_SIZE
int "Stack size for the CANopen SYNC thread"
depends on CANOPEN_SYNC_THREAD
default 256
help
Size of the stack used for the internal thread which
processes CANopen SYNC RPDOs and TPDOs.
config CANOPEN_SYNC_THREAD_PRIORITY
int "Priority for CANopen SYNC thread"
depends on CANOPEN_SYNC_THREAD
default 0 if !COOP_ENABLED
default -5
help
Priority level of the internal thread which processes
CANopen SYNC RPDOs and TPDOs.
config CANOPEN_PROGRAM_DOWNLOAD
bool "CANopen program download"
depends on BOOTLOADER_MCUBOOT
select IMG_MANAGER
default y
help
Enable support for program download over CANopen according
to the CiA 302-3 (draft) specification.
endif # CANOPENNODE

156
modules/canopennode/canbus/canopen.h Normal file
View file

@ -0,0 +1,156 @@
/*
* Copyright (c) 2019 Vestas Wind Systems A/S
*
* SPDX-License-Identifier: Apache-2.0
*/
/**
* @defgroup CAN CAN BUS
* @{
* @}
*/
/**
* @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,
};
struct canopen_context {
const struct device *dev;
};
/**
* @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);
/**
* @brief Attach CANopen object dictionary program download handlers.
*
* Attach CANopen program download functions to object dictionary
* indexes 0x1F50, 0x1F51, 0x1F56, and 0x1F57. This function must be
* called after calling CANopenNode `CO_init()`.
*
* @param nmt CANopenNode NMT object
* @param sdo CANopenNode SDO server object
* @param em CANopenNode Emergency object
*/
void canopen_program_download_attach(CO_NMT_t *nmt, CO_SDO_t *sdo, CO_EM_t *em);
/**
* @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);
/**
* @brief Indicate CANopen program download in progress
*
* Indicate that a CANopen program download is in progress.
*
* @param in_progress true if program download is in progress, false otherwise
*/
void canopen_leds_program_download(bool in_progress);
#ifdef __cplusplus
}
#endif
/**
* @}
*/
#endif /* ZEPHYR_INCLUDE_CANOPEN_H_ */

102
modules/canopennode/canopen_leds.c Normal file
View file

@ -0,0 +1,102 @@
/*
* 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;
bool program_download : 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);
if (canopen_leds.program_download) {
green = LED_TRIPLE_FLASH(canopen_leds.nmt);
} else {
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));
}
}
void canopen_leds_program_download(bool in_progress)
{
canopen_leds.program_download = in_progress;
}

439
modules/canopennode/canopen_program.c Normal file
View file

@ -0,0 +1,439 @@
/*
* Copyright (c) 2020 Vestas Wind Systems A/S
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <CANopen.h>
#include <canbus/canopen.h>
#include <dfu/flash_img.h>
#include <dfu/mcuboot.h>
#include <storage/flash_map.h>
#include <sys/crc.h>
#define LOG_LEVEL CONFIG_CANOPEN_LOG_LEVEL
#include <logging/log.h>
LOG_MODULE_REGISTER(canopen_program);
/* Object dictionary indexes */
#define OD_H1F50_PROGRAM_DATA 0x1F50
#define OD_H1F51_PROGRAM_CTRL 0x1F51
#define OD_H1F56_PROGRAM_SWID 0x1F56
#define OD_H1F57_FLASH_STATUS 0x1F57
/* Common program control commands and status */
#define PROGRAM_CTRL_STOP 0x00
#define PROGRAM_CTRL_START 0x01
#define PROGRAM_CTRL_RESET 0x02
#define PROGRAM_CTRL_CLEAR 0x03
/* Zephyr specific program control and status */
#define PROGRAM_CTRL_ZEPHYR_CONFIRM 0x80
/* Flash status bits */
#define FLASH_STATUS_IN_PROGRESS BIT(0)
/* Flash common error bits values */
#define FLASH_STATUS_NO_ERROR (0U << 1U)
#define FLASH_STATUS_NO_VALID_PROGRAM (1U << 1U)
#define FLASH_STATUS_DATA_FORMAT_UNKNOWN (2U << 1U)
#define FLASH_STATUS_DATA_FORMAT_ERROR (3U << 1U)
#define FLASH_STATUS_FLASH_NOT_CLEARED (4U << 1U)
#define FLASH_STATUS_FLASH_WRITE_ERROR (5U << 1U)
#define FLASH_STATUS_GENERAL_ADDR_ERROR (6U << 1U)
#define FLASH_STATUS_FLASH_SECURED (7U << 1U)
#define FLASH_STATUS_UNSPECIFIED_ERROR (63U << 1)
struct canopen_program_context {
uint32_t flash_status;
size_t total;
CO_NMT_t *nmt;
CO_EM_t *em;
struct flash_img_context flash_img_ctx;
uint8_t program_status;
bool flash_written;
};
static struct canopen_program_context ctx;
static void canopen_program_set_status(uint32_t status)
{
ctx.program_status = status;
}
static uint32_t canopen_program_get_status(void)
{
/*
* Non-confirmed boot image takes precedence over other
* status. This must be checked on every invocation since the
* app may be using other means of confirming the image.
*/
if (!boot_is_img_confirmed()) {
return PROGRAM_CTRL_ZEPHYR_CONFIRM;
}
return ctx.program_status;
}
static CO_SDO_abortCode_t canopen_odf_1f50(CO_ODF_arg_t *odf_arg)
{
int err;
if (odf_arg->subIndex != 1U) {
return CO_SDO_AB_NONE;
}
if (odf_arg->reading) {
return CO_SDO_AB_WRITEONLY;
}
if (canopen_program_get_status() != PROGRAM_CTRL_CLEAR) {
ctx.flash_status = FLASH_STATUS_FLASH_NOT_CLEARED;
return CO_SDO_AB_DATA_DEV_STATE;
}
if (odf_arg->firstSegment) {
err = flash_img_init(&ctx.flash_img_ctx);
if (err) {
LOG_ERR("failed to initialize flash img (err %d)", err);
CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
CO_EMC_HARDWARE, err);
ctx.flash_status = FLASH_STATUS_FLASH_WRITE_ERROR;
return CO_SDO_AB_HW;
}
ctx.flash_status = FLASH_STATUS_IN_PROGRESS;
if (IS_ENABLED(CONFIG_CANOPEN_LEDS)) {
canopen_leds_program_download(true);
}
ctx.total = odf_arg->dataLengthTotal;
LOG_DBG("total = %d", ctx.total);
}
err = flash_img_buffered_write(&ctx.flash_img_ctx, odf_arg->data,
odf_arg->dataLength,
odf_arg->lastSegment);
if (err) {
CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
CO_EMC_HARDWARE, err);
ctx.flash_status = FLASH_STATUS_FLASH_WRITE_ERROR;
canopen_leds_program_download(false);
return CO_SDO_AB_HW;
}
if (odf_arg->lastSegment) {
/* ctx.total is zero if not provided by download process */
if (ctx.total != 0 &&
ctx.total != flash_img_bytes_written(&ctx.flash_img_ctx)) {
LOG_WRN("premature end of program download");
ctx.flash_status = FLASH_STATUS_DATA_FORMAT_ERROR;
} else {
LOG_DBG("program downloaded");
ctx.flash_written = true;
ctx.flash_status = FLASH_STATUS_NO_ERROR;
}
canopen_program_set_status(PROGRAM_CTRL_STOP);
canopen_leds_program_download(false);
}
return CO_SDO_AB_NONE;
}
static inline CO_SDO_abortCode_t canopen_program_cmd_stop(void)
{
if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {
return CO_SDO_AB_DATA_DEV_STATE;
}
LOG_DBG("program stopped");
canopen_program_set_status(PROGRAM_CTRL_STOP);
return CO_SDO_AB_NONE;
}
static inline CO_SDO_abortCode_t canopen_program_cmd_start(void)
{
int err;
if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {
return CO_SDO_AB_DATA_DEV_STATE;
}
if (ctx.flash_written) {
LOG_DBG("requesting upgrade and reset");
err = boot_request_upgrade(BOOT_UPGRADE_TEST);
if (err) {
LOG_ERR("failed to request upgrade (err %d)", err);
CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
CO_EMC_HARDWARE, err);
return CO_SDO_AB_HW;
}
ctx.nmt->resetCommand = CO_RESET_APP;
} else {
LOG_DBG("program started");
canopen_program_set_status(PROGRAM_CTRL_START);
}
return CO_SDO_AB_NONE;
}
static inline CO_SDO_abortCode_t canopen_program_cmd_clear(void)
{
int err;
if (canopen_program_get_status() != PROGRAM_CTRL_STOP) {
return CO_SDO_AB_DATA_DEV_STATE;
}
if (!IS_ENABLED(CONFIG_IMG_ERASE_PROGRESSIVELY)) {
LOG_DBG("erasing flash area");
err = boot_erase_img_bank(FLASH_AREA_ID(image_1));
if (err) {
LOG_ERR("failed to erase image bank (err %d)", err);
CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
CO_EMC_HARDWARE, err);
return CO_SDO_AB_HW;
}
}
LOG_DBG("program cleared");
canopen_program_set_status(PROGRAM_CTRL_CLEAR);
ctx.flash_status = FLASH_STATUS_NO_ERROR;
ctx.flash_written = false;
return CO_SDO_AB_NONE;
}
static inline CO_SDO_abortCode_t canopen_program_cmd_confirm(void)
{
int err;
if (canopen_program_get_status() == PROGRAM_CTRL_ZEPHYR_CONFIRM) {
err = boot_write_img_confirmed();
if (err) {
LOG_ERR("failed to confirm image (err %d)", err);
CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
CO_EMC_HARDWARE, err);
return CO_SDO_AB_HW;
}
LOG_DBG("program confirmed");
canopen_program_set_status(PROGRAM_CTRL_START);
}
return CO_SDO_AB_NONE;
}
static CO_SDO_abortCode_t canopen_odf_1f51(CO_ODF_arg_t *odf_arg)
{
CO_SDO_abortCode_t ab;
uint8_t cmd;
if (odf_arg->subIndex != 1U) {
return CO_SDO_AB_NONE;
}
if (odf_arg->reading) {
odf_arg->data[0] = canopen_program_get_status();
return CO_SDO_AB_NONE;
}
if (CO_NMT_getInternalState(ctx.nmt) != CO_NMT_PRE_OPERATIONAL) {
LOG_DBG("not in pre-operational state");
return CO_SDO_AB_DATA_DEV_STATE;
}
/* Preserve old value */
cmd = odf_arg->data[0];
memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint8_t));
LOG_DBG("program status = %d, cmd = %d", canopen_program_get_status(),
cmd);
switch (cmd) {
case PROGRAM_CTRL_STOP:
ab = canopen_program_cmd_stop();
break;
case PROGRAM_CTRL_START:
ab = canopen_program_cmd_start();
break;
case PROGRAM_CTRL_CLEAR:
ab = canopen_program_cmd_clear();
break;
case PROGRAM_CTRL_ZEPHYR_CONFIRM:
ab = canopen_program_cmd_confirm();
break;
case PROGRAM_CTRL_RESET:
__fallthrough;
default:
LOG_DBG("unsupported command '%d'", cmd);
ab = CO_SDO_AB_INVALID_VALUE;
}
return ab;
}
#ifdef CONFIG_BOOTLOADER_MCUBOOT
/** @brief Calculate crc for region in flash
*
* @param flash_area Flash area to read from, must be open
* @offset Offset to read from
* @size Number of bytes to include in calculation
* @pcrc Pointer to uint32_t where crc will be written if return value is 0
*
* @return 0 if successful, negative errno on failure
*/
static int flash_crc(const struct flash_area *flash_area,
off_t offset, size_t size, uint32_t *pcrc)
{
uint32_t crc = 0;
uint8_t buffer[32];
while (size > 0) {
size_t len = MIN(size, sizeof(buffer));
int err = flash_area_read(flash_area, offset, buffer, len);
if (err) {
return err;
}
crc = crc32_ieee_update(crc, buffer, len);
offset += len;
size -= len;
}
*pcrc = crc;
return 0;
}
static CO_SDO_abortCode_t canopen_odf_1f56(CO_ODF_arg_t *odf_arg)
{
const struct flash_area *flash_area;
struct mcuboot_img_header header;
off_t offset = 0;
uint32_t crc = 0;
uint8_t fa_id;
uint32_t len;
int err;
if (odf_arg->subIndex != 1U) {
return CO_SDO_AB_NONE;
}
if (!odf_arg->reading) {
/* Preserve old value */
memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint32_t));
return CO_SDO_AB_READONLY;
}
/* Reading from flash and calculating crc can take 100ms or more, and
* this function is called with the can od lock taken.
*
* Release the lock before performing time consuming work, and reacquire
* before return.
*/
CO_UNLOCK_OD();
/*
* Calculate the CRC32 of the image that is running or will be
* started upon receiveing the next 'start' command.
*/
if (ctx.flash_written) {
fa_id = FLASH_AREA_ID(image_1);
} else {
fa_id = FLASH_AREA_ID(image_0);
}
err = boot_read_bank_header(fa_id, &header, sizeof(header));
if (err) {
LOG_WRN("failed to read bank header (err %d)", err);
CO_setUint32(odf_arg->data, 0U);
CO_LOCK_OD();
return CO_SDO_AB_NONE;
}
if (header.mcuboot_version != 1) {
LOG_WRN("unsupported mcuboot header version %d",
header.mcuboot_version);
CO_setUint32(odf_arg->data, 0U);
CO_LOCK_OD();
return CO_SDO_AB_NONE;
}
len = header.h.v1.image_size;
err = flash_area_open(fa_id, &flash_area);
if (err) {
LOG_ERR("failed to open flash area (err %d)", err);
CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
CO_EMC_HARDWARE, err);
CO_LOCK_OD();
return CO_SDO_AB_HW;
}
err = flash_crc(flash_area, offset, len, &crc);
flash_area_close(flash_area);
if (err) {
LOG_ERR("failed to read flash (err %d)", err);
CO_errorReport(ctx.em, CO_EM_NON_VOLATILE_MEMORY,
CO_EMC_HARDWARE, err);
CO_LOCK_OD();
return CO_SDO_AB_HW;
}
CO_setUint32(odf_arg->data, crc);
CO_LOCK_OD();
return CO_SDO_AB_NONE;
}
#endif /* CONFIG_BOOTLOADER_MCUBOOT */
static CO_SDO_abortCode_t canopen_odf_1f57(CO_ODF_arg_t *odf_arg)
{
if (odf_arg->subIndex != 1U) {
return CO_SDO_AB_NONE;
}
if (!odf_arg->reading) {
/* Preserve old value */
memcpy(odf_arg->data, odf_arg->ODdataStorage, sizeof(uint32_t));
return CO_SDO_AB_READONLY;
}
CO_setUint32(odf_arg->data, ctx.flash_status);
return CO_SDO_AB_NONE;
}
void canopen_program_download_attach(CO_NMT_t *nmt, CO_SDO_t *sdo, CO_EM_t *em)
{
canopen_program_set_status(PROGRAM_CTRL_START);
ctx.flash_status = FLASH_STATUS_NO_ERROR;
ctx.flash_written = false;
ctx.nmt = nmt;
ctx.em = em;
CO_OD_configure(sdo, OD_H1F50_PROGRAM_DATA, canopen_odf_1f50,
NULL, 0U, 0U);
CO_OD_configure(sdo, OD_H1F51_PROGRAM_CTRL, canopen_odf_1f51,
NULL, 0U, 0U);
if (IS_ENABLED(CONFIG_BOOTLOADER_MCUBOOT)) {
CO_OD_configure(sdo, OD_H1F56_PROGRAM_SWID, canopen_odf_1f56,
NULL, 0U, 0U);
}
CO_OD_configure(sdo, OD_H1F57_FLASH_STATUS, canopen_odf_1f57,
NULL, 0U, 0U);
}

231
modules/canopennode/canopen_storage.c Normal file
View 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;
uint32_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(uint32_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;
uint32_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(uint32_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 CONFIG_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;
ssize_t 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;
}

50
modules/canopennode/canopen_sync.c Normal file
View file

@ -0,0 +1,50 @@
/*
* Copyright (c) 2019 Vestas Wind Systems A/S
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <CANopen.h>
/**
* @brief CANopen sync thread.
*
* The CANopen real-time sync thread processes SYNC RPDOs and TPDOs
* through the CANopenNode stack with an interval of 1 millisecond.
*
* @param p1 Unused
* @param p2 Unused
* @param p3 Unused
*/
static void canopen_sync_thread(void *p1, void *p2, void *p3)
{
uint32_t start; /* cycles */
uint32_t stop; /* cycles */
uint32_t delta; /* cycles */
uint32_t elapsed = 0; /* microseconds */
bool sync;
ARG_UNUSED(p1);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
while (true) {
start = k_cycle_get_32();
if (CO && CO->CANmodule[0] && CO->CANmodule[0]->CANnormal) {
CO_LOCK_OD();
sync = CO_process_SYNC(CO, elapsed);
CO_process_RPDO(CO, sync);
CO_process_TPDO(CO, sync, elapsed);
CO_UNLOCK_OD();
}
k_sleep(K_MSEC(1));
stop = k_cycle_get_32();
delta = stop - start;
elapsed = (uint32_t)k_cyc_to_ns_floor64(delta) / NSEC_PER_USEC;
}
}
K_THREAD_DEFINE(canopen_sync, CONFIG_CANOPEN_SYNC_THREAD_STACK_SIZE,
canopen_sync_thread, NULL, NULL, NULL,
CONFIG_CANOPEN_SYNC_THREAD_PRIORITY, 0, 1);