Stm32Base/Core/User/modbus_deal.c

#include "modbus_deal.h"
#include "usart.h"
#include "string.h"
#include "stdio.h" 
#include "crc.h"
#include "iap.h"
#include "parameters.h"
#include "modbus_implement.h"

typedef struct _mb_ctx
{
	uint8_t* rxbuf;
	uint8_t* txbuf;
	uint16_t rx_len;  //recv data length
	uint16_t tx_len;  //will be transfer data length
	uint16_t txbuf_size;
	uint8_t  exception_code;
	uint8_t  func_code;   // function code 
	uint16_t cmd;    			// register

}MB_CTX;

static MB_CTX g_mb_ctx;



void mb_03_handle(void)
{
	
		uint16_t tx_offset = 0;
		uint16_t crc;
		uint16_t reg_num = ((uint16_t)g_mb_ctx.rxbuf[4]<<8)|g_mb_ctx.rxbuf[5];
		uint16_t read_len = reg_num<<1; // wanted bytes 
		//
		g_mb_ctx.txbuf[0] = g_mb_ctx.rxbuf[0]; 		// slave addr 
		g_mb_ctx.txbuf[1] = g_mb_ctx.rxbuf[1]; 		// func code 
		g_mb_ctx.txbuf[2] = 0x00;
	
		tx_offset = 3;
	
		switch(g_mb_ctx.cmd){
			case Cmd_FirmwareVersion:
				if(16 == read_len){
					tx_offset += Read_FirmwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);	
				}else if(18 == read_len){
					tx_offset += Read_FirmwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_HardwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else if(22 == read_len){
					tx_offset += Read_FirmwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_HardwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Deviceid(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else if(24 == read_len){
					tx_offset += Read_FirmwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_HardwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Deviceid(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Devicetype(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else {
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			case Cmd_HardwareVersion:
				if(2 == read_len){
					tx_offset += Read_HardwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else if(6 == read_len){
					tx_offset += Read_HardwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Deviceid(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else if(8 == read_len){
					tx_offset += Read_HardwareVersion(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Deviceid(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Devicetype(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			case Cmd_DeviceID:
				if(4 == read_len){
					tx_offset += Read_Deviceid(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else if(6 == read_len){
					tx_offset += Read_Deviceid(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Devicetype(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			case Cmd_DeviceType:
				if(2 == read_len){
					tx_offset += Read_Devicetype(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			case Cmd_Addr:
				if(2 == read_len){
					tx_offset += Read_Addr(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else if(4 == read_len){
					tx_offset += Read_Addr(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
					tx_offset += Read_Baudrate(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else {
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			case Cmd_Baudrate:
				if(2 == read_len){
					tx_offset += Read_Baudrate(g_mb_ctx.txbuf+tx_offset, g_mb_ctx.txbuf_size-tx_offset);
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			default:
				g_mb_ctx.exception_code = INVALID_COMMAND;

				break;
		};
		
		if(NO_EXCEPTION == g_mb_ctx.exception_code){
				g_mb_ctx.txbuf[2] = (uint8_t)read_len;
				g_mb_ctx.tx_len = tx_offset;
		
		}else{
				g_mb_ctx.txbuf[1] |= 0x80; 		// func code 
				g_mb_ctx.txbuf[2] = g_mb_ctx.exception_code;
				g_mb_ctx.tx_len = 3;
		}
		
		crc = mb_crc16(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
		
		g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)(crc & 0x00ff); 
		g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)((crc>>8) & 0x00ff);
		
		rs485_TransmitData(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
			
}

void mb_06_handle(void)
{
		uint16_t crc;
		uint8_t ret_write = RET_OK;

		memcpy(g_mb_ctx.txbuf, g_mb_ctx.rxbuf, g_mb_ctx.rx_len);
	
		switch(g_mb_ctx.cmd){
			case Cmd_HardwareVersion:
				if(2 == g_mb_ctx.rx_len-4-2){
					ret_write = Write_HardwareVersion(g_mb_ctx.rxbuf+4, 2); 
					if(RET_DATAINVALID == ret_write)
						g_mb_ctx.exception_code = INVALID_DATA;
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}

				break;

			case Cmd_DeviceType:
				if(2 == g_mb_ctx.rx_len-4-2){
					ret_write = Write_Devicetype(g_mb_ctx.rxbuf+4, 2); 
					if(RET_DATAINVALID == ret_write)
						g_mb_ctx.exception_code = INVALID_DATA;
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}

				break;
			case Cmd_Addr:
				if(2 == g_mb_ctx.rx_len-4-2){
					ret_write = Write_Addr(g_mb_ctx.rxbuf+4, 2); 
					if(RET_DATAINVALID == ret_write)
						g_mb_ctx.exception_code = INVALID_DATA;
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			case Cmd_Baudrate:
				if(2 == g_mb_ctx.rx_len-4-2){
					ret_write = Write_Baudrate(g_mb_ctx.rxbuf+4, 2); 
					if(RET_DATAINVALID == ret_write)
						g_mb_ctx.exception_code = INVALID_DATA;
				}else{
					g_mb_ctx.exception_code = INVALID_DATA;
				}
				break;
			case Cmd_Reboot:
				//NVIC_SystemReset();
				ret_write = RET_NEED_REBOOT;
				break;
			case Cmd_Reset:
				ResetConfig();
				ret_write = RET_NEED_SAVE|RET_NEED_REBOOT;
				//NVIC_SystemReset();
				break;
			default:
				g_mb_ctx.exception_code = INVALID_COMMAND;
				break;
		};
		
		if(NO_EXCEPTION == g_mb_ctx.exception_code){
				g_mb_ctx.tx_len = g_mb_ctx.rx_len;
		
		}else{
				g_mb_ctx.txbuf[1] |= 0x80; 		// func code 
				g_mb_ctx.txbuf[2] = g_mb_ctx.exception_code;
				g_mb_ctx.tx_len = 3;
			
				crc = mb_crc16(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
				g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)(crc & 0x00ff); 
			  g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)((crc>>8) & 0x00ff);
		}
		
		rs485_TransmitData(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
		
		if((ret_write&RET_NEED_SAVE) > 0 ){
				SaveConfig();
		}
		
		if((ret_write&RET_NEED_REBOOT) > 0 ){
				//mdelay(200);
				HAL_Delay(100);
				NVIC_SystemReset();
		}

		
}

void mb_10_handle(void)
{
		uint16_t crc;
		uint8_t ret_write = RET_OK;
		uint16_t reg_num = ((uint16_t)g_mb_ctx.rxbuf[4]<<8)|g_mb_ctx.rxbuf[5];
		uint16_t data_len = g_mb_ctx.rxbuf[6];

		memcpy(g_mb_ctx.txbuf, g_mb_ctx.rxbuf, g_mb_ctx.rx_len);
	
		// 先判段数据长度是否一致
		if(data_len != reg_num*2){
			g_mb_ctx.exception_code = INVALID_DATA;
		}else{
			if(data_len != g_mb_ctx.rx_len-7-2){
				g_mb_ctx.exception_code = INVALID_DATA;
			}
		
		}
		
		if(NO_EXCEPTION == g_mb_ctx.exception_code){
			
			switch(g_mb_ctx.cmd){
				case Cmd_HardwareVersion:
					if(8 == data_len){
						ret_write |= Write_HardwareVersion(g_mb_ctx.rxbuf+7, 2); 
						ret_write |= Write_Deviceid(g_mb_ctx.rxbuf+7+2, 4); 
						ret_write |= Write_Devicetype(g_mb_ctx.rxbuf+7+6, 2); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else if(6 == data_len){
						ret_write |= Write_HardwareVersion(g_mb_ctx.rxbuf+7, 2); 
						ret_write |= Write_Deviceid(g_mb_ctx.rxbuf+7+2, 4); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else if(2 == g_mb_ctx.rx_len-4-2){
						ret_write = Write_HardwareVersion(g_mb_ctx.rxbuf+7, 2); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else{
						g_mb_ctx.exception_code = INVALID_DATA;
					}

					break;
					
				case Cmd_DeviceID:
					if(6 == data_len){
						ret_write |= Write_Deviceid(g_mb_ctx.rxbuf+7, 4); 
						ret_write |= Write_Devicetype(g_mb_ctx.rxbuf+7+4, 2); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else if(4 == data_len){
						ret_write |= Write_Deviceid(g_mb_ctx.rxbuf+7, 4); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else{
						g_mb_ctx.exception_code = INVALID_DATA;
					}

					break;

				case Cmd_DeviceType:
					if(2 == data_len){
						ret_write |= Write_Devicetype(g_mb_ctx.rxbuf+7, 2); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else{
						g_mb_ctx.exception_code = INVALID_DATA;
					}

					break;
				case Cmd_Addr:
					if(4 == data_len){
						ret_write |= Write_Addr(g_mb_ctx.rxbuf+7, 2); 
						ret_write |= Write_Addr(g_mb_ctx.rxbuf+7+2, 2); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else if(2 == data_len){
						ret_write |= Write_Addr(g_mb_ctx.rxbuf+7, 2); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else{
						g_mb_ctx.exception_code = INVALID_DATA;
					}
					break;
				case Cmd_Baudrate:
					if(2 == data_len){
						ret_write |= Write_Baudrate(g_mb_ctx.rxbuf+7, 2); 
						if(ret_write&RET_DATAINVALID)
							g_mb_ctx.exception_code = INVALID_DATA;
					}else{
						g_mb_ctx.exception_code = INVALID_DATA;
					}
					break;

				default:
					g_mb_ctx.exception_code = INVALID_COMMAND;
					break;
			};
		}
	
		
		
		if(NO_EXCEPTION == g_mb_ctx.exception_code){
				//只需要收到的前面6个字节的数据
				g_mb_ctx.tx_len = 6;
		
		}else{
				g_mb_ctx.txbuf[1] |= 0x80; 		// func code 
				g_mb_ctx.txbuf[2] = g_mb_ctx.exception_code;
				g_mb_ctx.tx_len = 3;
		}
		
		crc = mb_crc16(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
		g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)(crc & 0x00ff); 
		g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)((crc>>8) & 0x00ff);
		rs485_TransmitData(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
		
		if((ret_write&RET_NEED_SAVE) > 0 ){
				SaveConfig();
		}
		
		if((ret_write&RET_NEED_REBOOT) > 0 ){
				//mdelay(200);
				//__set_FAULTMASK(1);//关闭所有中断
				HAL_Delay(100);
				NVIC_SystemReset();//复位函数
		}
}

void mb_42_handle(void)
{
	
	uint16_t crc;
	uint8_t ret_write = RET_OK;
	
	memcpy(g_mb_ctx.txbuf, g_mb_ctx.rxbuf, 4);
	g_mb_ctx.tx_len = 4;
	
	if(Cmd_IapUpgrade == g_mb_ctx.cmd){
		
		g_mb_ctx.tx_len += IAP_CmdHandle(g_mb_ctx.rxbuf+g_mb_ctx.tx_len, g_mb_ctx.rx_len-4-2, g_mb_ctx.txbuf+g_mb_ctx.tx_len);
		if(1 == iap_reboot){
			ret_write |= RET_NEED_REBOOT;
		}
		
	}else{
		g_mb_ctx.exception_code = INVALID_COMMAND;
	}
	
	if(NO_EXCEPTION == g_mb_ctx.exception_code){
			//啥都不用做
	
	}else{
			g_mb_ctx.txbuf[1] |= 0x80; 		// func code 
			g_mb_ctx.txbuf[2] = g_mb_ctx.exception_code;
			g_mb_ctx.tx_len = 3;
	}
	
	crc = mb_crc16(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
	g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)(crc & 0x00ff); 
	g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)((crc>>8) & 0x00ff);
	
	rs485_TransmitData(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
	
	if((ret_write&RET_NEED_SAVE) > 0 ){
			SaveConfig();
	}
	
	if((ret_write&RET_NEED_REBOOT) > 0 ){
			//mdelay(200);
			HAL_Delay(100);
			NVIC_SystemReset();
	}
	
	
	
}


void mb_funccodeerror_handle(void)
{
	uint16_t crc;
	
	memcpy(g_mb_ctx.txbuf, g_mb_ctx.rxbuf, 4);
	
	
	g_mb_ctx.txbuf[1] |= 0x80;
	g_mb_ctx.txbuf[2] = INVALID_FUNCTION_CODE;
	g_mb_ctx.tx_len = 3;
	
	crc = mb_crc16(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
	
	g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)(crc & 0x00ff); 
	g_mb_ctx.txbuf[g_mb_ctx.tx_len++] = (uint8_t)((crc>>8) & 0x00ff);
	
	rs485_TransmitData(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
	
}

void mb_handle(void)
{
		g_mb_ctx.func_code = g_mb_ctx.rxbuf[1];
		g_mb_ctx.cmd = ((uint16_t)g_mb_ctx.rxbuf[2]<<8) | g_mb_ctx.rxbuf[3]; 
	
		if(MB_FUNC_03 == g_mb_ctx.func_code){
				mb_03_handle();
		}else if(MB_FUNC_06 == g_mb_ctx.func_code){
				mb_06_handle();
		}else if(MB_FUNC_10 == g_mb_ctx.func_code){
				mb_10_handle();
		}else if(MB_FUNC_42 == g_mb_ctx.func_code){
				mb_42_handle();
		}else{
				mb_funccodeerror_handle();
		}
}


void Modbus_StartReceive(void)
{
		rs485_ReceiveData();
}

void Modbus_DataHandle(void)
{
		g_mb_ctx.rxbuf = rs485_info.recv_buffer;
		g_mb_ctx.rx_len = rs485_info.recv_len;
		g_mb_ctx.txbuf = rs485_info.send_buffer;
		g_mb_ctx.txbuf_size = UART_TRANSMIT_DATA_POOL_COUNT;
		g_mb_ctx.tx_len = 0;
		g_mb_ctx.exception_code = NO_EXCEPTION;
	
		uint16_t crc;
		//uint16_t ret_len = 0;
		//uint8_t ret_write = RET_OK;
	
		if(g_mb_ctx.rx_len > 2){
			
			//check rs485 addr 
			if(config->addr == g_mb_ctx.rxbuf[0] || BROADCAST_ADDR == g_mb_ctx.rxbuf[0]){
				
	#if 1					
						// check crc
						crc = ((uint16_t)g_mb_ctx.rxbuf[g_mb_ctx.rx_len-1]<<8) | g_mb_ctx.rxbuf[g_mb_ctx.rx_len-2]; 
						if (crc == mb_crc16(g_mb_ctx.rxbuf, g_mb_ctx.rx_len-2))
						{
								mb_handle();		
						}
						
//						printf("recv 444  crc[%x], calculate22 crc[%x]  \r\n", crc, mb_crc16(g_mb_ctx.rxbuf, g_mb_ctx.rx_len-2));
						
						
	#else
			memcpy(g_mb_ctx.txbuf, (void *)g_mb_ctx.rxbuf, g_mb_ctx.rx_len);
			g_mb_ctx.tx_len = g_mb_ctx.rx_len;
				
				printf("rs485 rev data length:[%d] \r\n", g_mb_ctx.rx_len);
			
			rs485_TransmitData(g_mb_ctx.txbuf, g_mb_ctx.tx_len);
				
	#endif 			
					
			}
		
		} //END rx_len 
		
		
		if(g_mb_ctx.rx_len > 0){
			rs485_ReceiveData();
		}
		

}