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

Add fix for bitpos2byteoffset 

Can't use BITS2BYTE when calculating true bit offset
in a byte. Instead add a new macro that don't
round up to full byte size.

fixes #139
pull/140/head
Andreas Karlsson 2022-11-26 13:09:28 +01:00
parent a7a017c26f
commit fff8e22603
1 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
soes/esc_coe.c
View File

@ -19,6 +19,7 @@
#include "esc_coe.h" #include "esc_coe.h"
#define BITS2BYTES(b) ((b + 7) >> 3) #define BITS2BYTES(b) ((b + 7) >> 3)
#define BITSPOS2BYTESOFFSET(b) (b >> 3)
/* Fetch value from object dictionary */ /* Fetch value from object dictionary */
#define OBJ_VALUE_FETCH(v, o) \ #define OBJ_VALUE_FETCH(v, o) \
@ -474,6 +475,12 @@ static uint32_t complete_access_subindex_loop(const _objd *objd,
uint32_t size = 0; uint32_t size = 0;
/* Clear padded mbxdata byte [1] on upload */
if ((load_type == UPLOAD) && (mbxdata != NULL))
{
mbxdata[1] = 0;
}
while (nsub <= SDOobjects[nidx].maxsub) while (nsub <= SDOobjects[nidx].maxsub)
{ {
uint16_t bitlen = (objd + nsub)->bitlength; uint16_t bitlen = (objd + nsub)->bitlength;
@ -522,14 +529,14 @@ static uint32_t complete_access_subindex_loop(const _objd *objd,
{ {
if (bitoffset == 0) if (bitoffset == 0)
{ {
mbxdata[BITS2BYTES(size)] = 0; mbxdata[BITSPOS2BYTESOFFSET(size)] = 0;
} }
mbxdata[BITS2BYTES(size)] |= mbxdata[BITSPOS2BYTESOFFSET(size)] |=
(*(uint8_t *)ul_source & bitmask) << bitoffset; (*(uint8_t *)ul_source & bitmask) << bitoffset;
} }
else else
{ {
mbxdata[BITS2BYTES(size)] &= ~(bitmask << bitoffset); mbxdata[BITSPOS2BYTESOFFSET(size)] &= ~(bitmask << bitoffset);
} }
} }
@ -1768,7 +1775,7 @@ void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * mappings)
if (obj->bitlength > 64) if (obj->bitlength > 64)
{ {
memcpy ( memcpy (
&buffer[BITS2BYTES (offset)], &buffer[BITSPOS2BYTESOFFSET (offset)],
obj->data, obj->data,
BITS2BYTES (obj->bitlength) BITS2BYTES (obj->bitlength)
); );
@ -1816,7 +1823,7 @@ void COE_pdoUnpack (uint8_t * buffer, int nmappings, _SMmap * mappings)
{ {
memcpy ( memcpy (
obj->data, obj->data,
&buffer[BITS2BYTES (offset)], &buffer[BITSPOS2BYTESOFFSET (offset)],
BITS2BYTES (obj->bitlength) BITS2BYTES (obj->bitlength)
); );
} }