431 lines
15 KiB
C
431 lines
15 KiB
C
#include "modbus.h"
|
||
#define TX_EN GPIO_WriteBit(RS485_GPIO, RS485_GPIO_Pin, Bit_SET);
|
||
#define TX_DIS GPIO_WriteBit(RS485_GPIO, RS485_GPIO_Pin, Bit_RESET);
|
||
#define MODBUS_LED_ON GPIO_WriteBit(Modbus_LED, Modbus_LED_Pin, Bit_SET);
|
||
#define MODBUS_LED_OFF GPIO_WriteBit(Modbus_LED, Modbus_LED_Pin, Bit_RESET);
|
||
|
||
static uint8_t buf_ptr = 0;
|
||
static unsigned char rx_buf_ptr = 0;
|
||
static unsigned char rx_buf[128];
|
||
static unsigned char tx_buf[128];
|
||
|
||
static uint16_t addr_buf_1 = 0, addr_buf_2 = 0;
|
||
static uint16_t regs_to_read = 0;
|
||
static uint16_t CRC16 = 0;
|
||
static unsigned char crc_buf[300];
|
||
static unsigned char modbus_id = 0;
|
||
static uint16_t reg_wr_data = 0;
|
||
static uint16_t modbus_reg_addr;
|
||
|
||
|
||
static uint16_t holding_registers[MAX_HOLDING_REGISTERS];
|
||
static uint16_t input_registers[MAX_INPUT_REGISTERS];
|
||
|
||
static unsigned char rx_flag = 0;
|
||
static unsigned char timer_state = 0;
|
||
|
||
|
||
static uint16_t dev_id = DEFAULT_DEVICE_MODBUS_ID;
|
||
const uint16_t com_dev_id = 247;
|
||
|
||
static void (*ModbusUartSendByte)(USART_TypeDef*, unsigned char);
|
||
static int32_t (*ModbusTcpSendByte)(uint8_t, unsigned char*, uint16_t);
|
||
static void (*ModbusRIRPoll)(void);
|
||
static void (*ModbusRHRPoll)(void);
|
||
static void (*ModbusWSRPoll)(uint16_t, uint16_t);
|
||
|
||
void modbus_connect_callback_send_byte(void (*s)(USART_TypeDef*, unsigned char)) {
|
||
ModbusUartSendByte = s;
|
||
}
|
||
|
||
void modbus_connect_callback_send_buf_tcp(int32_t (*s)(uint8_t, unsigned char*, uint16_t)) {
|
||
ModbusTcpSendByte = s;
|
||
}
|
||
|
||
void modbus_connect_callback_rir_poll(void (*s)(void)) {
|
||
ModbusRIRPoll = s;
|
||
}
|
||
|
||
void modbus_connect_callback_rhr_poll(void (*s)(void)) {
|
||
ModbusRHRPoll = s;
|
||
}
|
||
|
||
void modbus_connect_callback_wsr_poll(void (*s)(uint16_t, uint16_t)) {
|
||
ModbusWSRPoll = s;
|
||
}
|
||
|
||
static uint16_t modbus_CRC16(unsigned char buf[], uint16_t len)
|
||
{
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> crc16
|
||
|
||
uint16_t crc = 0xFFFF;
|
||
//U8 crc_lsb, crc_msb;
|
||
for (uint16_t pos = 0; pos < len; pos++) {
|
||
crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc
|
||
for (int i = 8; i != 0; i--) { // Loop over each bit
|
||
if ((crc & 0x0001) != 0) { // If the LSB is set
|
||
crc >>= 1; // Shift right and XOR 0xA001
|
||
crc ^= 0xA001;
|
||
}
|
||
else // Else LSB is not set
|
||
crc >>= 1; // Just shift right
|
||
}
|
||
}
|
||
|
||
// Note, this number has low and high bytes swapped,
|
||
// so use it accordingly (or swap bytes)
|
||
// swapping bytes
|
||
crc = ((crc<<8)&0xff00)|((crc>>8)&0x00ff);
|
||
|
||
return crc;
|
||
}
|
||
|
||
void modbus_wsr_answer() {
|
||
crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
|
||
crc_buf[1] = (unsigned char) MODBUS_WSR_CMD;
|
||
crc_buf[2] = (unsigned char) (modbus_reg_addr >> 8);
|
||
crc_buf[3] = (unsigned char) (modbus_reg_addr & 0xff);
|
||
crc_buf[4] = (unsigned char) (reg_wr_data >> 8);
|
||
crc_buf[5] = (unsigned char) (reg_wr_data & 0xff);
|
||
|
||
CRC16 = modbus_CRC16(crc_buf, 6);
|
||
|
||
switch(CURRENT_OPERATION_MODE) {
|
||
case 0:
|
||
TX_EN;
|
||
ModbusUartSendByte(Modbus_UART, (modbus_id == dev_id) ? dev_id : com_dev_id);
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)MODBUS_WSR_CMD);
|
||
ModbusUartSendByte(Modbus_UART, modbus_reg_addr >> 8);
|
||
ModbusUartSendByte(Modbus_UART, modbus_reg_addr & 0xff);
|
||
ModbusUartSendByte(Modbus_UART, reg_wr_data >> 8);
|
||
ModbusUartSendByte(Modbus_UART, reg_wr_data & 0xff);
|
||
|
||
ModbusUartSendByte(Modbus_UART, CRC16 >> 8);
|
||
ModbusUartSendByte(Modbus_UART, CRC16 & 0xff);
|
||
TX_DIS;
|
||
break;
|
||
case 1:
|
||
tx_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
|
||
tx_buf[1] = MODBUS_WSR_CMD;
|
||
tx_buf[2] = modbus_reg_addr >> 8;
|
||
tx_buf[3] = modbus_reg_addr & 0xff;
|
||
tx_buf[4] = reg_wr_data >> 8;
|
||
tx_buf[5] = reg_wr_data & 0xff;
|
||
tx_buf[6] = CRC16 >> 8;
|
||
tx_buf[7] = CRC16 & 0xff;
|
||
|
||
ModbusTcpSendByte(DEFAULT_TCP_MODBUS_SOCKET, tx_buf, 8);
|
||
break;
|
||
}
|
||
}
|
||
|
||
void modbus_rir_answer() {
|
||
buf_ptr = 0;
|
||
addr_buf_1 = modbus_reg_addr;
|
||
addr_buf_2 = addr_buf_1;
|
||
|
||
crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
|
||
crc_buf[1] = MODBUS_RIR_CMD;
|
||
crc_buf[2] = regs_to_read * 2;
|
||
|
||
unsigned char cnt = CRC_COUNT_OFFSET;
|
||
|
||
for (int i = 0; i < regs_to_read; i++) {
|
||
crc_buf[cnt++] = (unsigned char)(input_registers[addr_buf_1] >> 8);
|
||
crc_buf[cnt++] = (unsigned char)(input_registers[addr_buf_1] & 0xff);
|
||
++addr_buf_1;
|
||
}
|
||
|
||
CRC16 = modbus_CRC16(crc_buf, regs_to_read * 2 + CRC_COUNT_OFFSET);
|
||
|
||
switch(CURRENT_OPERATION_MODE) {
|
||
case 0:
|
||
TX_EN;
|
||
ModbusUartSendByte(Modbus_UART, (modbus_id == dev_id) ? dev_id : com_dev_id);
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char) MODBUS_RIR_CMD);
|
||
ModbusUartSendByte(Modbus_UART, regs_to_read * 2);
|
||
|
||
for (int i = 0; i < regs_to_read; i++) {
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(input_registers[addr_buf_2] >> 8));
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(input_registers[addr_buf_2] & 0xff));
|
||
++addr_buf_2;
|
||
}
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 >> 8));
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 & 0xff));
|
||
TX_DIS;
|
||
break;
|
||
case 1:
|
||
tx_buf[buf_ptr++] = (modbus_id == dev_id) ? dev_id : com_dev_id;
|
||
tx_buf[buf_ptr++] = (unsigned char) MODBUS_RIR_CMD;
|
||
tx_buf[buf_ptr++] = regs_to_read * 2;
|
||
|
||
for (int i = 0; i < regs_to_read; i++) {
|
||
tx_buf[buf_ptr++] = (unsigned char)(input_registers[addr_buf_2] >> 8);
|
||
tx_buf[buf_ptr++] = (unsigned char)(input_registers[addr_buf_2] & 0xff);
|
||
++addr_buf_2;
|
||
}
|
||
tx_buf[buf_ptr++] = CRC16 >> 8;
|
||
tx_buf[buf_ptr++] = CRC16 & 0xff;
|
||
ModbusTcpSendByte(DEFAULT_TCP_MODBUS_SOCKET, tx_buf, buf_ptr);
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
void modbus_rhr_answer() {
|
||
buf_ptr = 0;
|
||
addr_buf_1 = modbus_reg_addr;
|
||
addr_buf_2 = addr_buf_1;
|
||
|
||
crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
|
||
crc_buf[1] = MODBUS_RHR_CMD;
|
||
crc_buf[2] = regs_to_read * 2;
|
||
|
||
unsigned char cnt = CRC_COUNT_OFFSET;
|
||
|
||
for (int i = 0; i < regs_to_read; i++) {
|
||
crc_buf[cnt++] = (unsigned char)(holding_registers[addr_buf_1] >> 8);
|
||
crc_buf[cnt++] = (unsigned char)(holding_registers[addr_buf_1] & 0xff);
|
||
++addr_buf_1;
|
||
}
|
||
|
||
CRC16 = modbus_CRC16(crc_buf, regs_to_read * 2 + CRC_COUNT_OFFSET);
|
||
|
||
switch(CURRENT_OPERATION_MODE) {
|
||
case 0:
|
||
TX_EN;
|
||
ModbusUartSendByte(Modbus_UART, (modbus_id == dev_id) ? dev_id : com_dev_id);
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char) MODBUS_RHR_CMD);
|
||
ModbusUartSendByte(Modbus_UART, regs_to_read * 2);
|
||
|
||
for (int i = 0; i < regs_to_read; i++) {
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(holding_registers[addr_buf_2] >> 8));
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(holding_registers[addr_buf_2] & 0xff));
|
||
++addr_buf_2;
|
||
}
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 >> 8));
|
||
ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 & 0xff));
|
||
TX_DIS;
|
||
break;
|
||
case 1:
|
||
tx_buf[buf_ptr++] = (modbus_id == dev_id) ? dev_id : com_dev_id;
|
||
tx_buf[buf_ptr++] = (unsigned char) MODBUS_RHR_CMD;
|
||
tx_buf[buf_ptr++] = regs_to_read * 2;
|
||
|
||
for (int i = 0; i < regs_to_read; i++) {
|
||
tx_buf[buf_ptr++] = (unsigned char)(holding_registers[addr_buf_2] >> 8);
|
||
tx_buf[buf_ptr++] = (unsigned char)(holding_registers[addr_buf_2] & 0xff);
|
||
++addr_buf_2;
|
||
}
|
||
tx_buf[buf_ptr++] = CRC16 >> 8;
|
||
tx_buf[buf_ptr++] = CRC16 & 0xff;
|
||
ModbusTcpSendByte(DEFAULT_TCP_MODBUS_SOCKET, tx_buf, buf_ptr);
|
||
|
||
break;
|
||
}
|
||
}
|
||
|
||
static unsigned char modbus_rx_CRC_check(unsigned char modbus_cmd) {
|
||
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> CRC <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
uint16_t CRC16_calc = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
unsigned char ans = 0;
|
||
|
||
modbus_reg_addr = (uint16_t)((rx_buf[2] << 8) | rx_buf[3]); // get starting reg addr
|
||
|
||
crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
|
||
crc_buf[2] = (unsigned char)(modbus_reg_addr >> 8);
|
||
crc_buf[3] = (unsigned char)(modbus_reg_addr & 0x00ff);
|
||
|
||
|
||
switch(modbus_cmd) {
|
||
case MODBUS_WSR_CMD:
|
||
reg_wr_data = (rx_buf[4] << 8) | rx_buf[5]; // get data to write into reg
|
||
|
||
crc_buf[1] = (unsigned char) MODBUS_WSR_CMD;
|
||
crc_buf[4] = (unsigned char)(reg_wr_data >> 8);
|
||
crc_buf[5] = (unsigned char)(reg_wr_data & 0x00ff);
|
||
break;
|
||
//----
|
||
case MODBUS_RHR_CMD:
|
||
regs_to_read = (rx_buf[4] << 8) | rx_buf[5]; // get number of regs to read
|
||
|
||
crc_buf[1] = (unsigned char) MODBUS_RHR_CMD;
|
||
crc_buf[4] = (unsigned char)(regs_to_read >> 8);
|
||
crc_buf[5] = (unsigned char)(regs_to_read & 0x00ff);
|
||
break;
|
||
//----
|
||
case MODBUS_RIR_CMD:
|
||
regs_to_read = (rx_buf[4] << 8) | rx_buf[5]; // get number of regs to read
|
||
|
||
crc_buf[1] = (unsigned char) MODBUS_RIR_CMD;
|
||
crc_buf[4] = (unsigned char)(regs_to_read >> 8);
|
||
crc_buf[5] = (unsigned char)(regs_to_read & 0x00ff);
|
||
break;
|
||
//----
|
||
default: break;
|
||
}
|
||
|
||
CRC16 = (rx_buf[6] << 8) | rx_buf[7]; // get CRC16 from rx msg
|
||
CRC16_calc = modbus_CRC16(crc_buf,6); // calc CRC16
|
||
|
||
if(CRC16_calc == CRC16)
|
||
ans = modbus_cmd;
|
||
|
||
return ans;
|
||
}
|
||
|
||
unsigned char modbus_get_poll(void)
|
||
{
|
||
/// update modbus regs and vars, send answer to master
|
||
unsigned char rx_cmd_code = 0; // ??? ??????????? ???????rx_cmd_code = 0;
|
||
// state 1 and 2, transmit end, rx buf has > 7 bytes ?
|
||
//
|
||
if ( rx_flag == 1 ) {
|
||
if( rx_buf_ptr > 7 ) {
|
||
modbus_id = rx_buf[0]; // get device ID from master msg
|
||
|
||
if((modbus_id == dev_id) || (modbus_id == com_dev_id)) {
|
||
switch(rx_buf[1]) {
|
||
case MODBUS_RHR_CMD: // ???? ??????? - ?????? R/W ?????????
|
||
if(modbus_rx_CRC_check(MODBUS_RHR_CMD) == MODBUS_RHR_CMD) {
|
||
rx_cmd_code = MODBUS_RHR_CMD;
|
||
}
|
||
break;
|
||
////-------------------------------------------------------------------
|
||
case MODBUS_WSR_CMD: // ???? ??????? - ?????? Read-only ?????????
|
||
if(modbus_rx_CRC_check(MODBUS_WSR_CMD) == MODBUS_WSR_CMD) {
|
||
rx_cmd_code = MODBUS_WSR_CMD;
|
||
holding_reg_write(modbus_reg_addr, reg_wr_data); // !!!!
|
||
}
|
||
break;
|
||
////-------------------------------------------------------------------
|
||
case MODBUS_RIR_CMD:
|
||
if(modbus_rx_CRC_check(MODBUS_RIR_CMD) == MODBUS_RIR_CMD) {
|
||
rx_cmd_code = MODBUS_RIR_CMD;
|
||
}
|
||
break;
|
||
} // switch(rx_buf[1])
|
||
modbus_reset();
|
||
} else {
|
||
modbus_reset();
|
||
}// if dev_id
|
||
} else { modbus_reset(); } // if(rx_buf_pos > 7)
|
||
}
|
||
|
||
return rx_cmd_code;
|
||
}
|
||
|
||
void modbus_reset(void)
|
||
{
|
||
for(int i = 0; i < 128; i++)
|
||
rx_buf[i] = 0;
|
||
|
||
rx_buf_ptr = 0;
|
||
rx_flag = 0;
|
||
}
|
||
|
||
void holding_reg_write(uint16_t red_addr, uint16_t value) {
|
||
holding_registers[red_addr] = value;
|
||
}
|
||
|
||
void holding_reg_read(uint16_t red_addr, uint16_t *usr_var_ptr) {
|
||
*usr_var_ptr = holding_registers[red_addr];
|
||
}
|
||
|
||
void input_reg_read(uint16_t red_addr, uint16_t *usr_var_ptr) {
|
||
*usr_var_ptr = input_registers[red_addr];
|
||
}
|
||
|
||
void input_reg_write(uint16_t reg_addr, uint16_t value) {
|
||
input_registers[reg_addr] = value;
|
||
}
|
||
|
||
uint16_t get_wr_reg_addr(void) {
|
||
return modbus_reg_addr;
|
||
}
|
||
|
||
uint16_t get_wr_reg_val(void) {
|
||
return reg_wr_data;
|
||
}
|
||
|
||
unsigned char get_modbus_id(void) {
|
||
return dev_id;
|
||
}
|
||
|
||
void set_modbus_id(unsigned char newID)
|
||
{
|
||
dev_id = newID;
|
||
modbus_reset();
|
||
}
|
||
|
||
static uint16_t RegisterAddr = 0;
|
||
static uint16_t RegisterValue = 0;
|
||
|
||
void modbus_poll(void) {
|
||
uint8_t mb_get_poll = modbus_get_poll();
|
||
switch(mb_get_poll) {
|
||
//////////////////////////// READING INPUTS //////////////////////////
|
||
case MODBUS_RIR_CMD:
|
||
MODBUS_LED_ON;
|
||
ModbusRIRPoll();
|
||
modbus_rir_answer();
|
||
MODBUS_LED_OFF;
|
||
break;
|
||
//////////////////////////// READING HOLDING //////////////////////////
|
||
case MODBUS_RHR_CMD:
|
||
MODBUS_LED_ON;
|
||
ModbusRHRPoll();
|
||
modbus_rhr_answer();
|
||
MODBUS_LED_OFF;
|
||
break;
|
||
//////////////////////////// WRITING HOLDING //////////////////////////
|
||
case MODBUS_WSR_CMD:
|
||
MODBUS_LED_ON;
|
||
modbus_wsr_answer();
|
||
|
||
RegisterAddr = get_wr_reg_addr(); // get address
|
||
RegisterValue = get_wr_reg_val(); // get the new value
|
||
|
||
ModbusWSRPoll(RegisterAddr, RegisterValue);
|
||
MODBUS_LED_OFF; // LED toggle
|
||
break;
|
||
} // switch
|
||
}
|
||
//======================= UART HANDLERS =========================//
|
||
void Modbus_UART_IRQHandler(void) {
|
||
if (USART_GetITStatus(Modbus_UART, USART_IT_RXNE) != RESET) {
|
||
while(USART_GetFlagStatus(Modbus_UART,USART_FLAG_RXNE) == RESET) {}
|
||
if (rx_buf_ptr < 128) {
|
||
rx_buf[rx_buf_ptr++] = (unsigned char)Modbus_UART -> DR;
|
||
} else {
|
||
Modbus_UART -> DR;
|
||
}
|
||
USART_ClearITPendingBit(Modbus_UART, USART_IT_RXNE);
|
||
|
||
if (timer_state == 0) {
|
||
timer_state = 1;
|
||
Modbus_TIM -> CNT = 0;
|
||
TIM_Cmd(Modbus_TIM, ENABLE);
|
||
}
|
||
}
|
||
}
|
||
|
||
void Modbus_TIM_IRQHandler(void) {
|
||
if (TIM_GetITStatus(Modbus_TIM, TIM_IT_Update) != RESET)
|
||
TIM_ClearITPendingBit(Modbus_TIM, TIM_IT_Update);
|
||
|
||
TIM_Cmd(Modbus_TIM, DISABLE);
|
||
timer_state = 0;
|
||
rx_flag = 1;
|
||
}
|
||
|
||
|
||
//======================== TCP HANDLERS ========================//
|
||
void Modbus_TCP_Handler(unsigned char* buf, uint8_t size) {
|
||
for (rx_buf_ptr = 0; rx_buf_ptr < size; rx_buf_ptr++) {
|
||
rx_buf[rx_buf_ptr] = buf[rx_buf_ptr];
|
||
}
|
||
|
||
rx_flag = 1;
|
||
}
|