F030C8xx_KLink - Gitblit

QuakeGod

2022-10-17 7a54c0d5730cb2d02229c0ce515bc100bf211079

提交 \| 用户 \| age
e1f350	1	/**
Q	2	******************************************************************************
	3	* @file : ModbusRTU.c
	4	* @brief : Modbus RTU Protocol program body
	5	******************************************************************************
	6	*/
	7	#include "ModbusRTU.h"
	8	#include "functions.h"
	9	#include "string.h"
	10	#include "Kmachine.h"
	11
	12	static inline unsigned char HiByte(uint16_t in)
	13	{
	14	return (in>>8)&0xff;
	15	}
	16
	17	static inline unsigned char LoByte(uint16_t in)
	18	{
	19	return (in&0xff);
	20	}
	21	static inline uint16_t swaphilo(uint16_t in)
	22	{
	23	return ((in&0xff)<<8)\|((in>>8)&0xff);
	24	}
	25
	26	static inline uint16_t mkshort(uint8_t hi, uint8_t lo)
	27	{
	28	return ((uint16_t)hi<<8)\|lo;
	29	}
	30
	31	uint8_t MyAddr = 1;
	32	int ModBusCRC16(void * pBuf, int len1)
	33	{
	34	return 0;
	35	}
	36
	37	//const uint16_t polynom = 0xA001;
	38	/*
	39	uint16_t crc16bitbybit(uint8_t *ptr, uint16_t len)
	40	{
	41	uint8_t i;
	42	uint16_t crc = 0xffff;
	43
	44	if (len == 0) {
	45	len = 1;
	46	}
	47	while (len--) {
	48	crc ^= *ptr;
	49	for (i = 0; i<8; i++)
	50	{
	51	if (crc & 1) {
	52	crc >>= 1;
	53	crc ^= polynom;
	54	}
	55	else {
	56	crc >>= 1;
	57	}
	58	}
	59	ptr++;
	60	}
	61	return(crc);
	62	}
	63	*/
	64	const uint16_t crctalbeabs[] = {
	65	0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401,
	66	0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400
	67	};
	68
d69aae	69	uint16_t crc16tablefast(const uint8_t *ptr, uint16_t len)
e1f350	70	{
Q	71	uint16_t crc = 0xffff;
	72	uint16_t i;
	73	uint8_t ch;
	74
	75	for (i = 0; i < len; i++) {
	76	ch = *ptr++;
	77	crc = crctalbeabs[(ch ^ crc) & 15] ^ (crc >> 4);
	78	crc = crctalbeabs[((ch >> 4) ^ crc) & 15] ^ (crc >> 4);
	79	}
	80
	81	return crc;
	82	}
	83
	84	//**********************************************************************
	85	//**************** Modbus slave **********************************
	86
d69aae	87	int ModBusSlaveCheckPkg(int nChn, void *ptr, uint16_t len1)
e1f350	88	{
Q	89	if (len1 <=4) return -1; //包长
	90	pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr;
	91	if (pPkg->Dst >127) return -2; //地址码
	92	if ((pPkg->Cmd&0x7f) > 0x1f) return -3; //功能码
	93	uint16_t crc = crc16tablefast(ptr,len1); //CRC 校验
	94	if (crc != 0 ) return 4; //CRC 校验错误
	95	return S_OK;
	96	}
	97
	98	int mkReqPkg(uint8_t ptr, uint8_t opcode, uint8_t nsize, uint8_t buf)
	99	{
	100	int len1=0;
	101
	102	return len1;
	103
	104	}
	105	/*
	106	static const unsigned short bitMasks[16]=
	107	{
	108	0x1<<0, 0x1<<1, 0x1<<2, 0x1<<3, 0x1<<4, 0x1<<5, 0x1<<6, 0x1<<7,
	109	0x1<<8, 0x1<<9, 0x1<<10, 0x1<<11, 0x1<<12, 0x1<<13, 0x1<<14, 0x1<<15,
	110	};
	111	*/
	112	static inline void SetAddrBit(unsigned short * pW, unsigned char bitAddr)
	113	{
	114	(*pW)\|=1<<(bitAddr&0xf);
	115	}
	116
	117	static inline void ResetBit(unsigned short * pW, unsigned char bitAddr)
	118	{
	119	(*pW)&=~(1<<(bitAddr&0xf));
	120	}
	121
	122	static inline void SetBitValue(unsigned short * pW, unsigned char bitAddr, unsigned char Value)
	123	{
	124	if (Value) { SetAddrBit(pW, bitAddr);}
	125	else {ResetBit(pW, bitAddr);}
	126	}
	127
	128	static inline unsigned char GetBitValue(unsigned short W, unsigned char bitAddr)
	129	{
	130	if (W&(1<<(bitAddr&0xf))) return 1;
	131	else return 0;
	132	}
	133
	134	uint8_t Pkgbuf[128];
	135
