From c814d2dacfe573698094498966ae4e6710f5d06d Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Wed, 23 Sep 2020 14:50:43 +0200 Subject: [PATCH 1/4] Allow the pre_object_upload_hook to modify size Change-Id: Ia95a1e8e3ba7e6bde3286f4eb15f8ba7f0cb85b2 --- soes/ecat_slv.c | 2 +- soes/esc.h | 4 +-- soes/esc_coe.c | 74 ++++++++++++++++++++++++------------------------- soes/esc_coe.h | 2 +- 4 files changed, 40 insertions(+), 42 deletions(-) diff --git a/soes/ecat_slv.c b/soes/ecat_slv.c index 16b3fc2..e1e6438 100644 --- a/soes/ecat_slv.c +++ b/soes/ecat_slv.c @@ -96,7 +96,7 @@ uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint uint32_t ESC_upload_pre_objecthandler (uint16_t index, uint8_t subindex, void * data, - size_t size, + size_t *size, uint16_t flags) { if (ESCvar.pre_object_upload_hook != NULL) diff --git a/soes/esc.h b/soes/esc.h index fda0b0d..73f5328 100644 --- a/soes/esc.h +++ b/soes/esc.h @@ -326,7 +326,7 @@ typedef struct esc_cfg uint32_t (*pre_object_upload_hook) (uint16_t index, uint8_t subindex, void * data, - size_t size, + size_t *size, uint16_t flags); uint32_t (*post_object_upload_hook) (uint16_t index, uint8_t subindex, @@ -450,7 +450,7 @@ typedef struct uint32_t (*pre_object_upload_hook) (uint16_t index, uint8_t subindex, void * data, - size_t size, + size_t *size, uint16_t flags); uint32_t (*post_object_upload_hook) (uint16_t index, uint8_t subindex, diff --git a/soes/esc_coe.c b/soes/esc_coe.c index 58fe954..bc046fa 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -330,17 +330,18 @@ static void SDO_upload (void) } coeres->index = htoes (index); coeres->subindex = subindex; - if (size <= 32) + coeres->command = COE_COMMAND_UPLOADRESPONSE + + COE_SIZE_INDICATOR; + /* convert bits to bytes */ + size = BITS2BYTES(size); + if (size <= 4) { /* expedited response i.e. length<=4 bytes */ - coeres->command = COE_COMMAND_UPLOADRESPONSE + - COE_SIZE_INDICATOR + COE_EXPEDITED_INDICATOR + dss; - /* convert bits to bytes */ - size = BITS2BYTES(size); + coeres->command += COE_EXPEDITED_INDICATOR + dss; void *dataptr = ((objd + nsub)->data) ? (objd + nsub)->data : (void *)&((objd + nsub)->value); abort = ESC_upload_pre_objecthandler (index, subindex, - dataptr, size, (objd + nsub)->flags); + dataptr, (size_t *)&size, (objd + nsub)->flags); if (abort == 0) { if ((objd + nsub)->data == NULL) @@ -362,34 +363,31 @@ static void SDO_upload (void) else { /* normal response i.e. length>4 bytes */ - coeres->command = COE_COMMAND_UPLOADRESPONSE + - COE_SIZE_INDICATOR; - /* convert bits to bytes */ - size = BITS2BYTES(size); - /* set total size in bytes */ - ESCvar.frags = size; - coeres->size = htoel (size); - if ((size + COE_HEADERSIZE) > ESC_MBXDSIZE) - { - /* segmented transfer needed */ - /* limit to mailbox size */ - size = ESC_MBXDSIZE - COE_HEADERSIZE; - /* number of bytes done */ - ESCvar.fragsleft = size; - /* signal segmented transfer */ - ESCvar.segmented = MBXSEU; - ESCvar.data = (objd + nsub)->data; - ESCvar.flags = (objd + nsub)->flags; - } - else - { - ESCvar.segmented = 0; - } - coeres->mbxheader.length = htoes (COE_HEADERSIZE + size); abort = ESC_upload_pre_objecthandler (index, subindex, - (objd + nsub)->data, ESCvar.frags, (objd + nsub)->flags); + (objd + nsub)->data, (size_t *)&size, (objd + nsub)->flags); if (abort == 0) { + /* set total size in bytes */ + ESCvar.frags = size; + coeres->size = htoel (size); + if ((size + COE_HEADERSIZE) > ESC_MBXDSIZE) + { + /* segmented transfer needed */ + /* limit to mailbox size */ + size = ESC_MBXDSIZE - COE_HEADERSIZE; + /* number of bytes done */ + ESCvar.fragsleft = size; + /* signal segmented transfer */ + ESCvar.segmented = MBXSEU; + ESCvar.data = (objd + nsub)->data; + ESCvar.flags = (objd + nsub)->flags; + } + else + { + ESCvar.segmented = 0; + } + coeres->mbxheader.length = htoes (COE_HEADERSIZE + size); + /* use dynamic data */ copy2mbx ((objd + nsub)->data, (&(coeres->size)) + 1, size); } @@ -597,8 +595,14 @@ static void SDO_upload_complete_access (void) return; } + /* expedited bits used calculation */ + uint8_t dss = (size > 24) ? 0 : (4 * (3 - ((size - 1) >> 3))); + + /* convert bits to bytes */ + size = BITS2BYTES(size); + abortcode = ESC_upload_pre_objecthandler(index, subindex, - objd->data, BITS2BYTES(size), objd->flags | COMPLETE_ACCESS_FLAG); + objd->data, (size_t *)&size, objd->flags | COMPLETE_ACCESS_FLAG); if (abortcode != 0) { SDO_abort (index, subindex, abortcode); @@ -612,12 +616,6 @@ static void SDO_upload_complete_access (void) COE_COMMAND_UPLOADRESPONSE | COE_COMPLETEACCESS | COE_SIZE_INDICATOR, index, subindex); - /* expedited bits used calculation */ - uint8_t dss = (size > 24) ? 0 : (4 * (3 - ((size - 1) >> 3))); - - /* convert bits to bytes */ - size = BITS2BYTES(size); - ESCvar.segmented = 0; if (size <= 4) diff --git a/soes/esc_coe.h b/soes/esc_coe.h index 689750c..0b2e7a6 100644 --- a/soes/esc_coe.h +++ b/soes/esc_coe.h @@ -127,7 +127,7 @@ extern uint32_t ESC_download_pre_objecthandler (uint16_t index, extern uint32_t ESC_upload_pre_objecthandler (uint16_t index, uint8_t subindex, void * data, - size_t size, + size_t *size, uint16_t flags); extern uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags); extern const _objectlist SDOobjects[]; From 48772707a6f85acaa086cdc04a156b8f636439d2 Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Tue, 13 Oct 2020 09:50:27 +0200 Subject: [PATCH 2/4] Fix handling of large objects in SDO Upload with Complete Access Change-Id: I64471e2ac1ac1865db43c48fd15b0e1d44651a66 --- soes/esc.h | 1 - soes/esc_coe.c | 2 +- soes/options.h | 4 ++++ 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/soes/esc.h b/soes/esc.h index 73f5328..f6f4535 100644 --- a/soes/esc.h +++ b/soes/esc.h @@ -292,7 +292,6 @@ #define APPSTATE_INPUT 0x01 #define APPSTATE_OUTPUT 0x02 -#define PREALLOC_FACTOR 3 #define PREALLOC_BUFFER_SIZE (PREALLOC_FACTOR * MBXSIZE) typedef struct sm_cfg diff --git a/soes/esc_coe.c b/soes/esc_coe.c index bc046fa..f18140d 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -468,7 +468,7 @@ static uint32_t complete_access_subindex_loop(const _objd *objd, while (nsub <= SDOobjects[nidx].maxsub) { - uint8_t bitlen = (objd + nsub)->bitlength; + uint16_t bitlen = (objd + nsub)->bitlength; void *ul_source = ((objd + nsub)->data != NULL) ? (objd + nsub)->data : (void *)&((objd + nsub)->value); uint8_t bitoffset = size % 8; diff --git a/soes/options.h b/soes/options.h index 1f3db05..3415e07 100644 --- a/soes/options.h +++ b/soes/options.h @@ -31,6 +31,10 @@ #define MBXBUFFERS 3 #endif +#ifndef PREALLOC_FACTOR +#define PREALLOC_FACTOR 3 +#endif + #ifndef MBX0_sma #define MBX0_sma 0x1000 #endif From 87b2c1eb9372710ac61083319ecfd5d51e67948c Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Thu, 22 Oct 2020 09:37:57 +0200 Subject: [PATCH 3/4] Fix for very large objects (with Complete Access) Change-Id: I95c7b0f73e581ef6dace14e9b8f02ceff50f38ca --- soes/esc_coe.c | 87 +++++++++++++++++++++++++++++--------------------- soes/esc_coe.h | 1 - 2 files changed, 51 insertions(+), 37 deletions(-) diff --git a/soes/esc_coe.c b/soes/esc_coe.c index f18140d..ceb78ea 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -236,15 +236,19 @@ static void copy2mbx (void *source, void *dest, uint16_t size) /** Function for sending an SDO Abort reply. * + * @param[in] reusembx = mailbox buffer to use (if 0 then claim a new buffer) * @param[in] index = index of object causing abort reply * @param[in] sub-index = sub-index of object causing abort reply * @param[in] abortcode = abort code to send in reply */ -void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode) +static void SDO_abort (uint8_t reusembx, uint16_t index, uint8_t subindex, uint32_t abortcode) { uint8_t MBXout; _COEsdo *coeres; - MBXout = ESC_claimbuffer (); + if (reusembx) + MBXout = reusembx; + else + MBXout = ESC_claimbuffer (); if (MBXout) { coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE]; @@ -260,13 +264,14 @@ void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode) } } -static void set_state_idle(uint16_t index, +static void set_state_idle (uint8_t reusembx, + uint16_t index, uint8_t subindex, uint32_t abortcode) { if (abortcode != 0) { - SDO_abort (index, subindex, abortcode); + SDO_abort (reusembx, index, subindex, abortcode); } MBXcontrol[0].state = MBXstate_idle; @@ -302,7 +307,7 @@ static void SDO_upload (void) uint8_t state = ESCvar.ALstatus & 0x0f; if (!READ_ACCESS(access, state)) { - set_state_idle (index, subindex, ABORT_WRITEONLY); + set_state_idle (0, index, subindex, ABORT_WRITEONLY); return; } MBXout = ESC_claimbuffer (); @@ -357,7 +362,8 @@ static void SDO_upload (void) } else { - SDO_abort (index, subindex, abort); + set_state_idle (MBXout, index, subindex, abort); + return; } } else @@ -393,7 +399,8 @@ static void SDO_upload (void) } else { - SDO_abort (index, subindex, abort); + set_state_idle (MBXout, index, subindex, abort); + return; } } if ((abort == 0) && (ESCvar.segmented == 0)) @@ -402,7 +409,8 @@ static void SDO_upload (void) (objd + nsub)->flags); if (abort != 0) { - SDO_abort (index, subindex, abort); + set_state_idle (MBXout, index, subindex, abort); + return; } } MBXcontrol[MBXout].state = MBXstate_outreq; @@ -410,12 +418,12 @@ static void SDO_upload (void) } else { - SDO_abort (index, subindex, ABORT_NOSUBINDEX); + SDO_abort (0, index, subindex, ABORT_NOSUBINDEX); } } else { - SDO_abort (index, subindex, ABORT_NOOBJECT); + SDO_abort (0, index, subindex, ABORT_NOOBJECT); } MBXcontrol[0].state = MBXstate_idle; ESCvar.xoe = 0; @@ -512,6 +520,10 @@ static uint32_t complete_access_subindex_loop(const _objd *objd, uint8_t bitmask = (1 << bitlen) - 1; if (READ_ACCESS(access, state)) { + if (bitoffset == 0) + { + mbxdata[BITS2BYTES(size)] = 0; + } mbxdata[BITS2BYTES(size)] |= (*(uint8_t *)ul_source & bitmask) << bitoffset; } @@ -563,7 +575,7 @@ static void SDO_upload_complete_access (void) (coesdo, &index, &subindex, &nidx, &nsub); if (abortcode != 0) { - set_state_idle (index, subindex, abortcode); + set_state_idle (0, index, subindex, abortcode); return; } @@ -573,7 +585,7 @@ static void SDO_upload_complete_access (void) /* It is a bad idea to call SDO_abort when ESC_claimbuffer fails, * because SDO_abort will also call ESC_claimbuffer ... */ - set_state_idle (index, subindex, 0); + set_state_idle (0, index, subindex, 0); return; } @@ -581,17 +593,17 @@ static void SDO_upload_complete_access (void) /* loop through the subindexes to get the total size */ uint32_t size = complete_access_subindex_loop(objd, nidx, nsub, NULL, UPLOAD, 0); - if (size > 0xffff) + if (BITS2BYTES(size) > 0xffff) { /* 'size' is in this case actually an abort code */ - set_state_idle (index, subindex, size); + set_state_idle (MBXout, index, subindex, size); return; } /* check that upload data fits in the preallocated buffer */ if ((size + PREALLOC_FACTOR * COE_HEADERSIZE) > PREALLOC_BUFFER_SIZE) { - set_state_idle (index, subindex, ABORT_GENERALERROR); + set_state_idle (MBXout, index, subindex, ABORT_CA_NOT_SUPPORTED); return; } @@ -605,7 +617,8 @@ static void SDO_upload_complete_access (void) objd->data, (size_t *)&size, objd->flags | COMPLETE_ACCESS_FLAG); if (abortcode != 0) { - SDO_abort (index, subindex, abortcode); + set_state_idle (MBXout, index, subindex, abortcode); + return; } /* copy subindex data into the preallocated buffer */ @@ -655,13 +668,14 @@ static void SDO_upload_complete_access (void) if (abortcode != 0) { - SDO_abort (index, subindex, abortcode); + set_state_idle (MBXout, index, subindex, abortcode); + return; } } MBXcontrol[MBXout].state = MBXstate_outreq; - set_state_idle (index, subindex, 0); + set_state_idle (MBXout, index, subindex, 0); } /** Function for handling the following SDO Upload if previous SDOUpload @@ -718,7 +732,8 @@ static void SDO_uploadsegment (void) coesdo->subindex, ESCvar.flags); if (abort != 0) { - SDO_abort (etohs (coesdo->index), coesdo->subindex, abort); + set_state_idle (MBXout, etohs (coesdo->index), coesdo->subindex, abort); + return; } } @@ -790,7 +805,7 @@ static void SDO_download (void) (type != DTYPE_ARRAY_OF_DINT) && (type != DTYPE_ARRAY_OF_UDINT)) { - set_state_idle (index, subindex, ABORT_TYPEMISMATCH); + set_state_idle (0, index, subindex, ABORT_TYPEMISMATCH); return; } } @@ -839,35 +854,35 @@ static void SDO_download (void) abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags); if (abort != 0) { - SDO_abort (index, subindex, abort); + SDO_abort (MBXout, index, subindex, abort); } } } else { - SDO_abort (index, subindex, abort); + SDO_abort (0, index, subindex, abort); } } else { if (access == ATYPE_RWpre) { - SDO_abort (index, subindex, ABORT_NOTINTHISSTATE); + SDO_abort (0, index, subindex, ABORT_NOTINTHISSTATE); } else { - SDO_abort (index, subindex, ABORT_READONLY); + SDO_abort (0, index, subindex, ABORT_READONLY); } } } else { - SDO_abort (index, subindex, ABORT_NOSUBINDEX); + SDO_abort (0, index, subindex, ABORT_NOSUBINDEX); } } else { - SDO_abort (index, subindex, ABORT_NOOBJECT); + SDO_abort (0, index, subindex, ABORT_NOOBJECT); } MBXcontrol[0].state = MBXstate_idle; ESCvar.xoe = 0; @@ -887,7 +902,7 @@ static void SDO_download_complete_access (void) (coesdo, &index, &subindex, &nidx, &nsub); if (abortcode != 0) { - set_state_idle (index, subindex, abortcode); + set_state_idle (0, index, subindex, abortcode); return; } @@ -910,10 +925,10 @@ static void SDO_download_complete_access (void) /* loop through the subindexes to get the total size */ uint32_t size = complete_access_subindex_loop(objd, nidx, nsub, NULL, DOWNLOAD, 0); - if (size > 0xffff) + if (BITS2BYTES(size) > 0xffff) { /* 'size' is in this case actually an abort code */ - set_state_idle (index, subindex, size); + set_state_idle (0, index, subindex, size); return; } /* The document ETG.1020 S (R) V1.3.0, chapter 12.2, states that @@ -928,7 +943,7 @@ static void SDO_download_complete_access (void) size, objd->flags | COMPLETE_ACCESS_FLAG); if (abortcode != 0) { - set_state_idle (index, subindex, abortcode); + set_state_idle (0, index, subindex, abortcode); return; } @@ -939,13 +954,13 @@ static void SDO_download_complete_access (void) objd->flags | COMPLETE_ACCESS_FLAG); if (abortcode != 0) { - set_state_idle (index, subindex, abortcode); + set_state_idle (0, index, subindex, abortcode); return; } } else { - set_state_idle (index, subindex, ABORT_TYPEMISMATCH); + set_state_idle (0, index, subindex, ABORT_TYPEMISMATCH); return; } @@ -961,7 +976,7 @@ static void SDO_download_complete_access (void) MBXcontrol[MBXout].state = MBXstate_outreq; } - set_state_idle (index, subindex, 0); + set_state_idle (MBXout, index, subindex, 0); } static void SDO_downloadsegment (void) @@ -993,7 +1008,7 @@ static void SDO_downloadsegment (void) (ESCvar.index, ESCvar.subindex, ESCvar.flags); if (abort != 0) { - set_state_idle (ESCvar.index, ESCvar.subindex, abort); + set_state_idle (MBXout, ESCvar.index, ESCvar.subindex, abort); return; } } @@ -1006,7 +1021,7 @@ static void SDO_downloadsegment (void) MBXcontrol[MBXout].state = MBXstate_outreq; } - set_state_idle (0, 0, 0); + set_state_idle (0, 0, 0, 0); } /** Function for sending an SDO Info Error reply. @@ -1435,7 +1450,7 @@ void ESC_coeprocess (void) } else { - SDO_abort (etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED); + SDO_abort (0, etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED); } MBXcontrol[0].state = MBXstate_idle; ESCvar.xoe = 0; diff --git a/soes/esc_coe.h b/soes/esc_coe.h index 0b2e7a6..b54ac7b 100644 --- a/soes/esc_coe.h +++ b/soes/esc_coe.h @@ -111,7 +111,6 @@ void ESC_coeprocess (void); int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex); int32_t SDO_findobject (uint16_t index); uint16_t sizeOfPDO (uint16_t index, int * nmappings, _SMmap * sm, int max_mappings); -void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode); void COE_initDefaultValues (void); void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * sm); From 8d5afc9b2d05bf349ee2e03f45c1ea1baea5241c Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Wed, 10 Feb 2021 14:58:15 +0100 Subject: [PATCH 4/4] Convert bits to bytes in the correct place --- soes/esc_coe.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/soes/esc_coe.c b/soes/esc_coe.c index ceb78ea..d1e4d75 100644 --- a/soes/esc_coe.c +++ b/soes/esc_coe.c @@ -593,7 +593,14 @@ static void SDO_upload_complete_access (void) /* loop through the subindexes to get the total size */ uint32_t size = complete_access_subindex_loop(objd, nidx, nsub, NULL, UPLOAD, 0); - if (BITS2BYTES(size) > 0xffff) + + /* expedited bits used calculation */ + uint8_t dss = (size > 24) ? 0 : (4 * (3 - ((size - 1) >> 3))); + + /* convert bits to bytes */ + size = BITS2BYTES(size); + + if (size > 0xffff) { /* 'size' is in this case actually an abort code */ set_state_idle (MBXout, index, subindex, size); @@ -606,13 +613,6 @@ static void SDO_upload_complete_access (void) set_state_idle (MBXout, index, subindex, ABORT_CA_NOT_SUPPORTED); return; } - - /* expedited bits used calculation */ - uint8_t dss = (size > 24) ? 0 : (4 * (3 - ((size - 1) >> 3))); - - /* convert bits to bytes */ - size = BITS2BYTES(size); - abortcode = ESC_upload_pre_objecthandler(index, subindex, objd->data, (size_t *)&size, objd->flags | COMPLETE_ACCESS_FLAG); if (abortcode != 0)