From 25f8d13b42def6640e0c794f71003126e5ad20c5 Mon Sep 17 00:00:00 2001 From: Daniel Udd Date: Mon, 6 Mar 2023 18:27:17 +0100 Subject: [PATCH 1/5] Add cb_notify event Make cb_notify event a public function and add parameters event and value to callback function. --- include/co_api.h | 16 ++++++++++++---- src/co_main.h | 2 +- src/co_od.c | 12 +++++++----- src/co_od.h | 21 +++++++++++++++++++++ test/mocks.cpp | 2 +- test/mocks.h | 2 +- test/test_sdo_server.cpp | 1 + test/test_util.h | 2 +- util/slave/slave.c | 2 +- 9 files changed, 46 insertions(+), 14 deletions(-) diff --git a/include/co_api.h b/include/co_api.h index 48a044f..4055c2c 100644 --- a/include/co_api.h +++ b/include/co_api.h @@ -223,11 +223,19 @@ typedef enum co_dtype /** Access function event */ typedef enum od_event { - OD_EVENT_READ, /**< Read subindex */ - OD_EVENT_WRITE, /**< Write subindex */ - OD_EVENT_RESTORE, /**< Restore default value */ + OD_EVENT_READ, /**< Read subindex */ + OD_EVENT_WRITE, /**< Write subindex */ + OD_EVENT_RESTORE, /**< Restore default value */ } od_event_t; +/** Notify event */ +typedef enum od_notify_event +{ + OD_NOTIFY_ACCESSED, + OD_NOTIFY_VALUE_SET, + OD_NOTIFY_SDO_RECEIVED, +} od_notify_event_t; + struct co_obj; struct co_entry; @@ -317,7 +325,7 @@ typedef struct co_cfg uint8_t msef[5]); /** Notify callback */ - void (*cb_notify) (co_net_t * net, uint16_t index, uint8_t subindex); + void (*cb_notify) (co_net_t * net, uint16_t index, uint8_t subindex, od_notify_event_t event, uint32_t value); /** Function to open dictionary store */ void * (*open) (co_store_t store, co_mode_t mode); diff --git a/src/co_main.h b/src/co_main.h index 6f50e11..32fbbed 100644 --- a/src/co_main.h +++ b/src/co_main.h @@ -277,7 +277,7 @@ struct co_net uint8_t msef[5]); /** Notify callback */ - void (*cb_notify) (co_net_t * net, uint16_t index, uint8_t subindex); + void (*cb_notify) (co_net_t * net, uint16_t index, uint8_t subindex, od_notify_event_t event, uint32_t value); /** Function to open dictionary store */ void * (*open) (co_store_t store, co_mode_t mode); diff --git a/src/co_od.c b/src/co_od.c index 49f481c..1af9b3a 100644 --- a/src/co_od.c +++ b/src/co_od.c @@ -36,16 +36,18 @@ static int co_subindex_equals ( return entry->subindex == subindex; } -static void co_od_notify ( +void co_od_notify ( co_net_t * net, const co_obj_t * obj, const co_entry_t * entry, - uint8_t subindex) + uint8_t subindex, + od_notify_event_t event, + uint32_t value) { if (entry->flags & OD_NOTIFY) { if (net->cb_notify) - net->cb_notify (net, obj->index, subindex); + net->cb_notify (net, obj->index, subindex, event, value); } } @@ -221,7 +223,7 @@ uint32_t co_od_set_value ( uint32_t v = value; result = obj->access (net, OD_EVENT_WRITE, obj, entry, subindex, &v); - co_od_notify (net, obj, entry, subindex); + co_od_notify (net, obj, entry, subindex, OD_NOTIFY_ACCESSED, 0); return result; } @@ -262,7 +264,7 @@ uint32_t co_od_set_value ( return CO_SDO_ABORT_GENERAL; } - co_od_notify (net, obj, entry, subindex); + co_od_notify (net, obj, entry, subindex, OD_NOTIFY_VALUE_SET, 0); return 0; } diff --git a/src/co_od.h b/src/co_od.h index f5e0803..c184529 100644 --- a/src/co_od.h +++ b/src/co_od.h @@ -271,6 +271,27 @@ uint32_t co_od_set_value ( uint8_t subindex, uint64_t value); +/** + * Trigger notification callback + * + * This functions triggers the notification callback of the subindex, + * if any. + * + * @param net network handle + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + * @param event event type + * @param value optional value + */ +void co_od_notify ( + co_net_t * net, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex, + od_notify_event_t event, + uint32_t value); + #ifdef __cplusplus } #endif diff --git a/test/mocks.cpp b/test/mocks.cpp index 087387a..af1cae1 100644 --- a/test/mocks.cpp +++ b/test/mocks.cpp @@ -175,7 +175,7 @@ void cb_sync (co_net_t * net) unsigned int cb_notify_calls; uint16_t cb_notify_index; uint16_t cb_notify_subindex; -void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex) +void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex, od_notify_event_t event, uint32_t value) { cb_notify_calls++; cb_notify_index = index; diff --git a/test/mocks.h b/test/mocks.h index fa2c07b..3ae03c3 100644 --- a/test/mocks.h +++ b/test/mocks.h @@ -108,7 +108,7 @@ void cb_sync (co_net_t * net); extern unsigned int cb_notify_calls; extern uint16_t cb_notify_index; extern uint16_t cb_notify_subindex; -void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex); +void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex, od_notify_event_t event, uint32_t value); void store_init (void); extern unsigned int store_open_calls; diff --git a/test/test_sdo_server.cpp b/test/test_sdo_server.cpp index e80633b..bb57a3f 100644 --- a/test/test_sdo_server.cpp +++ b/test/test_sdo_server.cpp @@ -132,6 +132,7 @@ TEST_F (SdoServerTest, SegmentedDownload) } EXPECT_STREQ ("new slave name", name1009); + EXPECT_EQ (1u, cb_notify_calls); } TEST_F (SdoServerTest, SegmentedDownloadCached) diff --git a/test/test_util.h b/test/test_util.h index 93ed4b4..25a0ea7 100644 --- a/test/test_util.h +++ b/test/test_util.h @@ -95,7 +95,7 @@ class TestBase : public ::testing::Test char name1009[20] = {0}; co_entry_t OD1009[1] = { - {0, OD_RW, DTYPE_VISIBLE_STRING, 8 * sizeof (name1009), 0, name1009}, + {0, OD_NOTIFY | OD_RW, DTYPE_VISIBLE_STRING, 8 * sizeof (name1009), 0, name1009}, }; char name100A[20]; diff --git a/util/slave/slave.c b/util/slave/slave.c index 834bcbc..aa18592 100644 --- a/util/slave/slave.c +++ b/util/slave/slave.c @@ -201,7 +201,7 @@ static void cb_sync (co_net_t * net) } /* Called when RPDO is received (if OD_NOTIFY is set) */ -static void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex) +static void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex, od_notify_event_t event, uint32_t value) { } From d8c5af030ad3ed328f0f7395bebdbfc85e6e2074 Mon Sep 17 00:00:00 2001 From: Daniel Udd Date: Mon, 6 Mar 2023 17:25:41 +0100 Subject: [PATCH 2/5] Fix SDO 7B last packet --- src/co_sdo_client.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/co_sdo_client.c b/src/co_sdo_client.c index aa90219..8ee4744 100644 --- a/src/co_sdo_client.c +++ b/src/co_sdo_client.c @@ -154,15 +154,15 @@ static int co_sdo_tx_download_init_rsp ( memcpy (&msg[1], job->sdo.data, size); msg[0] = CO_SDO_CCS_DOWNLOAD_SEG_REQ | ((7 - (size & 0x07)) << 1); - if (size < 7) - msg[0] |= CO_SDO_C; job->sdo.toggle = 0; - job->sdo.data += size; job->sdo.remain -= size; job->sdo.total += size; + if (job->sdo.remain == 0) + msg[0] |= CO_SDO_C; + os_channel_send (net->channel, 0x600 + node, msg, sizeof (msg)); } @@ -209,13 +209,14 @@ static int co_sdo_tx_download_seg_rsp ( msg[0] = CO_SDO_CCS_DOWNLOAD_SEG_REQ | ((7 - (size & 0x07)) << 1); if (job->sdo.toggle) msg[0] |= CO_SDO_TOGGLE; - if (size < 7) - msg[0] |= CO_SDO_C; job->sdo.data += size; job->sdo.remain -= size; job->sdo.total += size; + if (job->sdo.remain == 0) + msg[0] |= CO_SDO_C; + os_channel_send (net->channel, 0x600 + node, msg, sizeof (msg)); } From a230742ae13598dd763cae4d92165e5c9eebab1b Mon Sep 17 00:00:00 2001 From: Daniel Udd Date: Mon, 6 Mar 2023 17:19:13 +0100 Subject: [PATCH 3/5] Fix SDO cached status The cached field of the SDO job was never cleared which would cause downloads of large objects to always fail once a small object had been downloaded. --- src/co_main.c | 2 ++ src/co_sdo_server.c | 2 ++ test/test_sdo_client.cpp | 4 ++-- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/co_main.c b/src/co_main.c index 86f3d15..40380d2 100644 --- a/src/co_main.c +++ b/src/co_main.c @@ -277,6 +277,7 @@ int co_sdo_read ( job->sdo.subindex = subindex; job->sdo.data = data; job->sdo.remain = size; + job->sdo.cached = false; job->callback = co_job_callback; job->timestamp = os_tick_current(); job->type = CO_JOB_SDO_READ; @@ -306,6 +307,7 @@ int co_sdo_write ( job->sdo.subindex = subindex; job->sdo.data = (uint8_t *)data; job->sdo.remain = size; + job->sdo.cached = false; job->callback = co_job_callback; job->timestamp = os_tick_current(); job->type = CO_JOB_SDO_WRITE; diff --git a/src/co_sdo_server.c b/src/co_sdo_server.c index 3d5762f..1c80278 100644 --- a/src/co_sdo_server.c +++ b/src/co_sdo_server.c @@ -129,6 +129,7 @@ static int co_sdo_rx_upload_init_req ( job->type = CO_JOB_SDO_UPLOAD; job->sdo.index = co_fetch_uint16 (&data[1]); job->sdo.subindex = data[3]; + job->sdo.cached = false; job->timestamp = os_tick_current(); /* Find requested object */ @@ -306,6 +307,7 @@ static int co_sdo_rx_download_init_req ( job->type = CO_JOB_SDO_DOWNLOAD; job->sdo.index = co_fetch_uint16 (&data[1]); job->sdo.subindex = data[3]; + job->sdo.cached = false; job->timestamp = os_tick_current(); /* Find requested object */ diff --git a/test/test_sdo_client.cpp b/test/test_sdo_client.cpp index 80c1f45..e611867 100644 --- a/test/test_sdo_client.cpp +++ b/test/test_sdo_client.cpp @@ -58,7 +58,7 @@ TEST_F (SdoClientTest, ExpeditedUpload) TEST_F (SdoClientTest, ExpeditedDownload) { - co_job_t job; + co_job_t job{}; uint16_t value = 0; uint8_t expected[][8] = { @@ -136,7 +136,7 @@ TEST_F (SdoClientTest, SegmentedUpload) TEST_F (SdoClientTest, SegmentedDownload) { - co_job_t job; + co_job_t job{}; const char * s = "hello world"; uint8_t expected[][8] = { From 5195c4853616f8e1f8247b6ea92ad4116430576e Mon Sep 17 00:00:00 2001 From: Daniel Udd Date: Mon, 6 Mar 2023 18:37:48 +0100 Subject: [PATCH 4/5] Remove duplicated defines These defines are in co_sdo.h. --- src/co_sdo_server.c | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/src/co_sdo_server.c b/src/co_sdo_server.c index 1c80278..05f473d 100644 --- a/src/co_sdo_server.c +++ b/src/co_sdo_server.c @@ -28,27 +28,6 @@ #include -#define CO_SDO_xCS(v) ((v)&0xE0) -#define CO_SDO_N(v) (((v) >> 2) & 0x03) -#define CO_SDO_E BIT (1) -#define CO_SDO_S BIT (0) - -#define CO_SDO_CCS_DOWNLOAD_SEG_REQ (0 << 5) -#define CO_SDO_CCS_DOWNLOAD_INIT_REQ (1 << 5) -#define CO_SDO_CCS_UPLOAD_INIT_REQ (2 << 5) -#define CO_SDO_CCS_UPLOAD_SEG_REQ (3 << 5) - -#define CO_SDO_SCS_UPLOAD_SEG_RSP (0 << 5) -#define CO_SDO_SCS_DOWNLOAD_SEG_RSP (1 << 5) -#define CO_SDO_SCS_UPLOAD_INIT_RSP (2 << 5) -#define CO_SDO_SCS_DOWNLOAD_INIT_RSP (3 << 5) - -#define CO_SDO_xCS_ABORT (4 << 5) - -#define CO_SDO_TOGGLE BIT (4) -#define CO_SDO_N_SEG(v) (((v) >> 1) & 0x07) -#define CO_SDO_C BIT (0) - void co_sdo_abort ( co_net_t * net, uint16_t id, From 32bb524e037f13bb379adcc3ea8f056893d49bda Mon Sep 17 00:00:00 2001 From: Daniel Udd Date: Mon, 6 Mar 2023 18:30:20 +0100 Subject: [PATCH 5/5] Fix segmented download of objects larger than 64 bits The notification callback was not being triggered for downloads of objects larger than 64 bits. Changed default timeout to 200ms for ongoing SDO transfers. --- CMakeLists.txt | 2 +- src/co_sdo_server.c | 203 +++++++++++++++++++++++++++++++------------- 2 files changed, 143 insertions(+), 62 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f05d4fd..efef17d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,7 +106,7 @@ set(MAX_RX_PDO "4" set(MAX_ERRORS "4" CACHE STRING "max size of error list") -set(SDO_TIMEOUT "100" +set(SDO_TIMEOUT "200" CACHE STRING "timeout in ms for ongoing SDO transfers") set(CO_THREAD_PRIO "10" diff --git a/src/co_sdo_server.c b/src/co_sdo_server.c index 05f473d..ba2c117 100644 --- a/src/co_sdo_server.c +++ b/src/co_sdo_server.c @@ -162,8 +162,7 @@ static int co_sdo_rx_upload_init_req ( if (job->sdo.remain <= sizeof (job->sdo.value)) { /* Object values up to 64 bits are fetched atomically */ - abort = - co_od_get_value (net, obj, entry, job->sdo.subindex, &job->sdo.value); + abort = co_od_get_value (net, obj, entry, job->sdo.subindex, &job->sdo.value); job->sdo.data = (uint8_t *)&job->sdo.value; } else @@ -269,6 +268,48 @@ static int co_sdo_rx_upload_seg_req ( return 0; } +static bool co_is_datatype_atomic (co_dtype_t datatype) +{ + switch (datatype) + { + case DTYPE_BOOLEAN: + case DTYPE_INTEGER8: + case DTYPE_INTEGER16: + case DTYPE_INTEGER32: + case DTYPE_UNSIGNED8: + case DTYPE_UNSIGNED16: + case DTYPE_UNSIGNED32: + case DTYPE_REAL32: + case DTYPE_REAL64: + case DTYPE_INTEGER64: + case DTYPE_UNSIGNED64: + return true; + + case DTYPE_VISIBLE_STRING: + case DTYPE_OCTET_STRING: + case DTYPE_UNICODE_STRING: + case DTYPE_TIME_OF_DAY: + case DTYPE_TIME_DIFFERENCE: + case DTYPE_DOMAIN: + case DTYPE_INTEGER24: + case DTYPE_INTEGER40: + case DTYPE_INTEGER48: + case DTYPE_INTEGER56: + case DTYPE_UNSIGNED24: + case DTYPE_UNSIGNED40: + case DTYPE_UNSIGNED48: + case DTYPE_UNSIGNED56: + case DTYPE_PDO_COMM_PARAM: + case DTYPE_PDO_MAPPING: + case DTYPE_SDO_PARAM: + case DTYPE_IDENTITY: + return false; + + default: + return false; + } +} + static int co_sdo_rx_download_init_req ( co_net_t * net, uint8_t node, @@ -327,40 +368,13 @@ static int co_sdo_rx_download_init_req ( return -1; } - job->sdo.remain = CO_BYTELENGTH (entry->bitlength); - job->sdo.toggle = 0; - - if (job->sdo.remain <= sizeof (job->sdo.value)) - { - /* Object values up to 64 bits are cached so that we can set - them atomically when the transfer is complete */ - job->sdo.data = (uint8_t *)&job->sdo.value; - job->sdo.cached = true; - } - else - { - /* Otherwise a pointer is used to access object */ - abort = co_od_get_ptr (net, obj, entry, job->sdo.subindex, &job->sdo.data); - if (abort) - { - co_sdo_abort ( - net, - 0x580 + net->node, - job->sdo.index, - job->sdo.subindex, - abort); - return -1; - } - } - /* Check for expedited download */ if (type & CO_SDO_E) { size_t size = (type & CO_SDO_S) ? 4 - CO_SDO_N (type) : 4; - uint32_t value; /* Validate size */ - if (size != job->sdo.remain) + if (size > CO_BYTELENGTH (entry->bitlength)) { co_sdo_abort ( net, @@ -371,11 +385,35 @@ static int co_sdo_rx_download_init_req ( return -1; } - /* Fetch value */ - value = co_fetch_uint32 (&data[4]); + if (co_is_datatype_atomic (entry->datatype)) + { + uint32_t value; - /* Atomically set value */ - abort = co_od_set_value (net, obj, entry, job->sdo.subindex, value); + /* Fetch value */ + value = co_fetch_uint32 (&data[4]); + + /* Atomically set value */ + abort = co_od_set_value (net, obj, entry, job->sdo.subindex, value); + } + else + { + /* Pointer is used to access object */ + abort = co_od_get_ptr (net, obj, entry, job->sdo.subindex, &job->sdo.data); + if (abort) + { + co_sdo_abort ( + net, + 0x580 + net->node, + job->sdo.index, + job->sdo.subindex, + abort); + return -1; + } + + memcpy (job->sdo.data, &data[4], size); + + co_od_notify (net, obj, entry, job->sdo.subindex, OD_NOTIFY_SDO_RECEIVED, size); + } /* Done */ job->type = CO_JOB_NONE; @@ -391,6 +429,46 @@ static int co_sdo_rx_download_init_req ( return -1; } } + else + { + job->sdo.total = co_fetch_uint32 (&data[4]); + job->sdo.remain = job->sdo.total; + + if (job->sdo.remain > CO_BYTELENGTH (entry->bitlength)) + { + co_sdo_abort ( + net, + 0x580 + net->node, + job->sdo.index, + job->sdo.subindex, + CO_SDO_ABORT_LENGTH); + return -1; + } + job->sdo.toggle = 0; + + if (co_is_datatype_atomic (entry->datatype)) + { + /* Datatypes with atomic functions are cached in job->sdo.value + * so they can be set atomically when the transfer is complete */ + job->sdo.data = (uint8_t *)&job->sdo.value; + job->sdo.cached = true; + } + else + { + /* Otherwise a pointer is used to access object */ + abort = co_od_get_ptr (net, obj, entry, job->sdo.subindex, &job->sdo.data); + if (abort) + { + co_sdo_abort ( + net, + 0x580 + net->node, + job->sdo.index, + job->sdo.subindex, + abort); + return -1; + } + } + } /* Dictionary has been written to and is now dirty */ net->config_dirty = 1; @@ -447,37 +525,36 @@ static int co_sdo_rx_download_seg_req ( /* Write complete */ job->type = CO_JOB_NONE; - if (job->sdo.cached) + /* Find requested object */ + obj = co_obj_find (net, job->sdo.index); + if (obj == NULL) { - /* Find requested object */ - obj = co_obj_find (net, job->sdo.index); - if (obj == NULL) - { - co_sdo_abort ( - net, - 0x580 + net->node, - job->sdo.index, - job->sdo.subindex, - CO_SDO_ABORT_BAD_INDEX); - return -1; - } + co_sdo_abort ( + net, + 0x580 + net->node, + job->sdo.index, + job->sdo.subindex, + CO_SDO_ABORT_BAD_INDEX); + return -1; + } - /* Find requested subindex */ - entry = co_entry_find (net, obj, job->sdo.subindex); - if (entry == NULL) - { - co_sdo_abort ( - net, - 0x580 + net->node, - job->sdo.index, - job->sdo.subindex, - CO_SDO_ABORT_BAD_SUBINDEX); - return -1; - } + /* Find requested subindex */ + entry = co_entry_find (net, obj, job->sdo.subindex); + if (entry == NULL) + { + co_sdo_abort ( + net, + 0x580 + net->node, + job->sdo.index, + job->sdo.subindex, + CO_SDO_ABORT_BAD_SUBINDEX); + return -1; + } + if (job->sdo.cached) + { /* Atomically set value */ - abort = - co_od_set_value (net, obj, entry, job->sdo.subindex, job->sdo.value); + abort = co_od_set_value (net, obj, entry, job->sdo.subindex, job->sdo.value); if (abort) { co_sdo_abort ( @@ -489,6 +566,10 @@ static int co_sdo_rx_download_seg_req ( return -1; } } + else + { + co_od_notify (net, obj, entry, job->sdo.subindex, OD_NOTIFY_SDO_RECEIVED, job->sdo.total); + } } /* Segmented response */