d69aae	136	int ModBusSlaveParsePkg(int nChn, void *ptr, uint16_t len)
e1f350	137	{
Q	138	uchar * bptr = (uchar *) ptr;
d69aae	139	int err=ModBusSlaveCheckPkg(nChn, ptr, len);
e1f350	140	if (err) return -1;
Q	141	pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr;
	142	uint8_t DstHost=pPkg->Dst;
	143	if (DstHost != MyAddr) return 0;
	144	uint8_t opcode = pPkg->Cmd;
	145	ushort Addr=mkshort(pPkg->AddrH,pPkg->AddrL);
	146	ushort nCount=swaphilo(pPkg->nCount);
	147	Pkgbuf[0]=MyAddr;
	148	Pkgbuf[1]=opcode;
	149	int len2=2;
	150	int needcrc=1;
	151	uint16_t crc;
	152	int nByteCount;
	153	unsigned short Addr1=(Addr&0xff0)>>4;
	154	unsigned char bitAddr=Addr&0xf;
	155	unsigned char thisValue;
	156	unsigned char bitmask=1;
	157	switch (opcode)
	158	{
	159	case None: //0
	160	break;
	161	case ReadCoils: //1 bits
	162	// Fetch Datas;
	163	nByteCount=(nCount+7)/8;
	164	Pkgbuf[2]=nByteCount;
	165	for (int i=0;i<nByteCount;i++)
	166	{
	167	thisValue=0;
	168	bitmask=1;
	169	for (int j=0;j<8&&i*8+j<nCount;j++)
	170	{
	171	Addr1=(Addr&0xff0)>>4;
	172	bitAddr=Addr&0xf;
d69aae	173	if (GetBitValue( KMem.WY[Addr1],bitAddr))
e1f350	174	{
Q	175	thisValue\|=bitmask;
	176	}
	177	bitmask<<=1;
	178	Addr++;
	179	}
	180	Pkgbuf[3+i]=thisValue;
	181	}
	182	len2+=1+nByteCount;
	183	break;
d69aae	184	case ReadInputs: //2 , bits
e1f350	185	// Fetch Datas;
Q	186	nByteCount=(nCount+7)/8;
	187	Pkgbuf[2]=nByteCount;
	188	for (int i=0;i<nByteCount;i++)
	189	{
	190	thisValue=0;
	191	bitmask=1;
	192	for (int j=0;j<8&&i*8+j<nCount;j++)
	193	{
	194	Addr1=(Addr&0xff0)>>4;
	195	bitAddr=Addr&0xf;
	196	if (GetBitValue( KMem.WX[Addr1],bitAddr))
	197	{
	198	thisValue\|=bitmask;
	199	}
	200	bitmask<<=1;
	201	Addr++;
	202	}
	203	Pkgbuf[3+i]=thisValue;
	204	}
	205	len2+=1+nByteCount;
	206	break;
	207	case ReadKeepRegs: //3 words
	208	// Fetch Datas;
	209	nByteCount=nCount*2;
	210	Pkgbuf[2]=nByteCount;
	211	for (int i=0;i<nCount;i++)
	212	{
	213	Pkgbuf[3+i*2]=(KMem.DT[Addr+i]>>8)&0xff;
	214	Pkgbuf[4+i*2]=(KMem.DT[Addr+i])&0xff;
	215	}
	216	len2+=1+nByteCount;
	217	break;
	218	case ReadInputRegs: //4 words
	219	// Fetch Datas;
	220	nByteCount=nCount*2;
	221	Pkgbuf[2]=nByteCount;
	222	for (int i=0;i<nCount;i++)
	223	{
	224	Pkgbuf[3+i*2]=HiByte(KMem.WX[Addr+i]);
	225	Pkgbuf[4+i*2]=LoByte(KMem.WX[Addr+i]);
	226	}
	227	len2+=1+nByteCount;
	228	break;
	229	case WriteCoil: //5 bit
	230	//Store Datas;
	231	//SetBitValue(&KMem.WR[Addr1],bitAddr,pPkg->nCount);
d69aae	232	if (nCount == 0) // set to 0
e1f350	233	{
d69aae	234	ResetBit(&KMem.WY[Addr1],bitAddr);
Q	235	}else if (nCount == 0xFF00) // set to 1
e1f350	236	{
d69aae	237	SetAddrBit(&KMem.WY[Addr1],bitAddr);
e1f350	238	}else
Q	239	{
	240	//error
	241	}
	242	memcpy(Pkgbuf,ptr,len);
	243	needcrc=0;
	244	len2=len;
	245	break;
	246	case WriteReg: //6 word
	247	//Store Datas;
	248	KMem.DT[Addr]=mkshort(bptr[4],bptr[5]);
	249	memcpy(Pkgbuf,ptr,len);
	250	needcrc=0;
	251	len2=len;
	252	break;
	253	case WriteCoils: //0x0f, 15 bits
	254	//Store Datas;
	255	nByteCount=(nCount+7)/8;
	256	Pkgbuf[2]=nByteCount;
	257	for (int i=0;i<nByteCount;i++)
	258	{
	259	thisValue=bptr[7+i];
	260	bitmask=1;
	261	for (int j=0;j<8&&i*8+j<nCount;j++)
	262	{
	263	Addr1=(Addr&0xff0)>>4;
	264	bitAddr=Addr&0xf;
	265
d69aae	266	SetBitValue( &KMem.WY[Addr1],bitAddr,bitmask&thisValue);
e1f350	267	bitmask<<=1;
Q	268	Addr++;
	269	}
	270	}
	271
	272	//makeReply
	273	Pkgbuf[2]=bptr[2]; //HiByte(Addr);
	274	Pkgbuf[3]=bptr[3]; //LoByte(Addr);
	275	Pkgbuf[4]=bptr[4]; //HiByte(nCount);
	276	Pkgbuf[5]=bptr[5]; //LoByte(nCount);
	277	len2+=4;
	278	break;
	279	case WriteRegs: //0x10, 16 words
	280	//Store Datas;
	281	for (int i=0;i<nCount;i++)
	282	{
	283	KMem.DT[Addr+i]=mkshort(bptr[7+i2],bptr[8+i2]);
	284	}
	285	//Make Reply
	286	Pkgbuf[2]=bptr[2]; //HiByte(Addr);
	287	Pkgbuf[3]=bptr[3]; //LoByte(Addr);
	288	Pkgbuf[4]=bptr[4]; //HiByte(nCount);
	289	Pkgbuf[5]=bptr[5]; //LoByte(nCount);
	290	len2+=4;
	291	break;
	292
	293	default: //others
	294	break;
	295	}
	296	if (err==0)
	297	{
	298	if (needcrc)
	299	{
	300	crc=crc16tablefast(Pkgbuf,len2);
	301	Pkgbuf[len2]=LoByte(crc);
	302	Pkgbuf[len2+1]=HiByte(crc);
	303	len2+=2;
	304	}
	305	//SendPkg(Pkgbuf,len2);
d69aae	306	SendPacket(nChn, Pkgbuf, len2);
e1f350	307	}
Q	308
	309	return 0;
	310	}
	311
	312