107 lines
2.8 KiB
C
107 lines
2.8 KiB
C
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include "mbport.h"
|
|
|
|
#include "mb.h"
|
|
|
|
#define BITS_UCHAR 8U
|
|
|
|
void xMBUtilSetBits (rt_uint8_t * ucByteBuf, rt_uint16_t usBitOffset,
|
|
rt_uint8_t ucNBits, rt_uint8_t ucValue)
|
|
{
|
|
rt_uint16_t usWordBuf;
|
|
rt_uint16_t usMask;
|
|
rt_uint16_t usByteOffset;
|
|
rt_uint16_t usNPreBits;
|
|
rt_uint16_t usValue = ucValue;
|
|
|
|
RT_ASSERT(ucNBits <= 8);
|
|
RT_ASSERT((size_t)BITS_UCHAR == sizeof(rt_uint8_t) * 8);
|
|
|
|
/* Calculate byte offset for first byte containing the bit values starting
|
|
* at usBitOffset. */
|
|
usByteOffset = (rt_uint16_t)((usBitOffset) / BITS_UCHAR);
|
|
|
|
/* How many bits precede our bits to set. */
|
|
usNPreBits = (rt_uint16_t)(usBitOffset - usByteOffset * BITS_UCHAR);
|
|
|
|
/* Move bit field into position over bits to set */
|
|
usValue <<= usNPreBits;
|
|
|
|
/* Prepare a mask for setting the new bits. */
|
|
usMask = (rt_uint16_t)((1 << (rt_uint16_t)ucNBits) - 1);
|
|
usMask <<= usBitOffset - usByteOffset * BITS_UCHAR;
|
|
|
|
/* copy bits into temporary storage. */
|
|
usWordBuf = ucByteBuf[usByteOffset];
|
|
usWordBuf |= ucByteBuf[usByteOffset + 1] << BITS_UCHAR;
|
|
|
|
/* Zero out bit field bits and then or value bits into them. */
|
|
usWordBuf = (rt_uint16_t)((usWordBuf & (~usMask)) | usValue);
|
|
|
|
/* move bits back into storage */
|
|
ucByteBuf[usByteOffset] = (rt_uint8_t)(usWordBuf & 0xFF);
|
|
ucByteBuf[usByteOffset + 1] = (rt_uint8_t)(usWordBuf >> BITS_UCHAR);
|
|
}
|
|
|
|
rt_uint8_t xMBUtilGetBits (rt_uint8_t * ucByteBuf, rt_uint16_t usBitOffset, rt_uint8_t ucNBits)
|
|
{
|
|
rt_uint16_t usWordBuf;
|
|
rt_uint16_t usMask;
|
|
rt_uint16_t usByteOffset;
|
|
rt_uint16_t usNPreBits;
|
|
|
|
/* Calculate byte offset for first byte containing the bit values starting
|
|
* at usBitOffset. */
|
|
usByteOffset = (rt_uint16_t)((usBitOffset) / BITS_UCHAR);
|
|
|
|
/* How many bits precede our bits to set. */
|
|
usNPreBits = (rt_uint16_t)(usBitOffset - usByteOffset * BITS_UCHAR);
|
|
|
|
/* Prepare a mask for setting the new bits. */
|
|
usMask = (rt_uint16_t)((1 << (rt_uint16_t)ucNBits) - 1);
|
|
|
|
/* copy bits into temporary storage. */
|
|
usWordBuf = ucByteBuf[usByteOffset];
|
|
usWordBuf |= ucByteBuf[usByteOffset + 1] << BITS_UCHAR;
|
|
|
|
/* throw away unneeded bits. */
|
|
usWordBuf >>= usNPreBits;
|
|
|
|
/* mask away bits above the requested bitfield. */
|
|
usWordBuf &= usMask;
|
|
|
|
return (rt_uint8_t)usWordBuf;
|
|
}
|
|
|
|
eMBException prveMBError2Exception (eMBErrorCode eErrorCode)
|
|
{
|
|
eMBException eStatus;
|
|
|
|
switch (eErrorCode) {
|
|
case MB_ENOERR:
|
|
eStatus = MB_EX_NONE;
|
|
break;
|
|
|
|
case MB_ENOREG:
|
|
eStatus = MB_EX_ILLEGAL_DATA_ADDRESS;
|
|
break;
|
|
|
|
case MB_ETIMEDOUT:
|
|
eStatus = MB_EX_SLAVE_BUSY;
|
|
break;
|
|
|
|
default:
|
|
eStatus = MB_EX_SLAVE_DEVICE_FAILURE;
|
|
break;
|
|
}
|
|
|
|
return eStatus;
|
|
}
|
|
|
|
|
|
|
|
|