pile_com_stm32/main_old/stm32/ModbusS.c

#include <string.h>
#include "ModbusS.h"
#include "config.h"
#include <stdio.h>
#include "driver/ledc.h"
#include "esp_err.h"
#include <string.h>
#include "esp_log.h"

#include "config.h"

static const char *TAG = "main";


static const uint8_t  auchCRCHi[] =
{
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
} ;
/* CRC??????*/
static const uint8_t auchCRCLo[] =
{
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
    0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
    0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
    0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
    0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
    0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
    0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
    0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
    0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
    0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
    0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
    0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
    0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
    0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
    0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
    0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
    0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
    0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
    0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
    0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
    0x43, 0x83, 0x41, 0x81, 0x80, 0x40
} ;

uint16_t crc16(uint8_t *puchMsg, uint16_t usDataLen)
{
    uint8_t uchCRCHi = 0xFF ; /* ?CRC????? */
    uint8_t uchCRCLo = 0xFF ; /* ?CRC ????? */
    uint32_t uIndex ; /* CRC?????? */
    while (usDataLen--) /* ??????? */
    {
        uIndex = uchCRCHi ^ *puchMsg++ ; /* ??CRC */
        uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex] ;
        uchCRCLo = auchCRCLo[uIndex] ;
    }
    return (uchCRCHi << 8 | uchCRCLo) ;
}//uint16 crc16(uint8 *puchMsg, uint16 usDataLen)

 
 
#pragma pack(0x10)  
uint16_t gWordVar[gWORD_SIZE];
uint8_t gBitVar[(gBIT_SIZE+7)/8];
#pragma pack()

