From 87b2c1eb9372710ac61083319ecfd5d51e67948c Mon Sep 17 00:00:00 2001 From: Lars Danielsson Date: Thu, 22 Oct 2020 09:37:57 +0200 Subject: [PATCH] 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);