/**
  ******************************************************************************
  * @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;
}