#include "ModbusRTUSlave.h"

uchar ModBus_Addr=1;//本从机地址

u8 *ModBus_RX_BUFF;//接收缓冲区
//u16 ModBus_RX_CNT=0;//接收数据长度
u8 *ModBus_TX_BUFF;//发送缓冲区
//u16 ModBus_TX_CNT=0;//发送数据长度


//Modbus寄存器和单片机寄存器的映射关系
u8 Modbus_InputIO[38]={0};//输入开关量寄存器
u8 Modbus_OutputIO[3]={0};//输出开关量寄存器
u16 Modbus_HoldReg[243]={0};//保持寄存器
bit HoldReg_Changed=0;
u8 HoldReg_BoardId=0xFF;
u8 HoldReg_Board_stationId=0xFF;
u8 HoldReg_Board_addr=0xFF;
u16 HoldReg_new_value;
void (*SendModbusData)(uchar len);
void Modbus_RTU_Init(u8* rxbuf,u8* txbuf,void (*f)(uchar len))
{
	ModBus_RX_BUFF = rxbuf;
	ModBus_TX_BUFF = txbuf;
	SendModbusData=f;
}
void Set_InputIO_Value(u16 index,bit value)
{
	u16 byteindex;
	u8 bitindex;
	byteindex = index / 8;
	bitindex = index % 8;
	if(value==1)
		Modbus_InputIO[byteindex] |= 1 << (bitindex);
	else
		Modbus_InputIO[byteindex] &= ~(1<< (bitindex));
}
u16 startRegAddr;
u16 RegNum;
u16 calCRC;
void Modbus_01_Solve(void)
{
        u16 ByteNum;
        u16 i;
        RegNum= (((u16)ModBus_RX_BUFF[4])<<8)|ModBus_RX_BUFF[5];//???????
        if((startRegAddr+RegNum)<=18)//?????+??????
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1];
                ByteNum=RegNum/8;//???
                if(RegNum%8) ByteNum+=1;//????????,????+1
                ModBus_TX_BUFF[2]=ByteNum;//?????????
                for(i=0;i<RegNum;i++)
                {
                        if(i%8==0) ModBus_TX_BUFF[3+i/8]=0x00;
                        ModBus_TX_BUFF[3+i/8]>>=1;//?????
                        ModBus_TX_BUFF[3+i/8]|=((Modbus_OutputIO[(startRegAddr+i)/8])<<(7-(startRegAddr+i)%8))&0x80;
                        if(i==RegNum-1)//?????????
                        {
                                if(RegNum%8) ModBus_TX_BUFF[3+i/8]>>=8-(RegNum%8);//????????????,???MSB??0
                        }
                }
                calCRC=crc16(ModBus_TX_BUFF,ByteNum+3);
                ModBus_TX_BUFF[ByteNum+3]=(calCRC>>8)&0xFF;
                ModBus_TX_BUFF[ByteNum+4]=calCRC&0xFF;
                SendModbusData(ByteNum+5);
        }
        else//?????+??????
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
                ModBus_TX_BUFF[2]=0x02; //???
                SendModbusData(3);
        }
}
void Modbus_02_Solve(void)
{
		u16 ByteNum;
		u16 i;
		RegNum= (((u16)ModBus_RX_BUFF[4])<<8)|ModBus_RX_BUFF[5];//???????
		if((startRegAddr+RegNum)<=304)//?????+??????
		{
				ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
				ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1];
				ByteNum=RegNum/8;//???
				if(RegNum%8) ByteNum+=1;//????????,????+1
				ModBus_TX_BUFF[2]=ByteNum;//?????????
				for(i=0;i<RegNum;i++)
				{
						if(i%8==0) ModBus_TX_BUFF[3+i/8]=0x00;
						ModBus_TX_BUFF[3+i/8]>>=1;//?????
						ModBus_TX_BUFF[3+i/8]|=(Modbus_InputIO[(startRegAddr+i)/8]<<(7-(startRegAddr+i)%8))&0x80;
						if(i==RegNum-1)//?????????
						{
								if(RegNum%8) ModBus_TX_BUFF[3+i/8]>>=8-(RegNum%8);//????????????,???MSB??0
						}
				}
				calCRC=crc16(ModBus_TX_BUFF,ByteNum+3);
				ModBus_TX_BUFF[ByteNum+3]=(calCRC>>8)&0xFF;
				ModBus_TX_BUFF[ByteNum+4]=calCRC&0xFF;
				SendModbusData(ByteNum+5);
		}
		else//?????+??????
		{
				ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
				ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
				ModBus_TX_BUFF[2]=0x02; //???
				SendModbusData(3);
		}
}
//读保持寄存器
void Modbus_03_Solve(void)
{
        u8 i;
        RegNum= (((u16)ModBus_RX_BUFF[4])<<8)|ModBus_RX_BUFF[5];
				if(RegNum>100){//长度过大，返回错误
						ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
						ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
						ModBus_TX_BUFF[2]=0x02; 
						SendModbusData(3);
						return;
				}
        if((startRegAddr+RegNum)<=243)
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1];
                ModBus_TX_BUFF[2]=RegNum*2;
                for(i=0;i<RegNum;i++)
                {
                        ModBus_TX_BUFF[3+i*2]=(Modbus_HoldReg[startRegAddr+i]>>8)&0xFF;//高位在前
                        ModBus_TX_BUFF[4+i*2]=Modbus_HoldReg[startRegAddr+i]&0xFF; //低位在后
                }
                calCRC=crc16(ModBus_TX_BUFF,RegNum*2+3);
                ModBus_TX_BUFF[RegNum*2+4]=calCRC&0xFF;
                ModBus_TX_BUFF[RegNum*2+3]=(calCRC>>8)&0xFF;
                SendModbusData(RegNum*2+5);
        }
        else
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
                ModBus_TX_BUFF[2]=0x02; 
                SendModbusData(3);
        }
}
//读输入寄存器
void Modbus_04_Solve(void)
{
        u8 i;
        RegNum= (((u16)ModBus_RX_BUFF[4])<<8)|ModBus_RX_BUFF[5];
        if((startRegAddr+RegNum)<=19)
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1];
                ModBus_TX_BUFF[2]=RegNum*2;
                for(i=0;i<RegNum;i++)
                {
                        ModBus_TX_BUFF[3+i*2]=Modbus_InputIO[(startRegAddr+i)*2+1];//高位在前
                        ModBus_TX_BUFF[4+i*2]=Modbus_InputIO[(startRegAddr+i)*2]; //低位在后
                }
                calCRC=crc16(ModBus_TX_BUFF,RegNum*2+3);
                ModBus_TX_BUFF[RegNum*2+4]=calCRC&0xFF;
                ModBus_TX_BUFF[RegNum*2+3]=(calCRC>>8)&0xFF;
                SendModbusData(RegNum*2+5);
        }
        else
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
                ModBus_TX_BUFF[2]=0x02; 
                SendModbusData(3);
        }
}
//写单个输出线圈
void Modbus_05_Solve(void)
{
        if(startRegAddr<18)//?????????
        {
                if((ModBus_RX_BUFF[4]==0xFF)||(ModBus_RX_BUFF[5]==0xFF)) Modbus_OutputIO[startRegAddr/8] |= 0x01<<(startRegAddr%8);
                else Modbus_OutputIO[startRegAddr/8] &= ~(0x01<<(startRegAddr%8));
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1];
                ModBus_TX_BUFF[2]=ModBus_RX_BUFF[2];
                ModBus_TX_BUFF[3]=ModBus_RX_BUFF[3];
                ModBus_TX_BUFF[4]=ModBus_RX_BUFF[4];
                ModBus_TX_BUFF[5]=ModBus_RX_BUFF[5];
               
                calCRC=crc16(ModBus_TX_BUFF,6);
                ModBus_TX_BUFF[6]=(calCRC>>8)&0xFF;
                ModBus_TX_BUFF[7]=calCRC&0xFF;
                SendModbusData(8);
        }
        else//?????????
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
                ModBus_TX_BUFF[2]=0x02; //???
                SendModbusData(3);
        }
}
//写单个保持寄存器
void Modbus_06_Solve(void)
{
		if(startRegAddr<243)
		{
			HoldReg_new_value = ModBus_RX_BUFF[5];//?????
			HoldReg_new_value |= ((u16)ModBus_RX_BUFF[4])<<8;//?????
			
			ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
			ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1];
			ModBus_TX_BUFF[2]=ModBus_RX_BUFF[2];
			ModBus_TX_BUFF[3]=ModBus_RX_BUFF[3];
			ModBus_TX_BUFF[4]=ModBus_RX_BUFF[4];
			ModBus_TX_BUFF[5]=ModBus_RX_BUFF[5];
			
			calCRC=crc16(ModBus_TX_BUFF,6);
			ModBus_TX_BUFF[7]=calCRC&0xFF;
			ModBus_TX_BUFF[6]=(calCRC>>8)&0xFF;
			SendModbusData(8);
			HoldReg_Changed = 1;
			HoldReg_BoardId = startRegAddr / 81 +1;
			if((startRegAddr % 81)<11){
				HoldReg_Board_stationId = 1;
				HoldReg_Board_addr = startRegAddr % 81;
			}else{
				HoldReg_Board_stationId = ((startRegAddr % 81) - 11) / 14 + 2;
				HoldReg_Board_addr = ((startRegAddr % 81) - 11) % 14;
			}
			//HoldReg_Board_stationId = (startRegAddr % 81) / 14 +1;
			//HoldReg_Board_addr = (startRegAddr % 81) % 14;
		}
		else
		{
				ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
				ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
				ModBus_TX_BUFF[2]=0x02;
				SendModbusData(3);			
		}
}
//写多个保持寄存器，10H
void Modbus_16_Solve(void)
{
        u8 i;
        RegNum= (((u16)ModBus_RX_BUFF[4])<<8)|ModBus_RX_BUFF[5];//???????
        if((startRegAddr+RegNum)<=195)//?????+??????
        {
                for(i=0;i<RegNum;i++)
                {
                    Modbus_HoldReg[startRegAddr+i]=ModBus_RX_BUFF[8+i*2]; //?????
                    Modbus_HoldReg[startRegAddr+i]|=((u16)ModBus_RX_BUFF[7+i*2])<<8; //?????
                }
               
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1];
                ModBus_TX_BUFF[2]=ModBus_RX_BUFF[2];
                ModBus_TX_BUFF[3]=ModBus_RX_BUFF[3];
                ModBus_TX_BUFF[4]=ModBus_RX_BUFF[4];
                ModBus_TX_BUFF[5]=ModBus_RX_BUFF[5];
               
                calCRC=crc16(ModBus_TX_BUFF,6);
                ModBus_TX_BUFF[7]=calCRC&0xFF;
                ModBus_TX_BUFF[6]=(calCRC>>8)&0xFF;
                SendModbusData(8);
        }
        else//?????+??????
        {
                ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
                ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
                ModBus_TX_BUFF[2]=0x02; //???
                SendModbusData(3);
        }
}
//MODBUSRTU SLAVE服务程序，放在主循环内使用

