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

Handle SDO entries with flexible length 

Change-Id: I1d2e1549c497aaef801b1ea42342843e5ff3e73d
pull/83/head
Lars Danielsson 2020-08-17 10:48:27 +02:00
parent e428008ea3
commit cde698b1ea
1 changed files with 56 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
soes/esc_coe.c
View File

@ -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