ln-dotnet
/
SOES
mirror of https://github.com/OpenEtherCATsociety/SOES.git
3
0
Fork 0
Code Issues Projects Releases Wiki Activity

Fix for very large objects (with Complete Access) 

Change-Id: I95c7b0f73e581ef6dace14e9b8f02ceff50f38ca
pull/92/head
Lars Danielsson 2020-10-22 09:37:57 +02:00
parent 48772707a6
commit 87b2c1eb93
2 changed files with 51 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
soes/esc_coe.c
View File

@ -236,15 +236,19 @@ static void copy2mbx (void *source, void *dest, uint16_t size)
/** Function for sending an SDO Abort reply. /** 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] index = index of object causing abort reply
* @param[in] sub-index = sub-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 * @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; uint8_t MBXout;
_COEsdo *coeres; _COEsdo *coeres;
MBXout = ESC_claimbuffer (); if (reusembx)
MBXout = reusembx;
else
MBXout = ESC_claimbuffer ();
if (MBXout) if (MBXout)
{ {
coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE]; 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, uint8_t subindex,
uint32_t abortcode) uint32_t abortcode)
{ {
if (abortcode != 0) if (abortcode != 0)
{ {
SDO_abort (index, subindex, abortcode); SDO_abort (reusembx, index, subindex, abortcode);
} }
MBXcontrol[0].state = MBXstate_idle; MBXcontrol[0].state = MBXstate_idle;
@ -302,7 +307,7 @@ static void SDO_upload (void)
uint8_t state = ESCvar.ALstatus & 0x0f; uint8_t state = ESCvar.ALstatus & 0x0f;
if (!READ_ACCESS(access, state)) if (!READ_ACCESS(access, state))
{ {
set_state_idle (index, subindex, ABORT_WRITEONLY); set_state_idle (0, index, subindex, ABORT_WRITEONLY);
return; return;
} }
MBXout = ESC_claimbuffer (); MBXout = ESC_claimbuffer ();
@ -357,7 +362,8 @@ static void SDO_upload (void)
} }
else else
{ {
SDO_abort (index, subindex, abort); set_state_idle (MBXout, index, subindex, abort);
return;
} }
} }
else else
@ -393,7 +399,8 @@ static void SDO_upload (void)
} }
else else
{ {
SDO_abort (index, subindex, abort); set_state_idle (MBXout, index, subindex, abort);
return;
} }
} }
if ((abort == 0) && (ESCvar.segmented == 0)) if ((abort == 0) && (ESCvar.segmented == 0))
@ -402,7 +409,8 @@ static void SDO_upload (void)
(objd + nsub)->flags); (objd + nsub)->flags);
if (abort != 0) if (abort != 0)
{ {
SDO_abort (index, subindex, abort); set_state_idle (MBXout, index, subindex, abort);
return;
} }
} }
MBXcontrol[MBXout].state = MBXstate_outreq; MBXcontrol[MBXout].state = MBXstate_outreq;
@ -410,12 +418,12 @@ static void SDO_upload (void)
} }
else else
{ {
SDO_abort (index, subindex, ABORT_NOSUBINDEX); SDO_abort (0, index, subindex, ABORT_NOSUBINDEX);
} }
} }
else else
{ {
SDO_abort (index, subindex, ABORT_NOOBJECT); SDO_abort (0, index, subindex, ABORT_NOOBJECT);
} }
MBXcontrol[0].state = MBXstate_idle; MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0; ESCvar.xoe = 0;
@ -512,6 +520,10 @@ static uint32_t complete_access_subindex_loop(const _objd *objd,
uint8_t bitmask = (1 << bitlen) - 1; uint8_t bitmask = (1 << bitlen) - 1;
if (READ_ACCESS(access, state)) if (READ_ACCESS(access, state))
{ {
if (bitoffset == 0)
{
mbxdata[BITS2BYTES(size)] = 0;
}
mbxdata[BITS2BYTES(size)] |= mbxdata[BITS2BYTES(size)] |=
(*(uint8_t *)ul_source & bitmask) << bitoffset; (*(uint8_t *)ul_source & bitmask) << bitoffset;
} }
@ -563,7 +575,7 @@ static void SDO_upload_complete_access (void)
(coesdo, &index, &subindex, &nidx, &nsub); (coesdo, &index, &subindex, &nidx, &nsub);
if (abortcode != 0) if (abortcode != 0)
{ {
set_state_idle (index, subindex, abortcode); set_state_idle (0, index, subindex, abortcode);
return; 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, /* It is a bad idea to call SDO_abort when ESC_claimbuffer fails,
* because SDO_abort will also call ESC_claimbuffer ... * because SDO_abort will also call ESC_claimbuffer ...
*/ */
set_state_idle (index, subindex, 0); set_state_idle (0, index, subindex, 0);
return; return;
} }
@ -581,17 +593,17 @@ static void SDO_upload_complete_access (void)
/* loop through the subindexes to get the total size */ /* loop through the subindexes to get the total size */
uint32_t size = complete_access_subindex_loop(objd, nidx, nsub, NULL, UPLOAD, 0); 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 */ /* 'size' is in this case actually an abort code */
set_state_idle (index, subindex, size); set_state_idle (MBXout, index, subindex, size);
return; return;
} }
/* check that upload data fits in the preallocated buffer */ /* check that upload data fits in the preallocated buffer */
if ((size + PREALLOC_FACTOR * COE_HEADERSIZE) > PREALLOC_BUFFER_SIZE) 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; return;
} }
@ -605,7 +617,8 @@ static void SDO_upload_complete_access (void)
objd->data, (size_t *)&size, objd->flags | COMPLETE_ACCESS_FLAG); objd->data, (size_t *)&size, objd->flags | COMPLETE_ACCESS_FLAG);
if (abortcode != 0) if (abortcode != 0)
{ {
SDO_abort (index, subindex, abortcode); set_state_idle (MBXout, index, subindex, abortcode);
return;
} }
/* copy subindex data into the preallocated buffer */ /* copy subindex data into the preallocated buffer */
@ -655,13 +668,14 @@ static void SDO_upload_complete_access (void)
if (abortcode != 0) if (abortcode != 0)
{ {
SDO_abort (index, subindex, abortcode); set_state_idle (MBXout, index, subindex, abortcode);
return;
} }
} }
MBXcontrol[MBXout].state = MBXstate_outreq; 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 /** Function for handling the following SDO Upload if previous SDOUpload
@ -718,7 +732,8 @@ static void SDO_uploadsegment (void)
coesdo->subindex, ESCvar.flags); coesdo->subindex, ESCvar.flags);
if (abort != 0) 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_DINT) &&
(type != DTYPE_ARRAY_OF_UDINT)) (type != DTYPE_ARRAY_OF_UDINT))
{ {
set_state_idle (index, subindex, ABORT_TYPEMISMATCH); set_state_idle (0, index, subindex, ABORT_TYPEMISMATCH);
return; return;
} }
} }
@ -839,35 +854,35 @@ static void SDO_download (void)
abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags); abort = ESC_download_post_objecthandler (index, subindex, (objd + nsub)->flags);
if (abort != 0) if (abort != 0)
{ {
SDO_abort (index, subindex, abort); SDO_abort (MBXout, index, subindex, abort);
} }
} }
} }
else else
{ {
SDO_abort (index, subindex, abort); SDO_abort (0, index, subindex, abort);
} }
} }
else else
{ {
if (access == ATYPE_RWpre) if (access == ATYPE_RWpre)
{ {
SDO_abort (index, subindex, ABORT_NOTINTHISSTATE); SDO_abort (0, index, subindex, ABORT_NOTINTHISSTATE);
} }
else else
{ {
SDO_abort (index, subindex, ABORT_READONLY); SDO_abort (0, index, subindex, ABORT_READONLY);
} }
} }
} }
else else
{ {
SDO_abort (index, subindex, ABORT_NOSUBINDEX); SDO_abort (0, index, subindex, ABORT_NOSUBINDEX);
} }
} }
else else
{ {
SDO_abort (index, subindex, ABORT_NOOBJECT); SDO_abort (0, index, subindex, ABORT_NOOBJECT);
} }
MBXcontrol[0].state = MBXstate_idle; MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0; ESCvar.xoe = 0;
@ -887,7 +902,7 @@ static void SDO_download_complete_access (void)
(coesdo, &index, &subindex, &nidx, &nsub); (coesdo, &index, &subindex, &nidx, &nsub);
if (abortcode != 0) if (abortcode != 0)
{ {
set_state_idle (index, subindex, abortcode); set_state_idle (0, index, subindex, abortcode);
return; return;
} }
@ -910,10 +925,10 @@ static void SDO_download_complete_access (void)
/* loop through the subindexes to get the total size */ /* loop through the subindexes to get the total size */
uint32_t size = complete_access_subindex_loop(objd, nidx, nsub, NULL, DOWNLOAD, 0); 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 */ /* 'size' is in this case actually an abort code */
set_state_idle (index, subindex, size); set_state_idle (0, index, subindex, size);
return; return;
} }
/* The document ETG.1020 S (R) V1.3.0, chapter 12.2, states that /* 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); size, objd->flags | COMPLETE_ACCESS_FLAG);
if (abortcode != 0) if (abortcode != 0)
{ {
set_state_idle (index, subindex, abortcode); set_state_idle (0, index, subindex, abortcode);
return; return;
} }
@ -939,13 +954,13 @@ static void SDO_download_complete_access (void)
objd->flags | COMPLETE_ACCESS_FLAG); objd->flags | COMPLETE_ACCESS_FLAG);
if (abortcode != 0) if (abortcode != 0)
{ {
set_state_idle (index, subindex, abortcode); set_state_idle (0, index, subindex, abortcode);
return; return;
} }
} }
else else
{ {
set_state_idle (index, subindex, ABORT_TYPEMISMATCH); set_state_idle (0, index, subindex, ABORT_TYPEMISMATCH);
return; return;
} }
@ -961,7 +976,7 @@ static void SDO_download_complete_access (void)
MBXcontrol[MBXout].state = MBXstate_outreq; MBXcontrol[MBXout].state = MBXstate_outreq;
} }
set_state_idle (index, subindex, 0); set_state_idle (MBXout, index, subindex, 0);
} }
static void SDO_downloadsegment (void) static void SDO_downloadsegment (void)
@ -993,7 +1008,7 @@ static void SDO_downloadsegment (void)
(ESCvar.index, ESCvar.subindex, ESCvar.flags); (ESCvar.index, ESCvar.subindex, ESCvar.flags);
if (abort != 0) if (abort != 0)
{ {
set_state_idle (ESCvar.index, ESCvar.subindex, abort); set_state_idle (MBXout, ESCvar.index, ESCvar.subindex, abort);
return; return;
} }
} }
@ -1006,7 +1021,7 @@ static void SDO_downloadsegment (void)
MBXcontrol[MBXout].state = MBXstate_outreq; 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. /** Function for sending an SDO Info Error reply.
@ -1435,7 +1450,7 @@ void ESC_coeprocess (void)
} }
else 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; MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0; ESCvar.xoe = 0;

1
soes/esc_coe.h
View File

@ -111,7 +111,6 @@ void ESC_coeprocess (void);
int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex); int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex);
int32_t SDO_findobject (uint16_t index); int32_t SDO_findobject (uint16_t index);
uint16_t sizeOfPDO (uint16_t index, int * nmappings, _SMmap * sm, int max_mappings); 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_initDefaultValues (void);
void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * sm); void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * sm);