UNS8 get_next_DCF_data(CO_Data* d, dcf_entry_t *dcf_entry, UNS8 nodeId)
{
UNS8* dcfend;
UNS32 nb_entries;
UNS32 szData;
UNS8* dcf;
if(!d->dcf_odentry)
return 0;
if(nodeId > d->dcf_odentry->bSubCount)
return 0;
szData = d->dcf_odentry->pSubindex[nodeId].size;
dcf = *(UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject;
nb_entries = UNS32_LE(*((UNS32*)dcf));
dcfend = dcf + szData;
if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries){
/* DCF data may not be 32/16b aligned,
* we cannot directly dereference d->dcf_cursor
* as UNS16 or UNS32
* Do it byte per byte taking care on endianess*/
#ifdef CANOPEN_BIG_ENDIAN
dcf_entry->Index = *(d->dcf_cursor++) << 8 |
*(d->dcf_cursor++);
#else
memcpy(&dcf_entry->Index, d->dcf_cursor,2);
d->dcf_cursor+=2;
#endif
dcf_entry->Subindex = *(d->dcf_cursor++);
#ifdef CANOPEN_BIG_ENDIAN
dcf_entry->Size = *(d->dcf_cursor++) << 24 |
*(d->dcf_cursor++) << 16 |
*(d->dcf_cursor++) << 8 |
*(d->dcf_cursor++);
#else
memcpy(&dcf_entry->Size, d->dcf_cursor,4);
d->dcf_cursor+=4;
#endif
d->dcf_data = dcf_entry->Data = d->dcf_cursor;
d->dcf_size = dcf_entry->Size;
d->dcf_cursor += dcf_entry->Size;
d->dcf_entries_count++;
return 1;
}
return 0;
}
static void send_consise_dcf_loop(CO_Data* d,UNS8 nodeId)
{
if(nodeId > d->dcf_odentry->bSubCount) return;
/* Loop on all DCF subindexes, corresponding to node ID until there is no request*/
//while (nodeId < d->dcf_odentry->bSubCount){
while (d->dcf_request>0) {
if(d->dcf_odentry->pSubindex[nodeId].bAccessType & DCF_TO_SEND) {
UNS8* dcfend;
UNS32 nb_entries;
UNS32 szData = d->dcf_odentry->pSubindex[nodeId].size;
{
UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject);
dcfend = dcf + szData;
if (!d->dcf_cursor) {
d->dcf_cursor = (UNS8*)dcf + 4;
d->dcf_entries_count = 0;
}
nb_entries = UNS32_LE(*((UNS32*)dcf));
}
/* condition on consise DCF string for NodeID, if big enough */
if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries) {
UNS16 target_Index;
UNS8 target_Subindex;
UNS32 target_Size;
/* DCF data may not be 32/16b aligned,
* we cannot directly dereference d->dcf_cursor
* as UNS16 or UNS32
* Do it byte per byte taking care on endianess*/
#ifdef CANOPEN_BIG_ENDIAN
target_Index = *(d->dcf_cursor++) << 8 |
*(d->dcf_cursor++);
#else
memcpy(&target_Index, d->dcf_cursor,2);
d->dcf_cursor+=2;
#endif
target_Subindex = *(d->dcf_cursor++);
#ifdef CANOPEN_BIG_ENDIAN
target_Size = *(d->dcf_cursor++) << 24 |
*(d->dcf_cursor++) << 16 |
*(d->dcf_cursor++) << 8 |
*(d->dcf_cursor++);
#else
memcpy(&target_Size, d->dcf_cursor,4);
d->dcf_cursor+=4;
#endif
_writeNetworkDict(d, /* CO_Data* d*/
nodeId, /* UNS8 nodeId*/
target_Index, /* UNS16 index*/
target_Subindex, /* UNS8 subindex*/
(UNS8)target_Size, /* UNS8 count*/
0, /* UNS8 dataType*/
d->dcf_cursor,/* void *data*/
CheckSDOAndContinue,/* SDOCallback_t
Callback*/
0); /* no endianize*/
/* Push d->dcf_cursor to the end of data*/
d->dcf_cursor += target_Size;
d->dcf_entries_count++;
/* send_consise_dcf_loop will be called by CheckSDOAndContinue for next DCF entry*/
return;
}
else
{
/* We have finished with the dcf entry. Change the flag, decrement the request
* and execute the bootup callback. */
d->dcf_odentry->pSubindex[nodeId].bAccessType&=~DCF_TO_SEND;
d->dcf_request--;
(*d->post_SlaveBootup)(d, nodeId);
}
}
SEEK_NEXT_DCF()
}
}
