From cde698b1eac2124656ea0da7dc9ef8a2231236d4 Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Mon, 17 Aug 2020 10:48:27 +0200 Subject: [PATCH 1/4] Handle SDO entries with flexible length Change-Id: I1d2e1549c497aaef801b1ea42342843e5ff3e73d --- soes/esc_coe.c | 101 +++++++++++++++++++++++++++---------------------- 1 file changed, 56 insertions(+), 45 deletions(-) diff --git a/soes/esc_coe.c b/soes/esc_coe.c index 73156fd..674d0d0 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -737,65 +737,76 @@ void SDO_download (void) mbxdata = (&(coesdo->size)) + 1; } actsize = BITS2BYTES((objd + nsub)->bitlength); - if (actsize == size) + if (actsize != size) { - abort = ESC_download_pre_objecthandler ( + /* entries with data types VISIBLE_STRING, OCTET_STRING, + * and UNICODE_STRING may have flexible length + */ + uint16_t type = (objd + nsub)->datatype; + if (type == DTYPE_VISIBLE_STRING) + { + /* pad with zeroes up to the maximum size of the entry */ + memset((objd + nsub)->data + size, 0, actsize - size); + } + else if ((type != DTYPE_OCTET_STRING) && + (type != DTYPE_UNICODE_STRING)) + { + set_state_idle (index, subindex, ABORT_TYPEMISMATCH); + return; + } + } + abort = ESC_download_pre_objecthandler ( index, subindex, mbxdata, size, (objd + nsub)->flags - ); - if (abort == 0) + ); + if (abort == 0) + { + if ((size > 4) && + (size > (coesdo->mbxheader.length - COE_HEADERSIZE))) { - if ((size > 4) && - (size > (coesdo->mbxheader.length - COE_HEADERSIZE))) - { - size = coesdo->mbxheader.length - COE_HEADERSIZE; - /* signal segmented transfer */ - ESCvar.segmented = MBXSED; - ESCvar.data = (objd + nsub)->data + size; - ESCvar.index = index; - ESCvar.subindex = subindex; - ESCvar.flags = (objd + nsub)->flags; - } - else - { - ESCvar.segmented = 0; - } - copy2mbx (mbxdata, (objd + nsub)->data, size); - MBXout = ESC_claimbuffer (); - if (MBXout) - { - coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE]; - coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH); - coeres->mbxheader.mbxtype = MBXCOE; - coeres->coeheader.numberservice = - htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12)); - coeres->index = htoes (index); - coeres->subindex = subindex; - coeres->command = COE_COMMAND_DOWNLOADRESPONSE; - coeres->size = htoel (0); - MBXcontrol[MBXout].state = MBXstate_outreq; - } - if (ESCvar.segmented == 0) - { - /* external object write handler */ - abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags); - if (abort != 0) - { - SDO_abort (index, subindex, abort); - } - } + size = coesdo->mbxheader.length - COE_HEADERSIZE; + /* signal segmented transfer */ + ESCvar.segmented = MBXSED; + ESCvar.data = (objd + nsub)->data + size; + ESCvar.index = index; + ESCvar.subindex = subindex; + ESCvar.flags = (objd + nsub)->flags; } else { - SDO_abort (index, subindex, abort); + ESCvar.segmented = 0; + } + copy2mbx (mbxdata, (objd + nsub)->data, size); + MBXout = ESC_claimbuffer (); + if (MBXout) + { + coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE]; + coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH); + coeres->mbxheader.mbxtype = MBXCOE; + coeres->coeheader.numberservice = + htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12)); + coeres->index = htoes (index); + coeres->subindex = subindex; + coeres->command = COE_COMMAND_DOWNLOADRESPONSE; + coeres->size = htoel (0); + MBXcontrol[MBXout].state = MBXstate_outreq; + } + if (ESCvar.segmented == 0) + { + /* external object write handler */ + abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags); + if (abort != 0) + { + SDO_abort (index, subindex, abort); + } } } else { - SDO_abort (index, subindex, ABORT_TYPEMISMATCH); + SDO_abort (index, subindex, abort); } } else From 86c17dbb14c3fd950341ae0488730acd4f2386cc Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Mon, 17 Aug 2020 11:15:28 +0200 Subject: [PATCH 2/4] Add handling of write-only objects Change-Id: I9bfdb11c5d1d83cb2c23115e204ffe6875b4457b --- soes/esc_coe.c | 87 +++++++++++++++++++++++++++++++------------------- soes/esc_coe.h | 3 +- 2 files changed, 56 insertions(+), 34 deletions(-) diff --git a/soes/esc_coe.c b/soes/esc_coe.c index 674d0d0..62831c0 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -29,6 +29,16 @@ #define SDO_COMPLETE_ACCESS(byte) \ ((byte & COE_COMPLETEACCESS) == COE_COMPLETEACCESS) +#define READ_ACCESS(access, state) \ + (((access & ATYPE_Rpre) && (state == ESCpreop)) || \ + ((access & ATYPE_Rsafe) && (state == ESCsafeop)) || \ + ((access & ATYPE_Rop) && (state == ESCop))) + +#define WRITE_ACCESS(access, state) \ + (((access & ATYPE_Wpre) && (state == ESCpreop)) || \ + ((access & ATYPE_Wsafe) && (state == ESCsafeop)) || \ + ((access & ATYPE_Wop) && (state == ESCop))) + typedef enum { UPLOAD, DOWNLOAD } load_t; /** Search for an object sub-index. @@ -241,6 +251,19 @@ void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode) } } +static void set_state_idle(uint16_t index, + uint8_t subindex, + uint32_t abortcode) +{ + if (abortcode != 0) + { + SDO_abort (index, subindex, abortcode); + } + + MBXcontrol[0].state = MBXstate_idle; + ESCvar.xoe = 0; +} + /** Function for responding on requested SDO Upload, sending the content * requested in a free Mailbox buffer. Depending of size of data expedited, * normal or segmented transfer is used. On error an SDO Abort will be sent. @@ -266,6 +289,13 @@ void SDO_upload (void) if (nsub >= 0) { objd = SDOobjects[nidx].objdesc; + uint8_t access = (objd + nsub)->flags & 0x3f; + uint8_t state = ESCvar.ALstatus & 0x0f; + if (!READ_ACCESS(access, state)) + { + set_state_idle (index, subindex, ABORT_WRITEONLY); + return; + } MBXout = ESC_claimbuffer (); if (MBXout) { @@ -384,19 +414,6 @@ void SDO_upload (void) ESCvar.xoe = 0; } -static void set_state_idle(uint16_t index, - uint8_t subindex, - uint32_t abortcode) -{ - if (abortcode != 0) - { - SDO_abort (index, subindex, abortcode); - } - - MBXcontrol[0].state = MBXstate_idle; - ESCvar.xoe = 0; -} - static uint32_t complete_access_get_variables(_COEsdo *coesdo, uint16_t *index, uint8_t *subindex, int16_t *nidx, int16_t *nsub) @@ -443,23 +460,12 @@ static uint32_t complete_access_subindex_loop(const _objd *objd, while (nsub <= SDOobjects[nidx].maxsub) { - if (load_type == DOWNLOAD) - { - uint8_t access = (objd + nsub)->flags & 0x3f; - uint8_t state = ESCvar.ALstatus & 0x0f; - if ((access != ATYPE_RW) && - ((access != ATYPE_RWpre) || - ((state != ESCpreop) && (state != ESCsafeop) && (state != ESCop)))) - { - return (access == ATYPE_RWpre) ? - ABORT_NOTINTHISSTATE : ABORT_READONLY; - } - } - uint8_t bitlen = (objd + nsub)->bitlength; void *ul_source = ((objd + nsub)->data != NULL) ? (objd + nsub)->data : (void *)&((objd + nsub)->value); uint8_t bitoffset = size % 8; + uint8_t access = (objd + nsub)->flags & 0x3f; + uint8_t state = ESCvar.ALstatus & 0x0f; if ((bitlen % 8) == 0) { @@ -473,10 +479,19 @@ static uint32_t complete_access_subindex_loop(const _objd *objd, /* copy a non-bit data type to a byte boundary */ if (load_type == UPLOAD) { - memcpy(&mbxdata[BITS2BYTES(size)], ul_source, - BITS2BYTES(bitlen)); + if (READ_ACCESS(access, state)) + { + memcpy(&mbxdata[BITS2BYTES(size)], ul_source, + BITS2BYTES(bitlen)); + } + else + { + /* return zeroes for upload of WO objects */ + memset(&mbxdata[BITS2BYTES(size)], 0, BITS2BYTES(bitlen)); + } } - else + /* download of RO objects shall be ignored */ + else if (WRITE_ACCESS(access, state)) { memcpy((objd + nsub)->data, &mbxdata[BITS2BYTES(size)], BITS2BYTES(bitlen)); @@ -487,8 +502,15 @@ static uint32_t complete_access_subindex_loop(const _objd *objd, { /* copy a bit data type into correct position */ uint8_t bitmask = (1 << bitlen) - 1; - mbxdata[BITS2BYTES(size)] |= - (*(uint8_t *)ul_source & bitmask) << bitoffset; + if (READ_ACCESS(access, state)) + { + mbxdata[BITS2BYTES(size)] |= + (*(uint8_t *)ul_source & bitmask) << bitoffset; + } + else + { + mbxdata[BITS2BYTES(size)] &= ~(bitmask << bitoffset); + } } /* Subindex 0 is padded to 16 bit */ @@ -721,8 +743,7 @@ void SDO_download (void) objd = SDOobjects[nidx].objdesc; uint8_t access = (objd + nsub)->flags & 0x3f; uint8_t state = ESCvar.ALstatus & 0x0f; - if (access == ATYPE_RW || - (access == ATYPE_RWpre && state == ESCpreop)) + if (WRITE_ACCESS(access, state)) { /* expedited? */ if (coesdo->command & COE_EXPEDITED_INDICATOR) diff --git a/soes/esc_coe.h b/soes/esc_coe.h index 20e25d5..fd96ba0 100644 --- a/soes/esc_coe.h +++ b/soes/esc_coe.h @@ -94,7 +94,8 @@ typedef struct #define ATYPE_TXPDO 0x80 #define ATYPE_RO (ATYPE_Rpre | ATYPE_Rsafe | ATYPE_Rop) -#define ATYPE_RW (ATYPE_Wpre | ATYPE_Wsafe | ATYPE_Wop | ATYPE_RO) +#define ATYPE_WO (ATYPE_Wpre | ATYPE_Wsafe | ATYPE_Wop) +#define ATYPE_RW (ATYPE_RO | ATYPE_WO) #define ATYPE_RWpre (ATYPE_Wpre | ATYPE_RO) #define TX_PDO_OBJIDX 0x1c13 From 12006a53a50533751125b3b4534e0d3322fc873c Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Mon, 17 Aug 2020 11:37:29 +0200 Subject: [PATCH 3/4] Make not exported functions static declared Change-Id: I7e9bdb7f651ab26bffc3466f8e3a050db91659c6 --- soes/esc_coe.c | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/soes/esc_coe.c b/soes/esc_coe.c index 62831c0..a212810 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -47,7 +47,7 @@ typedef enum { UPLOAD, DOWNLOAD } load_t; * @param[in] subindex = value on sub-index of object we want to locate * @return local array index if we succeed, -1 if we didn't find the index. */ -int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex) +static int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex) { const _objd *objd; int16_t n = 0; @@ -220,7 +220,7 @@ uint16_t sizeOfPDO (uint16_t index, int * nmappings, _SMmap * mappings, * @param[in] dest = pointer to destination * @param[in] size = Size to copy */ -void copy2mbx (void *source, void *dest, uint16_t size) +static void copy2mbx (void *source, void *dest, uint16_t size) { memcpy (dest, source, size); } @@ -268,7 +268,7 @@ static void set_state_idle(uint16_t index, * requested in a free Mailbox buffer. Depending of size of data expedited, * normal or segmented transfer is used. On error an SDO Abort will be sent. */ -void SDO_upload (void) +static void SDO_upload (void) { _COEsdo *coesdo, *coeres; uint16_t index; @@ -719,7 +719,7 @@ static void SDO_uploadsegment (void) /** Function for handling incoming requested SDO Download, validating the * request and sending an response. On error an SDO Abort will be sent. */ -void SDO_download (void) +static void SDO_download (void) { _COEsdo *coesdo, *coeres; uint16_t index; @@ -997,7 +997,7 @@ static void SDO_downloadsegment (void) * * @param[in] abortcode = = abort code to send in reply */ -void SDO_infoerror (uint32_t abortcode) +static void SDO_infoerror (uint32_t abortcode) { uint8_t MBXout; _COEobjdesc *coeres; @@ -1027,7 +1027,7 @@ void SDO_infoerror (uint32_t abortcode) /** Function for handling incoming requested SDO Get OD List, validating the * request and sending an response. On error an SDO Info Error will be sent. */ -void SDO_getodlist (void) +static void SDO_getodlist (void) { uint16_t frags; uint8_t MBXout = 0; @@ -1119,9 +1119,8 @@ void SDO_getodlist (void) } /** Function for continuing sending left overs from previous requested * SDO Get OD List, validating the request and sending an response. - * */ -void SDO_getodlistcont (void) +static void SDO_getodlistcont (void) { uint8_t MBXout; uint16_t i, n, s; @@ -1168,7 +1167,7 @@ void SDO_getodlistcont (void) * validating the request and sending an response. On error an * SDO Info Error will be sent. */ -void SDO_getod (void) +static void SDO_getod (void) { uint8_t MBXout; uint16_t index; @@ -1240,7 +1239,7 @@ void SDO_getod (void) * validating the request and sending an response. On error an * SDO Info Error will be sent. */ -void SDO_geted (void) +static void SDO_geted (void) { uint8_t MBXout; uint16_t index; From 32c89019408c0126f46f528c994512570b5dbb22 Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Mon, 17 Aug 2020 11:55:17 +0200 Subject: [PATCH 4/4] Optimize function SDO_findsubindex() Since most objects contain all subindexes (i.e. are not sparse), check the most likely scenario first. Change-Id: Idfda8ae3a3903c8312e02c9354b241af804f69ed --- soes/esc_coe.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/soes/esc_coe.c b/soes/esc_coe.c index a212810..61fe5cb 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -54,6 +54,15 @@ static int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex) uint8_t maxsub; objd = SDOobjects[nidx].objdesc; maxsub = SDOobjects[nidx].maxsub; + + /* Since most objects contain all subindexes (i.e. are not sparse), + * check the most likely scenario first + */ + if ((subindex <= maxsub) && ((objd + subindex)->subindex == subindex)) + { + return subindex; + } + while (((objd + n)->subindex < subindex) && (n < maxsub)) { n++;