int isBitHI(uint16_t Add)
{
    if(gBitVar[(Add)>>3] & (1<<((Add)&0x07)))
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

void ModBusBitWriteHook(uint16_t addr, uint16_t length)
{
}
extern void reset_depth(void);
uint16_t reboot_req ;
extern uint16_t last_pile_id;
extern int zb_ModBusWordWriteHook(uint16_t addr, uint16_t length);
void ModBusWordWriteHook(uint16_t addr, uint16_t length)
{
   // ESP_LOGI(TAG, "ModBusWordWriteHook");
	if(addr == RECORD_REG_ADDR && gWordVar[RECORD_REG_ADDR] == 0xffff){
		reset_depth();
        gWordVar[RECORD_REG_ADDR] = 0;
	}
    if(addr == DEPTH_RESET_ADDR && gWordVar[DEPTH_RESET_ADDR] == 0x5555){
		config_load();
		gWordVar[DEPTH_RESET_ADDR] = 0;
	}
	if(addr == DEPTH_RESET_ADDR && gWordVar[DEPTH_RESET_ADDR] == 0x55aa){
        gWordVar[RECORD_REG_ADDR] = 9999;   //强制增加桩号
		reset_depth();
		gWordVar[DEPTH_RESET_ADDR] = 0;
	}
	if(addr == LAST_PILE_ID_ADDR){
        last_pile_id = gWordVar[LAST_PILE_ID_ADDR];
    }
    else if(addr == FLOW_CONFIG_ADDR && gWordVar[FLOW_CONFIG_ADDR] == 0x55aa){
		save_flow_cfg();
	}
	else if(addr == DEPTH_CONFIG_ADDR && gWordVar[DEPTH_CONFIG_ADDR] == 0x55aa){
        save_depth_cfg();
	}
	else if(addr == CAL_4_20MA_ADDR && gWordVar[CAL_4_20MA_ADDR] == 0x55aa){
		save_cal_4_20ma();
	}
	// else if(addr >= FLOW_CONFIG_ADDR && addr < (FLOW_CONFIG_ADDR + sizeof(flow_config_t))/*  && gWordVar[FLOW_CONFIG_ADDR] == 0x55aa */){
	// 	save_flow_cfg();
	// }
	// else if(addr >= DEPTH_CONFIG_ADDR && addr < (DEPTH_CONFIG_ADDR + sizeof(depth_config_t))){
    //     save_depth_cfg();
	// }
	// else if(addr >= CAL_4_20MA_ADDR && addr < (CAL_4_20MA_ADDR + sizeof(cal_4_20ma_t))){
	// 	save_cal_4_20ma();
	// }
	else if(addr == REBOOT_REW_ADDR ){
		if(gWordVar[REBOOT_REW_ADDR] == 0x55aa){
			reboot_req = 0x55aa;
		}
		else if(gWordVar[REBOOT_REW_ADDR] == 0x55ab){
			esp_restart(); 
		}
		else if(gWordVar[REBOOT_REW_ADDR] == 0xaa55){
			restore_default();
			esp_restart(); 
		}
	}
	else{
		zb_ModBusWordWriteHook(addr,length);
	}
}
extern volatile int modbus_save_flg;

uint16_t modbus_addr = 1;
int ModbusSlaveProcess(uint8_t *txbuf, uint8_t *rxbuf, uint16_t rxLen, int is_crc)
{
    uint16_t crcData,crcChk;
    uint8_t Offset,ByteAdd;
    uint16_t ByteNumber;
    uint16_t add;
    uint16_t length;
    int out_len =  0;
    int i;
    uint8_t *pDat;
    uint16_t *pWordVar;

    if(rxLen<4){
        return 0;
    }

    /*地址校验*/    
    if((rxbuf[0]==modbus_addr) || (rxbuf[0]==255)){

        /*crc校验*/
        if(is_crc != 0){
            crcData = rxbuf[rxLen-1]+(rxbuf[rxLen-2]<<8);
            crcChk = crc16(rxbuf,rxLen-2);
            if( crcData != crcChk){
                return 0;
            }
        }

        add = ((uint16_t)rxbuf[2]<<8)+ rxbuf[3];
        length = (rxbuf[4]<<8) | rxbuf[5];
				
        txbuf[0] = rxbuf[0];
        switch(rxbuf[1])
        {
        case 0x01:		//Read Coils
            if((add+length) > gBIT_SIZE)
            {
                txbuf[1] = 0x81;
                txbuf[2] = 0x02;	//ILLEGAL DATA ADDRESS
                out_len = 3;
                break;
            }
            txbuf[1]=0x01;
            ByteNumber = (length+7)/8;
            txbuf[2]= ByteNumber;
            ByteAdd = add>>3;		//add/8
            Offset = add&0x07;		//add%8
            for(i=0; i<(ByteNumber-1); i++)
            {
                txbuf[3+i] = gBitVar[ByteAdd+i]>>Offset;
                txbuf[3+i] |= gBitVar[ByteAdd+i+1]<<(8-Offset);
            }
            txbuf[3+ByteNumber-1] = gBitVar[ByteAdd+ByteNumber-1]>>Offset;
            txbuf[3+ByteNumber-1] &= 0xff>>Offset;
            out_len =  ByteNumber+3;
            break;
        case 0x03:
        case 0x04:	//Read Holding Registers
            if((add+length) > gWORD_SIZE)
            {
                txbuf[1] = 0x80 + rxbuf[1];
                txbuf[2] = 0x02;	//ILLEGAL DATA ADDRESS
                out_len =  3;;
                break;
            }
            if(length > 512)
            {
                length = 512;
            }
            if(rxbuf[1]==4)
            {
                add += 64;
            }
            txbuf[1]=rxbuf[1];
            ByteNumber = length*2;
            txbuf[2]= ByteNumber&0xff;
            pDat = &txbuf[3];
            pWordVar = (uint16_t *)&gWordVar[add];
            for(i=0; i<length; i++)
            {
                *pDat++ = *pWordVar>>8;
                *pDat++ = *pWordVar&0xff;
                pWordVar++;
            }
            out_len =  ByteNumber+3;
            break;
        case 0x05:	//Write Single Coil
            if(add >= gBIT_SIZE)
            {
                txbuf[1] = 0x85;
                txbuf[2] = 0x02;	//ILLEGAL DATA ADDRESS
                out_len = 3;
                break;
            }
            ByteAdd = add>>3;		//same add/8
            Offset = add&0x07;		//same add%8
            if(rxbuf[4] == 0)
            {
                clrBit(add);
            }
            else if(rxbuf[4] == 0xff)
            {
                setBit(add);
            }
            else
            {
                txbuf[1] = 0x85;
                txbuf[2] = 0x03;	//ILLEGAL DATA VALUE
                out_len = 3;
                break;
            }
            memcpy(txbuf,rxbuf,6);
            ModBusBitWriteHook(add,1);
            out_len = 6;
            break;
        case 0x06:	//Write Single Register
            // ESP_LOGI(TAG, "modbus 0x06");
            if(add >= gWORD_SIZE)
            {
                txbuf[1] = 0x86;
                txbuf[2] = 0x02;	//ILLEGAL DATA ADDRESS
                out_len = 3;
                break;
            }
			gWordVar[add] = (rxbuf[4]<<8)+rxbuf[5]; 
            // ESP_LOGI(TAG, "gWordVar[%d]=0x%04x", add, gWordVar[add]);
       //     ESP_LOGI(TAG, "%d %d", depth_config->input_type, depth_config->port);
            memcpy(txbuf,rxbuf,6);
            ModBusWordWriteHook(add,1);
            out_len = 6;
            break;
        case 0x0F:	//Write Multiple Coil
            if((add+length) > gBIT_SIZE)
            {
                txbuf[1] = 0x8F;
                txbuf[2] = 0x02;	//ILLEGAL DATA ADDRESS
                out_len = 3;
            }
						
            txbuf[1]=0x0F;
            txbuf[2] = rxbuf[2];
            txbuf[3] = rxbuf[3];
            txbuf[4] = 0;
            txbuf[5] = length&0xff;
            pDat = rxbuf+7;
            for(i=0; i<length; i++)
            {
                if(*(pDat+(i>>3)) & (1<<(i&0x07)))
                    setBit(i+add);
                else
                    clrBit(i+add);
            }
            ModBusBitWriteHook(add,length);
            out_len = 6;
            break;
        case 0x10:	//Write Multiple registers
            if((add+length) > gWORD_SIZE)
            {
                txbuf[1] = 0x90;
                txbuf[2] = 0x02;	//ILLEGAL DATA ADDRESS
                crcChk = crc16(txbuf,3);
                txbuf[3] = crcChk>>8;
                txbuf[4] = crcChk;
                out_len = 3;
            }
            txbuf[1]=0x10;
            txbuf[2] = rxbuf[2];
            txbuf[3] = rxbuf[3];
            txbuf[4]=  0;
            txbuf[5]= length&0xff;
            pDat = rxbuf+7;
            pWordVar = (uint16_t *)&gWordVar[add];
			{
				for(i=0; i<length; i++)
				{
						*pWordVar++ = (*pDat<<8)|*(pDat+1);
						pDat+=2;
				}
			}
            // ESP_LOGI(TAG, "write gWordVar:");
            // for (i = 0; i < length; i++)
            //     ESP_LOGI(TAG, "gWordVar[%d]=0x%04x", add + i, gWordVar[add + i]);
		//	ESP_LOGI(TAG, "%d %d", depth_config->input_type, depth_config->port);			

            ModBusWordWriteHook(add,length);
            out_len = 6;
            break;
        }//switch(rxbuf[1])
		
        if(is_crc && out_len > 0)
        {
            crcChk = crc16(txbuf,out_len);
            txbuf[out_len++] = crcChk>>8;
            txbuf[out_len++] = crcChk & 0xff;
        }
    }
    return out_len;
}