void ModBusRtuSlave_Service(u8 framelen)
{
	u16 recCRC;
	u16 calCRC;
	if(ModBus_RX_BUFF[0]==ModBus_Addr)//????
	{
		if((ModBus_RX_BUFF[1]==0x01)||(ModBus_RX_BUFF[1]==0x02)|| (ModBus_RX_BUFF[1]==03) || (ModBus_RX_BUFF[1]==04) ||(ModBus_RX_BUFF[1]==0x05)|| (ModBus_RX_BUFF[1]==0x10)|| (ModBus_RX_BUFF[1]==0x06))//允许的功能码
		{
			startRegAddr=(((u16)ModBus_RX_BUFF[2])<<8)|ModBus_RX_BUFF[3];//起始地址
			calCRC=crc16(ModBus_RX_BUFF,framelen-2);//CRC计算
			recCRC=ModBus_RX_BUFF[framelen-1]|(((u16)ModBus_RX_BUFF[framelen-2])<<8);//接收到的CRC
			if(calCRC==recCRC)//CRC校验
			{
							/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
				switch(ModBus_RX_BUFF[1])//????????????
				{       
					case 01: //读取多个输出线圈
					{
						Modbus_01_Solve();
						break;
					}
					case 02: //读取多个输入线圈
					{
						Modbus_02_Solve();
						break;
					}
					case 03: //读取多个保持寄存器
					{
						Modbus_03_Solve();
						break;
					}
					 case 04: //读取多个输入寄存器
					{
						Modbus_04_Solve();
						break;
					}                                                              
					case 05: //写单个输出线圈
					{
						Modbus_05_Solve();
						break;
					}
					case 06: //写单个保持寄存器
					{
						Modbus_06_Solve();
						break;
					}
					case 16://写多个保持寄存器
						Modbus_16_Solve();
					break;																			 
				}
							//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			}
			else//CRC校验失败
			{
				ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
				ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
				ModBus_TX_BUFF[2]=0x04;
				SendModbusData(3);
			}                                                       
		}
		else//错误的功能码
		{
			ModBus_TX_BUFF[0]=ModBus_RX_BUFF[0];
			ModBus_TX_BUFF[1]=ModBus_RX_BUFF[1]|0x80;
			ModBus_TX_BUFF[2]=0x01;
			SendModbusData(3);
		}
	}
}
