/**
|
******************************************************************************
|
* @file : ModbusRTU.c
|
* @brief : Modbus RTU Protocol program body
|
******************************************************************************
|
*/
|
#include "ModbusRTU.h"
|
#include "functions.h"
|
#include "string.h"
|
#include "Kmachine.h"
|
|
static inline unsigned char HiByte(uint16_t in)
|
{
|
return (in>>8)&0xff;
|
}
|
|
static inline unsigned char LoByte(uint16_t in)
|
{
|
return (in&0xff);
|
}
|
static inline uint16_t swaphilo(uint16_t in)
|
{
|
return ((in&0xff)<<8)|((in>>8)&0xff);
|
}
|
|
static inline uint16_t mkshort(uint8_t hi, uint8_t lo)
|
{
|
return ((uint16_t)hi<<8)|lo;
|
}
|
|
uint8_t MyAddr = 1;
|
int ModBusCRC16(void * pBuf, int len1)
|
{
|
return 0;
|
}
|
|
//const uint16_t polynom = 0xA001;
|
/*
|
uint16_t crc16bitbybit(uint8_t *ptr, uint16_t len)
|
{
|
uint8_t i;
|
uint16_t crc = 0xffff;
|
|
if (len == 0) {
|
len = 1;
|
}
|
while (len--) {
|
crc ^= *ptr;
|
for (i = 0; i<8; i++)
|
{
|
if (crc & 1) {
|
crc >>= 1;
|
crc ^= polynom;
|
}
|
else {
|
crc >>= 1;
|
}
|
}
|
ptr++;
|
}
|
return(crc);
|
}
|
*/
|
const uint16_t crctalbeabs[] = {
|
0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401,
|
0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400
|
};
|
|
uint16_t crc16tablefast(const uint8_t *ptr, uint16_t len)
|
{
|
uint16_t crc = 0xffff;
|
uint16_t i;
|
uint8_t ch;
|
|
for (i = 0; i < len; i++) {
|
ch = *ptr++;
|
crc = crctalbeabs[(ch ^ crc) & 15] ^ (crc >> 4);
|
crc = crctalbeabs[((ch >> 4) ^ crc) & 15] ^ (crc >> 4);
|
}
|
|
return crc;
|
}
|
|
//**********************************************************************
|
//****************** Modbus slave ************************************
|
|
int ModBusSlaveCheckPkg(int nChn, void *ptr, uint16_t len1)
|
{
|
if (len1 <=4) return -1; //包长
|
pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr;
|
if (pPkg->Dst >127) return -2; //地址码
|
if ((pPkg->Cmd&0x7f) > 0x1f) return -3; //功能码
|
uint16_t crc = crc16tablefast(ptr,len1); //CRC 校验
|
if (crc != 0 ) return 4; //CRC 校验错误
|
return S_OK;
|
}
|
|
int mkReqPkg(uint8_t *ptr, uint8_t opcode, uint8_t nsize, uint8_t *buf)
|
{
|
int len1=0;
|
|
return len1;
|
|
}
|
/*
|
static const unsigned short bitMasks[16]=
|
{
|
0x1<<0, 0x1<<1, 0x1<<2, 0x1<<3, 0x1<<4, 0x1<<5, 0x1<<6, 0x1<<7,
|
0x1<<8, 0x1<<9, 0x1<<10, 0x1<<11, 0x1<<12, 0x1<<13, 0x1<<14, 0x1<<15,
|
};
|
*/
|
static inline void SetAddrBit(unsigned short * pW, unsigned char bitAddr)
|
{
|
(*pW)|=1<<(bitAddr&0xf);
|
}
|
|
static inline void ResetBit(unsigned short * pW, unsigned char bitAddr)
|
{
|
(*pW)&=~(1<<(bitAddr&0xf));
|
}
|
|
static inline void SetBitValue(unsigned short * pW, unsigned char bitAddr, unsigned char Value)
|
{
|
if (Value) { SetAddrBit(pW, bitAddr);}
|
else {ResetBit(pW, bitAddr);}
|
}
|
|
static inline unsigned char GetBitValue(unsigned short W, unsigned char bitAddr)
|
{
|
if (W&(1<<(bitAddr&0xf))) return 1;
|
else return 0;
|
}
|
|
uint8_t Pkgbuf[128];
|
|
int ModBusSlaveParsePkg(int nChn, void *ptr, uint16_t len)
|
{
|
uchar * bptr = (uchar *) ptr;
|
int err=ModBusSlaveCheckPkg(nChn, ptr, len);
|
if (err) return -1;
|
pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr;
|
uint8_t DstHost=pPkg->Dst;
|
if (DstHost != MyAddr) return 0;
|
uint8_t opcode = pPkg->Cmd;
|
ushort Addr=mkshort(pPkg->AddrH,pPkg->AddrL);
|
ushort nCount=swaphilo(pPkg->nCount);
|
Pkgbuf[0]=MyAddr;
|
Pkgbuf[1]=opcode;
|
int len2=2;
|
int needcrc=1;
|
uint16_t crc;
|
int nByteCount;
|
unsigned short Addr1=(Addr&0xff0)>>4;
|
unsigned char bitAddr=Addr&0xf;
|
unsigned char thisValue;
|
unsigned char bitmask=1;
|
switch (opcode)
|
{
|
case None: //0
|
break;
|
case ReadCoils: //1 bits
|
// Fetch Datas;
|
nByteCount=(nCount+7)/8;
|
Pkgbuf[2]=nByteCount;
|
for (int i=0;i<nByteCount;i++)
|
{
|
thisValue=0;
|
bitmask=1;
|
for (int j=0;j<8&&i*8+j<nCount;j++)
|
{
|
Addr1=(Addr&0xff0)>>4;
|
bitAddr=Addr&0xf;
|
if (GetBitValue( KMem.WY[Addr1],bitAddr))
|
{
|
thisValue|=bitmask;
|
}
|
bitmask<<=1;
|
Addr++;
|
}
|
Pkgbuf[3+i]=thisValue;
|
}
|
len2+=1+nByteCount;
|
break;
|
case ReadInputs: //2 , bits
|
// Fetch Datas;
|
nByteCount=(nCount+7)/8;
|
Pkgbuf[2]=nByteCount;
|
for (int i=0;i<nByteCount;i++)
|
{
|
thisValue=0;
|
bitmask=1;
|
for (int j=0;j<8&&i*8+j<nCount;j++)
|
{
|
Addr1=(Addr&0xff0)>>4;
|
bitAddr=Addr&0xf;
|
if (GetBitValue( KMem.WX[Addr1],bitAddr))
|
{
|
thisValue|=bitmask;
|
}
|
bitmask<<=1;
|
Addr++;
|
}
|
Pkgbuf[3+i]=thisValue;
|
}
|
len2+=1+nByteCount;
|
break;
|
case ReadKeepRegs: //3 words
|
// Fetch Datas;
|
nByteCount=nCount*2;
|
Pkgbuf[2]=nByteCount;
|
for (int i=0;i<nCount;i++)
|
{
|
Pkgbuf[3+i*2]=(KMem.DT[Addr+i]>>8)&0xff;
|
Pkgbuf[4+i*2]=(KMem.DT[Addr+i])&0xff;
|
}
|
len2+=1+nByteCount;
|
break;
|
case ReadInputRegs: //4 words
|
// Fetch Datas;
|
nByteCount=nCount*2;
|
Pkgbuf[2]=nByteCount;
|
for (int i=0;i<nCount;i++)
|
{
|
Pkgbuf[3+i*2]=HiByte(KMem.WX[Addr+i]);
|
Pkgbuf[4+i*2]=LoByte(KMem.WX[Addr+i]);
|
}
|
len2+=1+nByteCount;
|
break;
|
case WriteCoil: //5 bit
|
//Store Datas;
|
//SetBitValue(&KMem.WR[Addr1],bitAddr,pPkg->nCount);
|
if (nCount == 0) // set to 0
|
{
|
ResetBit(&KMem.WY[Addr1],bitAddr);
|
}else if (nCount == 0xFF00) // set to 1
|
{
|
SetAddrBit(&KMem.WY[Addr1],bitAddr);
|
}else
|
{
|
//error
|
}
|
memcpy(Pkgbuf,ptr,len);
|
needcrc=0;
|
len2=len;
|
break;
|
case WriteReg: //6 word
|
//Store Datas;
|
KMem.DT[Addr]=mkshort(bptr[4],bptr[5]);
|
memcpy(Pkgbuf,ptr,len);
|
needcrc=0;
|
len2=len;
|
break;
|
case WriteCoils: //0x0f, 15 bits
|
//Store Datas;
|
nByteCount=(nCount+7)/8;
|
Pkgbuf[2]=nByteCount;
|
for (int i=0;i<nByteCount;i++)
|
{
|
thisValue=bptr[7+i];
|
bitmask=1;
|
for (int j=0;j<8&&i*8+j<nCount;j++)
|
{
|
Addr1=(Addr&0xff0)>>4;
|
bitAddr=Addr&0xf;
|
|
SetBitValue( &KMem.WY[Addr1],bitAddr,bitmask&thisValue);
|
bitmask<<=1;
|
Addr++;
|
}
|
}
|
|
//makeReply
|
Pkgbuf[2]=bptr[2]; //HiByte(Addr);
|
Pkgbuf[3]=bptr[3]; //LoByte(Addr);
|
Pkgbuf[4]=bptr[4]; //HiByte(nCount);
|
Pkgbuf[5]=bptr[5]; //LoByte(nCount);
|
len2+=4;
|
break;
|
case WriteRegs: //0x10, 16 words
|
//Store Datas;
|
for (int i=0;i<nCount;i++)
|
{
|
KMem.DT[Addr+i]=mkshort(bptr[7+i*2],bptr[8+i*2]);
|
}
|
//Make Reply
|
Pkgbuf[2]=bptr[2]; //HiByte(Addr);
|
Pkgbuf[3]=bptr[3]; //LoByte(Addr);
|
Pkgbuf[4]=bptr[4]; //HiByte(nCount);
|
Pkgbuf[5]=bptr[5]; //LoByte(nCount);
|
len2+=4;
|
break;
|
|
default: //others
|
break;
|
}
|
if (err==0)
|
{
|
if (needcrc)
|
{
|
crc=crc16tablefast(Pkgbuf,len2);
|
Pkgbuf[len2]=LoByte(crc);
|
Pkgbuf[len2+1]=HiByte(crc);
|
len2+=2;
|
}
|
//SendPkg(Pkgbuf,len2);
|
SendPacket(nChn, Pkgbuf, len2);
|
}
|
|
return 0;
|
}
|
