From ed861444276491b82964ee4ddd03c39e39f0a473 Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Tue, 13 Oct 2020 09:59:30 +0200 Subject: [PATCH] Add Configuration Manager functionality --- CMakeLists.txt | 6 + co_options.h.in | 4 + include/co_api.h | 33 ++ include/co_obj.h | 299 +++++++++++++++++ options.h.in | 4 + src/CMakeLists.txt | 2 + src/co_main.c | 27 ++ src/co_main.h | 27 +- src/co_mngr.c | 791 ++++++++++++++++++++++++++++++++++++++++++++ src/co_mngr.h | 162 +++++++++ src/co_node_guard.c | 99 ++++-- src/co_obj.c | 89 +++++ test/CMakeLists.txt | 2 + test/mocks.cpp | 36 ++ test/mocks.h | 10 + test/test_mngr.cpp | 411 +++++++++++++++++++++++ 16 files changed, 1976 insertions(+), 26 deletions(-) create mode 100644 src/co_mngr.c create mode 100644 src/co_mngr.h create mode 100644 test/test_mngr.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1caa504..55468fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,6 +65,9 @@ set_property(CACHE CO_NODE_GUARD_LOG PROPERTY STRINGS ${LOG_STATE_VALUES}) set(CO_LSS_LOG ON CACHE STRING "log lss events") set_property(CACHE CO_LSS_LOG PROPERTY STRINGS ${LOG_STATE_VALUES}) +set(CO_CONF_MNGR_LOG ON CACHE STRING "log mngr events") +set_property(CACHE CO_CONF_MNGR_LOG PROPERTY STRINGS ${LOG_STATE_VALUES}) + set(MAX_NODES "16" CACHE STRING "max number of statically allocated nodes") @@ -92,6 +95,9 @@ set(SDO_TIMEOUT "100" set(CO_THREAD_PRIO "10" CACHE STRING "priority of main thread") +set(CO_CONF_MNGR "127" + CACHE STRING "number of supported slaves in configuration manager") + set(CO_THREAD_STACK_SIZE "4096" CACHE STRING "stack size of main thread") diff --git a/co_options.h.in b/co_options.h.in index ff7456c..9b58ddc 100644 --- a/co_options.h.in +++ b/co_options.h.in @@ -44,4 +44,8 @@ #define MAX_ERRORS (@MAX_ERRORS@) #endif +#ifndef CO_CONF_MNGR +#define CO_CONF_MNGR (@CO_CONF_MNGR@) +#endif + #endif /* CO_OPTIONS_H */ diff --git a/include/co_api.h b/include/co_api.h index e0b6c3f..d50811c 100644 --- a/include/co_api.h +++ b/include/co_api.h @@ -172,6 +172,28 @@ typedef enum co_dtype DTYPE_IDENTITY = 0x0023, } co_dtype_t; +#if CO_CONF_MNGR > 1 +typedef enum co_mngr_status +{ + ERROR_STATUS_OK, + ERROR_STATUS_A, + ERROR_STATUS_B, + ERROR_STATUS_C, + ERROR_STATUS_D, + ERROR_STATUS_E, + ERROR_STATUS_F, + ERROR_STATUS_G, + ERROR_STATUS_H, + ERROR_STATUS_I, + ERROR_STATUS_J, + ERROR_STATUS_K, + ERROR_STATUS_L, + ERROR_STATUS_M, + ERROR_STATUS_N, + ERROR_STATUS_O, +} co_mngr_status_t; +#endif + /* Entry flags */ #define OD_READ (1U << 0) /**< Entry is readable */ #define OD_WRITE (1U << 1) /**< Entry is writeable */ @@ -284,6 +306,13 @@ typedef struct co_cfg /** Function to close dictionary store */ int (*close)(void * arg); + +#if CO_CONF_MNGR > 1 + /** Function to write dcf configuration to slaves */ + co_mngr_status_t (*cb_write_dcf) (void * arg, uint8_t node); +#endif + + int num_devices; } co_cfg_t; /** @@ -459,6 +488,10 @@ CO_EXPORT int co_error_clear (co_client_t * client, uint8_t mask); */ CO_EXPORT int co_error_get (co_client_t * client, uint8_t * error); +CO_EXPORT co_mngr_status_t co_nmt_mngr_startup (co_client_t * client); + +CO_EXPORT co_mngr_status_t co_nmt_mngr_startup_node (co_client_t * client, uint8_t node); + #ifdef __cplusplus } #endif diff --git a/include/co_obj.h b/include/co_obj.h index fa6ae77..22d1224 100644 --- a/include/co_obj.h +++ b/include/co_obj.h @@ -91,6 +91,45 @@ extern const co_entry_t OD1800[]; /** Entry descriptor for RPDO mapping parameter object (1A00h - 1BFFh) */ extern const co_entry_t OD1A00[]; +/** Entry descriptor for NMT inhibit time value object (102Ah) */ +extern const co_entry_t OD102A[]; + +/** Entry descriptor for Expected configuration date value object (1F26h) */ +extern const co_entry_t OD1F26[]; + +/** Entry descriptor for Expected configuration time value object (1F27h) */ +extern const co_entry_t OD1F27[]; + +/** Entry descriptor for NMT startup value object (1F80h) */ +extern const co_entry_t OD1F80[]; + +/** Entry descriptor for NMT slave assignment value object (1F81h) */ +extern const co_entry_t OD1F81[]; + +/** Entry descriptor for Request NMT value object (1F82h) */ +extern const co_entry_t OD1F82[]; + +/** Entry descriptor for Request node guarding value object (1F83h) */ +extern const co_entry_t OD1F83[]; + +/** Entry descriptor for Device type identification value object (1F84h) */ +extern const co_entry_t OD1F84[]; + +/** Entry descriptor for Vendor identification value object (1F85h) */ +extern const co_entry_t OD1F85[]; + +/** Entry descriptor for Product code value object (1F86h) */ +extern const co_entry_t OD1F86[]; + +/** Entry descriptor for Revision number value object (1F87h) */ +extern const co_entry_t OD1F87[]; + +/** Entry descriptor for Serial number value object (1F88h) */ +extern const co_entry_t OD1F88[];; + +/** Entry descriptor for Boot time value object (1F89h) */ +extern const co_entry_t OD1F89[]; + /** * Access function for Error register object (1001h) * @@ -519,6 +558,266 @@ uint32_t co_od1A00_fn ( uint8_t subindex, uint32_t * value); +/** + * Access function for NMT inhibit time value object (102Ah) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od102a_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Expected configuration date value object (1F26h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f26_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Expected configuration time value object (1F27h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f27_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for NMT startup value object (1F80h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f80_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for NMT slave assignment value object (1F81h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f81_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Request NMT value object (1F82h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f82_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Request node guarding value object (1F83h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f83_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Device type identification value object (1F84h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f84_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Vendor identification value object (1F85h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f85_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Product code value object (1F86h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f86_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Revision number value object (1F87h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f87_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Serial number value object (1F88h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f88_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +/** + * Access function for Boot time value object (1F89h) + * + * @param net network handle + * @param event read/write/restore + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param value value to read or write + * + * @return sdo abort code + */ +uint32_t co_od1f89_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + #ifdef __cplusplus } #endif diff --git a/options.h.in b/options.h.in index a877330..815ff8a 100644 --- a/options.h.in +++ b/options.h.in @@ -70,4 +70,8 @@ #define CO_THREAD_STACK_SIZE (@CO_THREAD_STACK_SIZE@) #endif +#ifndef CO_CONF_MNGR_LOG +#define CO_CONF_MNGR_LOG (LOG_STATE_@CO_CONF_MNGR_LOG@) +#endif + #endif /* OPTIONS_H */ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a4c4c5c..e572f19 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -42,4 +42,6 @@ target_sources (canopen PRIVATE co_node_guard.c co_node_guard.h co_obj.c + co_mngr.c + co_mngr.h ) diff --git a/src/co_main.c b/src/co_main.c index 75d62ad..9633c53 100644 --- a/src/co_main.c +++ b/src/co_main.c @@ -182,6 +182,29 @@ static void co_job_callback (co_job_t * job) os_sem_signal (client->sem); } +void co_nmt_rtr(co_net_t * net, uint8_t node) +{ + uint8_t _data[1]; + os_channel_send (net->channel, CO_RTR_MASK + CO_FUNCTION_NMT_ERR + node, _data, 1); +} + +/* TODO: issue nmt job? */ +void co_nmt_net (co_net_t * net, co_nmt_cmd_t cmd, uint8_t node) +{ + uint8_t data[] = { cmd, node }; + + if ((net->node == node)) + { + co_nmt_rx(net, node, data, sizeof(data)); + return; + } + else if (node == 0) + { + co_nmt_rx(net, node, data, sizeof(data)); + } + os_channel_send (net->channel, CO_FUNCTION_NMT, data, sizeof(data)); +} + /* TODO: issue nmt job? */ void co_nmt (co_client_t * client, co_nmt_cmd_t cmd, uint8_t node) { @@ -382,6 +405,10 @@ co_net_t * co_init (const char * canif, const co_cfg_t * cfg) net->write = cfg->write; net->close = cfg->close; +#if CO_CONF_MNGR > 0 + net->cb_write_dcf = cfg->cb_write_dcf; +#endif + net->job_periodic = CO_JOB_PERIODIC; net->job_rx = CO_JOB_RX; diff --git a/src/co_main.h b/src/co_main.h index fada7bc..684afb5 100644 --- a/src/co_main.h +++ b/src/co_main.h @@ -40,7 +40,7 @@ extern "C" #define CO_EXT_MASK BIT (29) #define CO_ID_MASK 0x1FFFFFFF -#define CO_COBID_INVALID BIT (31) +#define CO_COBID_INVALID (uint32_t)BIT (31) #define CO_NODE_GET(id) ((id) & 0x7F) /* Pre-defined connection set (see CiA 301 chapter 7.3.3) */ @@ -275,8 +275,33 @@ struct co_net /** Function to close dictionary store */ int (*close)(void * arg); + +#if CO_CONF_MNGR > 1 + uint32_t conf_exp_conf_date[CO_CONF_MNGR]; + uint32_t conf_exp_conf_time[CO_CONF_MNGR]; + + uint16_t nmt_inhibit; + uint32_t nmt_startup; + uint32_t nmt_slave_assignment[CO_CONF_MNGR]; + uint8_t nmt_request[CO_CONF_MNGR]; + uint8_t nmt_request_node_guard[CO_CONF_MNGR]; + uint32_t nmt_node_ts_resp[CO_CONF_MNGR]; + uint32_t nmt_node_ts_req[CO_CONF_MNGR]; + uint32_t nmt_slave_device_type[CO_CONF_MNGR]; + uint32_t nmt_slave_vendor_id[CO_CONF_MNGR]; + uint32_t nmt_slave_prod_code[CO_CONF_MNGR]; + uint32_t nmt_slave_rev_num[CO_CONF_MNGR]; + uint32_t nmt_slave_ser_num[CO_CONF_MNGR]; + uint32_t nmt_slave_boot_time[CO_CONF_MNGR]; + + /** Function to write dcf configuration to slaves */ + co_mngr_status_t (*cb_write_dcf) (void * arg, uint8_t node); +#endif }; +void co_nmt_rtr (co_net_t * net, uint8_t node); +void co_nmt_net (co_net_t * net, co_nmt_cmd_t cmd, uint8_t node); + #ifdef __cplusplus } #endif diff --git a/src/co_mngr.c b/src/co_mngr.c new file mode 100644 index 0000000..b848534 --- /dev/null +++ b/src/co_mngr.c @@ -0,0 +1,791 @@ +/********************************************************************* + * _ _ _ + * _ __ | |_ _ | | __ _ | |__ ___ + * | '__|| __|(_)| | / _` || '_ \ / __| + * | | | |_ _ | || (_| || |_) |\__ \ + * |_| \__|(_)|_| \__,_||_.__/ |___/ + * + * http://www.rt-labs.com + * Copyright 2017 rt-labs AB, Sweden. + * See LICENSE file in the project root for full license information. + ********************************************************************/ + +#ifdef UNIT_TEST +#define co_sdo_read mock_co_sdo_read +#define co_nmt_net mock_co_nmt_net +#define co_nmt mock_co_nmt +#define os_channel_send mock_os_channel_send +#define os_channel_bus_off mock_os_channel_bus_off +#define os_channel_bus_on mock_os_channel_bus_on +#define os_channel_set_bitrate mock_os_channel_set_bitrate +#define os_channel_set_filter mock_os_channel_set_filter +#define co_od_restore mock_co_od_restore +#endif + +#include "co_mngr.h" +#include +#include "co_main.h" +#include "co_od.h" +#include "co_util.h" +#include "co_sdo.h" +#include "options.h" +#include "osal.h" +#include "log.h" + +#include +#include + +#if CO_CONF_MNGR > 0 + +uint32_t co_od102a_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_inhibit; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_inhibit = *value; + } + + return 0; +} + +uint32_t co_od1f26_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->conf_exp_conf_date[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->conf_exp_conf_date[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f27_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->conf_exp_conf_time[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->conf_exp_conf_time[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f80_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_startup; + } + else if (event == OD_EVENT_WRITE) + { + if ((*value & ~CO_MNGR_NMT_STARTUP_SUPPORT) != 0) + { + /* Unsupported configuration */ + return CO_SDO_ABORT_VALUE; + } + net->nmt_startup = *value; + } + return 0; +} + +uint32_t co_od1f81_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_slave_assignment[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_slave_assignment[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f82_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + if (subindex == 128) + { + return CO_SDO_ABORT_ACCESS_WO; + } + else + { + *value = net->nmt_request[subindex - 1]; + } + } + else if (event == OD_EVENT_WRITE) + { + co_nmt_cmd_t nmt_cmd; + + //TODO create macro for states + switch(*value) + { + case 5: nmt_cmd = CO_NMT_OPERATIONAL; break; + case 4: nmt_cmd = CO_NMT_STOPPED; break; + case 127: nmt_cmd = CO_NMT_PRE_OPERATIONAL; break; + case 6: nmt_cmd = CO_NMT_RESET_NODE; break; + case 7: nmt_cmd = CO_NMT_RESET_COMMUNICATION; break; + default: + LOG_ERROR (CO_NMT_LOG, "bad nmt request command %"PRIu32"\n", *value); + return CO_SDO_ABORT_VALUE; + } + + if (subindex == 128) + { + co_nmt_net (net, nmt_cmd , 0); + } + else + { + co_nmt_net (net, nmt_cmd , subindex); + } + } + return 0; +} + +static void co_od1f83_activate_node_guarding (co_net_t * net, uint8_t node) +{ + net->nmt_node_ts_resp[node - 1] = os_get_current_time_us(); + net->nmt_request_node_guard[node - 1] = 1; +} + +uint32_t co_od1f83_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + if (subindex == 128) + { + return CO_SDO_ABORT_ACCESS_WO; + } + else + { + *value = net->nmt_request_node_guard[subindex - 1]; + } + } + else if (event == OD_EVENT_WRITE) + { + if (subindex == 128) + { + uint8_t i; + + for (i = 0; i < CO_CONF_MNGR; i++) + { + if (*value == 1) + { + co_od1f83_activate_node_guarding (net, i + 1); + } + else + { + return CO_SDO_ABORT_VALUE; + } + } + } + else if (net->node != subindex) + { + if (*value == 1) + { + co_od1f83_activate_node_guarding (net, subindex); + } + else + { + return CO_SDO_ABORT_VALUE; + } + } + } + return 0; +} + +uint32_t co_od1f84_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_slave_device_type[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_slave_device_type[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f85_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_slave_vendor_id[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_slave_vendor_id[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f86_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_slave_prod_code[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_slave_prod_code[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f87_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_slave_rev_num[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_slave_rev_num[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f88_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_slave_ser_num[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_slave_ser_num[subindex - 1] = *value; + } + return 0; +} + +uint32_t co_od1f89_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value) +{ + if (event == OD_EVENT_READ) + { + *value = net->nmt_slave_boot_time[subindex - 1]; + } + else if (event == OD_EVENT_WRITE) + { + net->nmt_slave_boot_time[subindex - 1] = *value; + } + return 0; +} + +static void co_mngr_reset (co_client_t * client) +{ + co_net_t * net = client->net; + uint8_t i; + bool reset_all = true; + uint32_t _max_boot_time = 0; + + for (i = 0; i < CO_CONF_MNGR; i++) + { + if (net->nmt_slave_boot_time[i] > _max_boot_time) + { + _max_boot_time = net->nmt_slave_boot_time[i]; + } + } + + for (i = 0; (i < CO_CONF_MNGR) && reset_all; i++) + { + if ((net->nmt_slave_assignment[i] & NMT_MNGR_RESET_COMM) != + NMT_MNGR_RESET_COMM) + { + reset_all = false; + } + } + + if (reset_all) + { + LOG_INFO (CO_CONF_MNGR_LOG, "reset communication for all nodes\n"); + co_nmt (client, CO_NMT_RESET_COMMUNICATION, 0); + } + else + { + LOG_INFO (CO_CONF_MNGR_LOG, "reset communication for nodes individually\n"); + for (i = 0; i < 127; i++) + { + if (i < CO_CONF_MNGR) + { + if ((net->nmt_slave_assignment[i] & NMT_MNGR_RESET_COMM) == + NMT_MNGR_RESET_COMM) + { + LOG_INFO (CO_CONF_MNGR_LOG, "reset communication for node %d\n", i + 1); + co_nmt (client, CO_NMT_RESET_COMMUNICATION, i + 1); + } + else + { + LOG_INFO (CO_CONF_MNGR_LOG, "skipped reset communication for node %d\n", i + 1); + } + } + else + { + LOG_INFO (CO_CONF_MNGR_LOG, "reset communication for node %d\n", i + 1); + co_nmt (client, CO_NMT_RESET_COMMUNICATION, i + 1); + } + } + } + + LOG_INFO (CO_CONF_MNGR_LOG, "Reset communication wait max boot time %"PRIu32" ms\n", _max_boot_time); + os_usleep(_max_boot_time * 1000); +} + +static void co_mngr_reset_node (co_client_t * client, uint8_t node) +{ + co_net_t * net = client->net; + + if ((net->nmt_slave_assignment[node - 1] & NMT_MNGR_RESET_COMM) == + NMT_MNGR_RESET_COMM) + { + LOG_INFO (CO_CONF_MNGR_LOG, "reset communication for node %d\n", node); + co_nmt (client, CO_NMT_RESET_COMMUNICATION, node); + } + else + { + LOG_INFO (CO_CONF_MNGR_LOG, "skipped reset communication for node %d\n", node); + } + + LOG_INFO (CO_CONF_MNGR_LOG, "Reset communication wait max boot time %"PRIu32" ms\n", net->nmt_slave_boot_time[node - 1]); + os_usleep(net->nmt_slave_boot_time[node - 1] * 1000); +} + +static co_mngr_status_t co_nmt_update_cfg (co_client_t * client, uint8_t node) +{ + co_net_t * net = client->net; + co_mngr_status_t ret; + + ret = net->cb_write_dcf (net->cb_arg, node); + if (ret != 0) + { + return ERROR_STATUS_J; + } + return ERROR_STATUS_OK; +} + +static co_mngr_status_t co_nmt_chk_cfg (co_client_t * client, uint8_t node) +{ + co_net_t * net = client->net; + + if (net->node == node) + { + /* Ignore the managers own node during configuration/boot phase */ + return ERROR_STATUS_OK; + } + if ((node > 0 ) && (node <= CO_CONF_MNGR)) + { + int ret; + uint32_t conf_date; + uint32_t conf_time; + /* Enter route C */ + /* Check configuration */ + /* Configuration date */ + if ((net->conf_exp_conf_date[node - 1] == 0) && + (net->conf_exp_conf_time[node - 1] == 0)) + { + /* Update configuration */ + return co_nmt_update_cfg (client, node); + } + else + { + ret = co_sdo_read (client, node, 0x1020, 1, &conf_date, sizeof(conf_date)); + if (ret != sizeof(conf_date)) + { + return ERROR_STATUS_J; + } + + ret = co_sdo_read (client, node, 0x1020, 2, &conf_time, sizeof(conf_time)); + if (ret != sizeof(conf_time)) + { + return ERROR_STATUS_J; + } + + if ((conf_date != net->conf_exp_conf_date[node - 1]) || + (conf_time != net->conf_exp_conf_time[node - 1])) + { + /* Update configuration */ + return co_nmt_update_cfg (client, node); + } + } + } + return ERROR_STATUS_J; +} + +static co_mngr_status_t co_nmt_start_err_cntl (co_client_t * client, uint8_t node) +{ + co_net_t * net = client->net; + uint8_t i; + + if ((node > 0 ) && (node <= CO_CONF_MNGR)) + { + /* Start error control service (also from E,D)*/ + /* Consumer heartbeat ?*/ + for (i = 0; i < MAX_HEARTBEATS; i++) + { + if (node == net->heartbeat[i].node) + { + /* wait at least until heartbeat should expire once */ + os_usleep(net->heartbeat[i].time * 1000); + if (net->heartbeat[i].is_alive) + { + LOG_INFO (CO_CONF_MNGR_LOG, "Error control started ok for node %d\n", node); + return ERROR_STATUS_OK; + } + else + { + LOG_INFO (CO_CONF_MNGR_LOG, "Error control started failed for node %d\n", node); + return ERROR_STATUS_K; + } + } + } + + /* Start Node guarding ? */ + if (net->nmt_slave_assignment[node - 1] & NMT_MNGR_SLAVE) + { + if (((net->nmt_slave_assignment[node - 1] >> 16) & 0xFFFF) > 0) + { + co_od1f83_activate_node_guarding (net, node); + } + } + } + return ERROR_STATUS_OK; +} + +static co_mngr_status_t co_nmt_start_slave (co_client_t * client, uint8_t node) +{ + co_net_t * net = client->net; + + /* In case the NMT slave is in NMT state Operational + * the process boot NMT slave shall finish successfully. */ + //TODO create macro for states + if (net->nmt_request[node - 1] == 5) + { + return ERROR_STATUS_OK; + } + else + { + /* The NMT master shall start the NMT slaves. */ + if ((net->nmt_startup & NMT_STARTUP_START_NODE) == NMT_STARTUP_START_NODE) + { + if ((net->nmt_startup & NMT_STARTUP_START_ALL)) + { + if (net->state == 5) + { + co_nmt (client, CO_NMT_OPERATIONAL, 0); + } + } + else + { + co_nmt (client, CO_NMT_OPERATIONAL, node); + } + return ERROR_STATUS_OK; + } + } + return ERROR_STATUS_OK; +} + +static co_mngr_status_t co_nmt_mngr_boot_slave (co_client_t * client, uint8_t node) +{ + co_net_t * net = client->net; + + if (net->node == node) + { + /* Ignore the managers own node during configuration/boot phase */ + return ERROR_STATUS_OK; + } + if ((node > 0 ) && (node <= CO_CONF_MNGR)) + { + if (net->nmt_slave_assignment[node - 1] & NMT_MNGR_SLAVE) + { + if (net->nmt_slave_assignment[node - 1] & NMT_MNGR_BOOT_SLAVE) + { + co_mngr_status_t stat; + uint32_t dev_type; + uint32_t vendor_id; + uint32_t prod_code; + uint32_t rev_num; + uint32_t ser_num; + + /* Get slave device type */ + int ret; + + if (net->nmt_slave_device_type[node - 1] != 0) + { + ret = co_sdo_read (client, node, 0x1000, 0, &dev_type, sizeof(dev_type)); + if (ret != sizeof(dev_type)) + { + return ERROR_STATUS_B; + } + + if (dev_type != net->nmt_slave_device_type[node - 1]) + { + return ERROR_STATUS_C; + } + } + + /* Check Identity */ + /* Vendor ID */ + if (net->nmt_slave_vendor_id[node - 1] != 0) + { + ret = co_sdo_read (client, node, 0x1018, 1, &vendor_id, sizeof(vendor_id)); + if (ret != sizeof(vendor_id)) + { + return ERROR_STATUS_D; + } + + if (vendor_id != net->nmt_slave_vendor_id[node - 1]) + { + return ERROR_STATUS_D; + } + } + + /* Product code */ + if (net->nmt_slave_prod_code[node - 1] != 0) + { + ret = co_sdo_read (client, node, 0x1018, 2, &prod_code, sizeof(prod_code)); + if (ret != sizeof(prod_code)) + { + return ERROR_STATUS_M; + } + + if ((prod_code != net->nmt_slave_prod_code[node - 1]) && + (net->nmt_slave_prod_code[node - 1] != 0)) + { + return ERROR_STATUS_M; + } + } + + /* Revision Number */ + if (net->nmt_slave_rev_num[node - 1] != 0) + { + ret = co_sdo_read (client, node, 0x1018, 3, &rev_num, sizeof(rev_num)); + if (ret != sizeof(rev_num)) + { + return ERROR_STATUS_N; + } + + if (rev_num != net->nmt_slave_rev_num[node - 1]) + { + return ERROR_STATUS_N; + } + } + + /* Serial Number */ + if (net->nmt_slave_ser_num[node - 1] != 0) + { + ret = co_sdo_read (client, node, 0x1018, 4, &ser_num, sizeof(ser_num)); + if (ret != sizeof(ser_num)) + { + return ERROR_STATUS_O; + } + + if (ser_num != net->nmt_slave_ser_num[node - 1]) + { + return ERROR_STATUS_O; + } + } + + stat = co_nmt_chk_cfg(client, node); + if (stat != ERROR_STATUS_OK) + { + return stat; + } + } + + /* from D,E, Start error control service */ + co_nmt_start_err_cntl (client, node); + + return co_nmt_start_slave (client, node); + } + else + { + return ERROR_STATUS_A; + } + } + else + { + return ERROR_STATUS_A; + } +} + +co_mngr_status_t co_nmt_mngr_startup (co_client_t * client) +{ + co_net_t * net = client->net; + + /* Configured as NMT master */ + if (net->nmt_startup & NMT_STARTUP_NMT_MASTER) + { + uint8_t i; + co_mngr_status_t stat; + bool boot_ok = true; + + /* NMT flying master */ + //TODO implement, now assumes won + + /* LSS required? */ + //TODO implement, skipped for now + + co_mngr_reset (client); + + /* Mandatory slaves started*/ + for (i = 0; i < CO_CONF_MNGR; i++) + { + stat = co_nmt_mngr_boot_slave (client, i + 1); + if ((stat != ERROR_STATUS_OK) && + (net->nmt_slave_assignment[i] & NMT_MNGR_MANDATORY)) + { + /* Halt start up procedure */ + return stat; + } + else + { + boot_ok = false; + } + } + + if (net->nmt_startup & NMT_STARTUP_NMT_MASTER_START) + { + co_nmt (client, CO_NMT_OPERATIONAL, net->node); + } + else + { + //TODO wait for application to set this node in operational + return ERROR_STATUS_OK; + } + + if ((net->nmt_startup & NMT_STARTUP_START_ALL) || + (net->nmt_startup & NMT_STARTUP_START_NODE)) + { + if ((net->nmt_startup & NMT_STARTUP_START_ALL) && + (boot_ok)) + { + /* Start remote node with node id 0 */ + co_nmt (client, CO_NMT_OPERATIONAL, 0); + } + else + { + /* Start remote node individually */ + for (i = 0; i < CO_CONF_MNGR; i++) + { + if (net->nmt_slave_assignment[i] & NMT_MNGR_SLAVE) + { + co_nmt (client, CO_NMT_OPERATIONAL, i + 1); + } + } + } + } + return ERROR_STATUS_OK; + } + else if (net->nmt_startup & NMT_STARTUP_NMT_MASTER_START) + { + co_nmt (client, CO_NMT_OPERATIONAL, net->node); + /* Enter slave mode */ + } + return ERROR_STATUS_OK; +} + +co_mngr_status_t co_nmt_mngr_startup_node (co_client_t * client, uint8_t node) +{ + co_net_t * net = client->net; + + co_mngr_status_t stat; + + co_mngr_reset_node (client, node); + + stat = co_nmt_mngr_boot_slave (client, node); + if ((stat != ERROR_STATUS_OK) && + (net->nmt_slave_assignment[node - 1] & NMT_MNGR_MANDATORY)) + { + /* Halt start up procedure */ + return stat; + } + + return ERROR_STATUS_OK; +} +#endif diff --git a/src/co_mngr.h b/src/co_mngr.h new file mode 100644 index 0000000..e072184 --- /dev/null +++ b/src/co_mngr.h @@ -0,0 +1,162 @@ +/********************************************************************* + * _ _ _ + * _ __ | |_ _ | | __ _ | |__ ___ + * | '__|| __|(_)| | / _` || '_ \ / __| + * | | | |_ _ | || (_| || |_) |\__ \ + * |_| \__|(_)|_| \__,_||_.__/ |___/ + * + * http://www.rt-labs.com + * Copyright 2017 rt-labs AB, Sweden. + * See LICENSE file in the project root for full license information. + ********************************************************************/ + +#ifndef CO_MNGR_H +#define CO_MNGR_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "co_api.h" +#include "co_main.h" + +#if CO_CONF_MNGR > 0 + +#define NMT_STARTUP_NMT_MASTER BIT (0) +#define NMT_STARTUP_START_ALL BIT (1) +#define NMT_STARTUP_NMT_MASTER_START BIT (2) +#define NMT_STARTUP_START_NODE BIT (3) +#define NMT_STARTUP_RESET_ALL BIT (4) +#define NMT_STARTUP_FLYING_MASTER BIT (5) +#define NMT_STARTUP_STOP_ALL_NODES BIT (6) + +#define NMT_MNGR_SLAVE BIT (0) +#define NMT_MNGR_BOOT_SLAVE BIT (2) +#define NMT_MNGR_MANDATORY BIT (3) +#define NMT_MNGR_RESET_COMM BIT (4) +#define NMT_MNGR_SW_VERSION BIT (5) +#define NMT_MNGR_SW_UPDATE BIT (6) +#define NMT_MNGR_RESTORE BIT (7) + +#define CO_MNGR_NMT_STARTUP_SUPPORT (NMT_STARTUP_NMT_MASTER | \ + NMT_STARTUP_START_ALL | \ + NMT_STARTUP_NMT_MASTER_START | \ + NMT_STARTUP_START_NODE | \ + NMT_STARTUP_RESET_ALL | \ + NMT_STARTUP_STOP_ALL_NODES \ + ) + +uint32_t co_od102a_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f26_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f27_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f80_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f81_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f82_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f83_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f84_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f85_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f86_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f87_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f88_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +uint32_t co_od1f89_fn ( + co_net_t * net, + od_event_t event, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + uint32_t * value); + +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* CO_MNGR_H */ + + diff --git a/src/co_node_guard.c b/src/co_node_guard.c index c9ef9d4..29df58b 100644 --- a/src/co_node_guard.c +++ b/src/co_node_guard.c @@ -14,6 +14,7 @@ ********************************************************************/ #ifdef UNIT_TEST +#define co_nmt_rtr mock_co_nmt_rtr #define os_channel_send mock_os_channel_send #define os_channel_receive mock_os_channel_receive #define os_get_current_time_us mock_os_get_current_time_us @@ -78,39 +79,53 @@ int co_node_guard_rx (co_net_t * net, uint32_t id, void * msg, size_t dlc) uint8_t _msg[1]; uint8_t state; - if (id != (CO_RTR_MASK | CO_FUNCTION_NMT_ERR | net->node)) - return -1; - if (dlc != 1) return -1; - net->node_guard.is_alive = true; - net->node_guard.timestamp = os_get_current_time_us(); - - /* Heartbeat producer (heartbeat is prioritised over node guarding)*/ - if (net->hb_time == 0) + if (id != (CO_RTR_MASK | CO_FUNCTION_NMT_ERR | net->node)) { - switch(net->state) +#if CO_CONF_MNGR > 0 + uint8_t * nmt_state = (uint8_t *) msg; + uint8_t _node = CO_NODE_GET(id); + /* Store a time stamp when the NMT state was received */ + if ((dlc == 1) && (_node > 0) && (_node <= CO_CONF_MNGR)) { - case STATE_STOP: - state = 4; - break; - case STATE_OP: - state = 5; - break; - case STATE_PREOP: - state = 127; - break; - default: - state = 0; - break; + net->nmt_node_ts_resp[_node - 1] = os_get_current_time_us(); + net->nmt_request[_node - 1] = 0x7F & nmt_state[0]; } - - co_put_uint8 (_msg, net->node_guard.toggle | state); - os_channel_send (net->channel, 0x700 + net->node, _msg, sizeof(_msg)); - net->node_guard.toggle = ~net->node_guard.toggle & 0x80; +#else + return -1; +#endif } + else + { + net->node_guard.is_alive = true; + net->node_guard.timestamp = os_get_current_time_us(); + /* Heartbeat producer (heartbeat is prioritised over node guarding)*/ + if (net->hb_time == 0) + { + switch(net->state) + { + case STATE_STOP: + state = 4; + break; + case STATE_OP: + state = 5; + break; + case STATE_PREOP: + state = 127; + break; + default: + state = 0; + break; + } + + co_put_uint8 (_msg, net->node_guard.toggle | state); + os_channel_send (net->channel, 0x700 + net->node, _msg, sizeof(_msg)); + net->node_guard.toggle = ~net->node_guard.toggle & 0x80; + } + } return 0; } @@ -137,6 +152,40 @@ int co_node_guard_timer (co_net_t * net, uint32_t now) co_emcy_tx (net, 0x8130, 0, NULL); } } +#if CO_CONF_MNGR > 0 + /* Evaluate all guarded nodes */ + { + uint8_t i; + for ( i = 0; i < CO_CONF_MNGR; i++) + { + if (net->nmt_request_node_guard[i] == 1) + { + uint32_t guard_time = ((net->nmt_slave_assignment[i] >> 16) & 0xFFFF); + guard_factor = + (guard_time * ((net->nmt_slave_assignment[i] >> 8) & 0xFF)); + + if ((guard_factor != 0)) + { + if (now - net->nmt_node_ts_resp[i] > 1000 * guard_factor) + { + /* Expired */ + LOG_ERROR (CO_NODE_GUARD_LOG, "remote node guarding expired, node: %d\n", i + 1); + net->nmt_request[i] = 1; /* CANopen device missing */ + co_emcy_tx (net, 0x8130, 0, NULL); + + /* Stall the next emcy call by the guard factor * guard time */ + net->nmt_node_ts_resp[i] = now; + } + if (now - net->nmt_node_ts_req[i] > 1000 * guard_time) + { + net->nmt_node_ts_req[i] = now; + co_nmt_rtr (net, i + 1); + } + } + } + } + } +#endif return 0; } diff --git a/src/co_obj.c b/src/co_obj.c index bb51d07..86c62f0 100644 --- a/src/co_obj.c +++ b/src/co_obj.c @@ -158,3 +158,92 @@ const co_entry_t OD1A00[] = { 0x00, OD_RW, DTYPE_UNSIGNED8, 8, MAX_PDO_ENTRIES, NULL }, { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, NULL }, }; + +/* Entry descriptor for NMT inhibit time value object (102Ah) */ +const co_entry_t OD102A[] = +{ + { 0x00, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL }, +}; + +/* Entry descriptor for Expected configuration date value object (1F26h) */ +const co_entry_t OD1F26[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for Expected configuration time value object (1F27h) */ +const co_entry_t OD1F27[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for NMT startup value object (1F80h) */ +const co_entry_t OD1F80[] = +{ + { 0x00, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL }, +}; + +/* Entry descriptor for NMT slave assignment value object (1F81h) */ +const co_entry_t OD1F81[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for Request NMT value object (1F82h) */ +const co_entry_t OD1F82[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR + 1, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED8, 8, 0x0, NULL }, +}; + +/* Entry descriptor for Request node guarding value object (1F83h) */ +const co_entry_t OD1F83[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR + 1, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED8, 8, 0x0, NULL }, +}; + +/* Entry descriptor for Device type identification value object (1F84h) */ +const co_entry_t OD1F84[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for Vendor identification value object (1F85h) */ +const co_entry_t OD1F85[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for Product code value object (1F86h) */ +const co_entry_t OD1F86[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for Revision number value object (1F87h) */ +const co_entry_t OD1F87[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for Serial number value object (1F88h) */ +const co_entry_t OD1F88[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; + +/* Entry descriptor for Boot time value object (1F89h) */ +const co_entry_t OD1F89[] = +{ + { 0x00, OD_RO, DTYPE_UNSIGNED8, 8, CO_CONF_MNGR, NULL }, + { 0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0x0, NULL }, +}; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 208c36d..ff67bb2 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -28,6 +28,7 @@ target_sources(co_test PRIVATE test_node_guard.cpp test_heartbeat.cpp test_osal.cpp + test_mngr.cpp # Test utils mocks.h @@ -54,6 +55,7 @@ target_sources(co_test PRIVATE ${CANOPEN_SOURCE_DIR}/src/co_node_guard.c ${CANOPEN_SOURCE_DIR}/src/co_heartbeat.c ${CANOPEN_SOURCE_DIR}/src/co_obj.c + ${CANOPEN_SOURCE_DIR}/src/co_mngr.c ) get_target_property(CANOPEN_OPTIONS canopen COMPILE_OPTIONS) diff --git a/test/mocks.cpp b/test/mocks.cpp index 3af34c6..cae722a 100644 --- a/test/mocks.cpp +++ b/test/mocks.cpp @@ -126,6 +126,13 @@ void mock_co_emcy_tx (co_net_t * net, uint16_t code) mock_co_emcy_tx_code = code; } +unsigned int mock_co_nmt_rtr_calls = 0; +void mock_co_nmt_rtr (co_net_t * net, uint8_t node) +{ + uint8_t _data[1]; + mock_co_nmt_rtr_calls++; + mock_os_channel_send (net->channel, CO_RTR_MASK + CO_FUNCTION_NMT_ERR + node, _data, 1); +} unsigned int cb_reset_calls; void cb_reset (void * arg) @@ -210,3 +217,32 @@ int store_close (void * arg) { return 0; } + +int mock_co_sdo_read (co_client_t * client, uint8_t node, uint16_t index, + uint8_t subindex, void * data, size_t size) +{ + return size; +} + +void mock_co_nmt (co_client_t * client, co_nmt_cmd_t cmd, uint8_t node) +{ + co_net_t * net = client->net; + mock_co_nmt_net (net, cmd, node); +} + +void mock_co_nmt_net (co_net_t * net, co_nmt_cmd_t cmd, uint8_t node) +{ + uint8_t data[] = { static_cast(cmd), node }; + + if ((node == 0) || (net->node == node)) + { + co_nmt_rx(net, node, data, sizeof(data)); + + if (node != 0) + { + return; + } + } + + mock_os_channel_send (net->channel, CO_FUNCTION_NMT + node, data, sizeof(data)); +} diff --git a/test/mocks.h b/test/mocks.h index b6a2930..cf530fe 100644 --- a/test/mocks.h +++ b/test/mocks.h @@ -25,6 +25,7 @@ extern "C" #include "co_api.h" #include "co_main.h" +#include "co_nmt.h" #include "osal.h" @@ -78,6 +79,9 @@ extern unsigned int mock_co_emcy_tx_calls; extern uint16_t mock_co_emcy_tx_code; void mock_co_emcy_tx (co_net_t * net, uint16_t code); +extern unsigned int mock_co_nmt_rtr_calls; +void mock_co_nmt_rtr (co_net_t * net, uint8_t node); + extern unsigned int cb_reset_calls; void cb_reset (void * arg); @@ -108,6 +112,12 @@ int store_read (void * arg, void * data, size_t size); int store_write (void * arg, const void * data, size_t size); int store_close (void * arg); +int mock_co_sdo_read (co_client_t * client, uint8_t node, uint16_t index, + uint8_t subindex, void * data, size_t size); + +void mock_co_nmt (co_client_t * client, co_nmt_cmd_t cmd, uint8_t node); +void mock_co_nmt_net (co_net_t * net, co_nmt_cmd_t cmd, uint8_t node); + #ifdef __cplusplus } #endif diff --git a/test/test_mngr.cpp b/test/test_mngr.cpp new file mode 100644 index 0000000..5259d81 --- /dev/null +++ b/test/test_mngr.cpp @@ -0,0 +1,411 @@ +/********************************************************************* + * _ _ _ + * _ __ | |_ _ | | __ _ | |__ ___ + * | '__|| __|(_)| | / _` || '_ \ / __| + * | | | |_ _ | || (_| || |_) |\__ \ + * |_| \__|(_)|_| \__,_||_.__/ |___/ + * + * http://www.rt-labs.com + * Copyright 2017 rt-labs AB, Sweden. + * See LICENSE file in the project root for full license information. + ********************************************************************/ + +#include "co_mngr.h" +#include "co_api.h" +#include "co_obj.h" +#include "co_nmt.h" +#include "co_node_guard.h" +#include "options.h" +#include "osal.h" +#include + +#include "mocks.h" +#include "test_util.h" + +#if CO_CONF_MNGR > 0 + +#define CALL_AND_CHECK(function, event, item) \ + ret = function(client.net, event, NULL, NULL, i + 1, &tmp); \ + EXPECT_EQ(0u, ret); \ + EXPECT_EQ(net.item[i], tmp); + +#define TEST_OD1Fxx(test, item) \ +TEST_F(MngrTest, test) \ +{ \ + uint32_t ret, tmp; \ + uint8_t i; \ + resetNmgrOD (&client); \ + for (i = 0; i < CO_CONF_MNGR; i++) \ + { \ + net.item[i] = 0xAAAAAAAA; \ + CALL_AND_CHECK(co_##test, OD_EVENT_READ, item); \ + tmp = 0x55555555; \ + CALL_AND_CHECK(co_##test, OD_EVENT_WRITE, item); \ + CALL_AND_CHECK(co_##test, OD_EVENT_READ, item); \ + } \ +} + +// Test fixture + +static const co_obj_t odtest_od[] = +{ + { 0x102A, OTYPE_VAR, 0, OD102A, co_od102a_fn }, + { 0x1F26, OTYPE_ARRAY, CO_CONF_MNGR, OD1F26, co_od1f26_fn }, + { 0x1F27, OTYPE_ARRAY, CO_CONF_MNGR, OD1F27, co_od1f27_fn }, + { 0x1F80, OTYPE_VAR, 0, OD1F80, co_od1f80_fn }, + { 0x1F81, OTYPE_ARRAY, CO_CONF_MNGR, OD1F81, co_od1f81_fn }, + { 0x1F82, OTYPE_ARRAY, CO_CONF_MNGR + 1, OD1F82, co_od1f82_fn }, + { 0x1F83, OTYPE_ARRAY, CO_CONF_MNGR + 1, OD1F83, co_od1f83_fn }, + { 0x1F84, OTYPE_ARRAY, CO_CONF_MNGR, OD1F84, co_od1f84_fn }, + { 0x1F85, OTYPE_ARRAY, CO_CONF_MNGR, OD1F85, co_od1f85_fn }, + { 0x1F86, OTYPE_ARRAY, CO_CONF_MNGR, OD1F86, co_od1f86_fn }, + { 0x1F87, OTYPE_ARRAY, CO_CONF_MNGR, OD1F87, co_od1f87_fn }, + { 0x1F88, OTYPE_ARRAY, CO_CONF_MNGR, OD1F88, co_od1f88_fn }, + { 0x1F89, OTYPE_ARRAY, CO_CONF_MNGR, OD1F89, co_od1f89_fn }, + { 0, OTYPE_VAR, 0, NULL, NULL}, +}; + +static void _co_mngr_cb_nmt (void * arg, co_state_t state) +{ + return; +} + +class MngrTest : public ::testing::Test +{ +protected: + virtual void SetUp() { + client.net = &net; + net.od = odtest_od; + net.open = store_open; + net.read = store_read; + net.write = store_write; + net.close = store_close; + net.cb_arg = NULL; + net.cb_nmt = _co_mngr_cb_nmt; + net.cb_reset = NULL; + net.cb_emcy = NULL; + net.number_of_errors = 0; + net.emcy.error = 0; + net.error_behavior = 0; + co_nmt_init(&net); + }; + + co_client_t client; + co_net_t net; + +}; + +// Tests + +static void resetNmgrOD (co_client_t * client) +{ + co_net_t * net = client->net; + + int i; + for (i = 0; i < CO_CONF_MNGR; i++) + { + net->conf_exp_conf_date[i] = 0; + net->conf_exp_conf_time[i] = 0; + net->conf_exp_conf_time[i] = 0; + net->nmt_slave_assignment[i] = 0; + net->nmt_request[i] = 0; + net->nmt_request_node_guard[i] = 0; + net->nmt_node_ts_resp[i] = 0; + net->nmt_node_ts_req[i] = 0; + net->nmt_slave_device_type[i] = 0; + net->nmt_slave_vendor_id[i] = 0; + net->nmt_slave_prod_code[i] = 0; + net->nmt_slave_rev_num[i] = 0; + net->nmt_slave_ser_num[i] = 0; + net->nmt_slave_boot_time[i] = 0; + } + + net->nmt_inhibit = 0; + net->nmt_startup = 0; +} + +TEST_F(MngrTest, od102a_fn) +{ + uint32_t ret; + uint32_t tmp; + + resetNmgrOD (&client); + + net.nmt_inhibit= 100; + + ret = co_od102a_fn(client.net, OD_EVENT_READ, NULL, NULL, 0, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_inhibit, tmp); + + tmp = 500; + ret = co_od102a_fn(client.net, OD_EVENT_WRITE, NULL, NULL, 0, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_inhibit, tmp); + + ret = co_od102a_fn(client.net, OD_EVENT_READ, NULL, NULL, 0, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_inhibit, tmp); +} + +TEST_OD1Fxx(od1f26_fn, conf_exp_conf_date) +TEST_OD1Fxx(od1f27_fn, conf_exp_conf_time) + +TEST_F(MngrTest, od1f80_fn) +{ + uint32_t ret; + uint32_t tmp; + + resetNmgrOD (&client); + + net.nmt_startup = 100; + + ret = co_od1f80_fn(client.net, OD_EVENT_READ, NULL, NULL, 0, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_startup, tmp); + + net.nmt_startup = 0; + + tmp = CO_MNGR_NMT_STARTUP_SUPPORT; + ret = co_od1f80_fn(client.net, OD_EVENT_WRITE, NULL, NULL, 0, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_startup, (uint32_t) CO_MNGR_NMT_STARTUP_SUPPORT); + + ret = co_od1f80_fn(client.net, OD_EVENT_READ, NULL, NULL, 0, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_startup, tmp); + + net.nmt_startup = 0; + + tmp = 0xFF; + ret = co_od1f80_fn(client.net, OD_EVENT_WRITE, NULL, NULL, 0, &tmp); + EXPECT_EQ(CO_SDO_ABORT_VALUE, ret); + EXPECT_EQ(net.nmt_startup, 0u); + + ret = co_od1f80_fn(client.net, OD_EVENT_READ, NULL, NULL, 0, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_startup, tmp); +} + +TEST_OD1Fxx(od1f81_fn, nmt_slave_assignment) + +TEST_F(MngrTest, od1f82_fn) +{ + uint32_t ret; + uint32_t tmp; + uint8_t i,n; + + resetNmgrOD (&client); + co_nmt_init(client.net); + + for (i = 0; i < CO_CONF_MNGR; i++) + { + net.nmt_request[i] = 100; + + ret = co_od1f82_fn(client.net, OD_EVENT_READ, NULL, NULL, i + 1, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_request[i], tmp); + } + + tmp = 0xFF; + ret = co_od1f82_fn(client.net, OD_EVENT_READ, NULL, NULL, 128, &tmp); + EXPECT_EQ(CO_SDO_ABORT_ACCESS_WO, ret); + + for (i = 0; i < CO_CONF_MNGR; i++) + { + net.nmt_request[i] = 0; + + for (n = 0; n < 255; n++) + { + mock_os_channel_send_id = 0; + + tmp = n; + + ret = co_od1f82_fn(client.net, OD_EVENT_WRITE, NULL, NULL, i + 1, &tmp); + if (((n >= 4) && (n <= 7)) || (n == 127)) + { + EXPECT_EQ(0u, ret); + EXPECT_EQ((uint32_t) (CO_FUNCTION_NMT + i + 1), mock_os_channel_send_id); + EXPECT_EQ(2u, mock_os_channel_send_dlc); + switch (tmp) + { + case 5: EXPECT_EQ(CO_NMT_OPERATIONAL, mock_os_channel_send_data[0]); break; + case 4: EXPECT_EQ(CO_NMT_STOPPED, mock_os_channel_send_data[0]); break; + case 127: EXPECT_EQ(CO_NMT_PRE_OPERATIONAL, mock_os_channel_send_data[0]); break; + case 6: EXPECT_EQ(CO_NMT_RESET_NODE, mock_os_channel_send_data[0]); break; + case 7: EXPECT_EQ(CO_NMT_RESET_COMMUNICATION, mock_os_channel_send_data[0]); break; + default: FAIL(); break; + } + EXPECT_EQ(i + 1, mock_os_channel_send_data[1]); + } + else + { + EXPECT_EQ(CO_SDO_ABORT_VALUE, ret); + EXPECT_EQ(0u, mock_os_channel_send_id); + } + } + } + + for (n = 0; n < 255; n++) + { + mock_os_channel_send_id = 0; + + tmp = n; + + ret = co_od1f82_fn(client.net, OD_EVENT_WRITE, NULL, NULL, 128, &tmp); + if (((n >= 4) && (n <= 7)) || (n == 127)) + { + EXPECT_EQ(0u, ret); + EXPECT_EQ((uint32_t) CO_FUNCTION_NMT, mock_os_channel_send_id); + EXPECT_EQ(2u, mock_os_channel_send_dlc); + switch (tmp) + { + case 5: + { + EXPECT_EQ(CO_NMT_OPERATIONAL, mock_os_channel_send_data[0]); + EXPECT_EQ(STATE_OP, net.state); + break; + } + case 4: + { + EXPECT_EQ(CO_NMT_STOPPED, mock_os_channel_send_data[0]); + EXPECT_EQ(STATE_STOP, net.state); + break; + } + case 127: + { + EXPECT_EQ(CO_NMT_PRE_OPERATIONAL, mock_os_channel_send_data[0]); + EXPECT_EQ(STATE_PREOP, net.state); + break; + } + case 6: + { + EXPECT_EQ(CO_NMT_RESET_NODE, mock_os_channel_send_data[0]); + EXPECT_EQ(STATE_PREOP, net.state); + break; + } + case 7: + { + EXPECT_EQ(CO_NMT_RESET_COMMUNICATION, mock_os_channel_send_data[0]); + EXPECT_EQ(STATE_PREOP, net.state); + break; + } + default: FAIL(); break; + } + EXPECT_EQ(0, mock_os_channel_send_data[1]); + } + else + { + EXPECT_EQ(CO_SDO_ABORT_VALUE, ret); + EXPECT_EQ(0u, mock_os_channel_send_id); + } + } +} + +TEST_F(MngrTest, od1f83_fn) +{ + uint32_t ret; + uint32_t tmp; + uint8_t i; + + resetNmgrOD (&client); + + for (i = 0; i < CO_CONF_MNGR; i++) + { + net.nmt_request_node_guard[i] = 100; + + ret = co_od1f83_fn(client.net, OD_EVENT_READ, NULL, NULL, i + 1, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_request_node_guard[i], tmp); + net.nmt_request_node_guard[i] = 0; + } + + tmp = 0xFF; + ret = co_od1f83_fn(client.net, OD_EVENT_READ, NULL, NULL, 128, &tmp); + EXPECT_EQ(CO_SDO_ABORT_ACCESS_WO, ret); + + for (i = 0; i < CO_CONF_MNGR; i++) + { + uint32_t _start_us = os_get_current_time_us(); + tmp = 1; + ret = co_od1f83_fn(client.net, OD_EVENT_WRITE, NULL, NULL, i + 1, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_request_node_guard[i], tmp); + + ret = co_od1f83_fn(client.net, OD_EVENT_READ, NULL, NULL, i + 1, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_request_node_guard[i], tmp); + + EXPECT_LE(_start_us, net.nmt_node_ts_resp[i]); + EXPECT_LE(net.nmt_node_ts_resp[i], os_get_current_time_us()); + } +} + +TEST_F(MngrTest, remote_node_guarding) +{ + uint32_t ret; + uint32_t tmp; + uint8_t i; + uint32_t _now; + uint8_t expected[][8] = { + { 0x30, 0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + }; + + resetNmgrOD (&client); + co_nmt_init(client.net); + net.state = STATE_OP; + net.emcy.cobid = 0x81u; + net.node_guard.is_alive = 0; + + /* Set guard time from node 2 */ + net.nmt_slave_assignment[1] = (10 << 16) + (5 << 8); + + mock_co_nmt_rtr_calls = 0; + mock_os_channel_send_id = 0; + + /* Setup node guarding on node 2 */ + tmp = 1; + ret = co_od1f83_fn(client.net, OD_EVENT_WRITE, NULL, NULL, 2, &tmp); + EXPECT_EQ(0u, ret); + EXPECT_EQ(net.nmt_request_node_guard[1], tmp); + + /* workaround since os_get_current_time_us is not reliable in windows */ + _now = os_get_current_time_us(); + _now += 1 * 1000; + EXPECT_EQ(STATE_OP, net.state); + + /* execute co_node_guard_timer */ + co_node_guard_timer(client.net, _now); + EXPECT_EQ(mock_co_nmt_rtr_calls, 1u); + + for (i = 1; i < 5; i++) + { + _now += 101 * 100; + /* execute co_node_guard_timer */ + co_node_guard_timer(client.net, _now); + EXPECT_EQ(mock_co_nmt_rtr_calls, (uint32_t) (i + 1)); + EXPECT_EQ((uint32_t) (CO_RTR_MASK + CO_FUNCTION_NMT_ERR + 2), mock_os_channel_send_id); + EXPECT_EQ(1u, mock_os_channel_send_dlc); + EXPECT_EQ(STATE_OP, net.state); + } + + EXPECT_EQ(STATE_OP, net.state); + + _now += 10 * 1000; + + /* execute co_node_guard_timer */ + co_node_guard_timer(client.net, _now); + EXPECT_EQ(mock_co_nmt_rtr_calls, 5u); + + EXPECT_EQ(0x81u, mock_os_channel_send_id); + EXPECT_EQ(8u, mock_os_channel_send_dlc); + EXPECT_TRUE(ArraysMatch(expected[0], mock_os_channel_send_data)); + EXPECT_EQ(STATE_PREOP, net.state); +} + +TEST_OD1Fxx(od1f84_fn, nmt_slave_device_type) +TEST_OD1Fxx(od1f85_fn, nmt_slave_vendor_id) +TEST_OD1Fxx(od1f86_fn, nmt_slave_prod_code) +TEST_OD1Fxx(od1f87_fn, nmt_slave_rev_num) +TEST_OD1Fxx(od1f88_fn, nmt_slave_ser_num) +TEST_OD1Fxx(od1f89_fn, nmt_slave_boot_time) +#endif