Files
chrg/apps/chrg/applications/mb/mbutils.c
T
2025-09-14 23:17:13 +08:00

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;
}