GDResignationMaterials/485-breaker-sensor/util/modbus_rtu.c

#include "modbus_rtu.h"

#include "FreeRTOS.h"
#include "crcLib.h"
#include "task.h"
#include <string.h>

static modbus_rtu_trans_fun trans_fun;
static modbus_rtu_recive_fun recive_fun;

void modbus_rtu_on_data_cb(uint8_t *data, uint16_t len);
void modbus_rtu_master_on_data_cb(uint8_t *data, uint16_t len);
void modbus_rtu_slave_on_data_cb(uint8_t *data, uint16_t len);

void modbus_rtu_task(void *arg)
{
	uint8_t *data = NULL;
	uint16_t len = 0;
	uint8_t res = 0;

	while (1)
	{
		data = recive_fun(10, &len);
		if (data != NULL)
		{
			modbus_rtu_on_data_cb(data, len);
		}

		vTaskDelay(pdMS_TO_TICKS(2));
	}
}

void modbus_rtu_init(modbus_rtu_trans_fun trans, modbus_rtu_recive_fun recive)
{
	trans_fun = trans;
	recive_fun = recive;

	xTaskCreate(modbus_rtu_task, "mb_rtu", 150, NULL, 2, NULL);
}

void modbus_rtu_on_data_cb(uint8_t *data, uint16_t len)
{
	if (modbus_get_mode() == 0)
	{
		// modbus_rtu_master_on_data_cb(data, len);
	}
	else
	{
		modbus_rtu_slave_on_data_cb(data, len);
	}
}

void modbus_rtu_slave_on_data_cb(uint8_t *data, uint16_t len)
{
	modbus_slave_t *modbus_slave = modbus_slave_get();
	if (len < 5)
	{
		return;
	}
	uint16_t crc = crc16_modbus(data, len - 2);
	uint16_t bcrc = (data[len - 1] << 8) + data[len - 2];
	if (crc != bcrc)
	{
		return;
	}

	uint8_t address = data[0];
	uint8_t function = data[1];
	if (address != modbus_slave->address)
	{
		return;
	}
	modbus_slave->send_buffer_len = 0;
	modbus_slave->send_buffer[modbus_slave->send_buffer_len++] = address;
	modbus_slave->send_buffer[modbus_slave->send_buffer_len++] = function;
	switch (function)
	{
	case 0x03:
	{
		if (len != 8)
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x01;
			modbus_slave->send_buffer_len += 2;
			break;
		}
		uint16_t reg_addr = (data[2] << 8) + data[3];
		uint16_t reg_num = (data[4] << 8) + data[5];
		uint16_t *tmp = pvPortMalloc(sizeof(uint16_t) * reg_num);
		int8_t res = modbus_slave_reg_read_mult(reg_addr, reg_num, tmp);
		if (res != 0)
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x02;
			modbus_slave->send_buffer_len += 2;
			vPortFree(tmp);
			break;
		}
		for (uint16_t i = 0; i < reg_num; i++)
		{
			modbus_slave->send_buffer[3 + i * 2] = tmp[i] >> 8;
			modbus_slave->send_buffer[4 + i * 2] = tmp[i] & 0xFF;
		}
		vPortFree(tmp);
		modbus_slave->send_buffer[2] = reg_num * 2;
		modbus_slave->send_buffer_len += reg_num * 2 + 1;
		break;
	}
	case 0x06:
	{
		if (len != 8)
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x01;
			modbus_slave->send_buffer_len += 2;
			break;
		}
		uint16_t reg_addr = (data[2] << 8) + data[3];
		uint16_t reg_data = (data[4] << 8) + data[5];
		int8_t res = modbus_slave_reg_write(reg_addr, reg_data);
		if (res != 0)
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x02;
			modbus_slave->send_buffer_len += 2;
			break;
		}
		modbus_slave->send_buffer[2] = reg_addr >> 8;
		modbus_slave->send_buffer[3] = reg_addr & 0xFF;
		modbus_slave->send_buffer[4] = reg_data >> 8;
		modbus_slave->send_buffer[5] = reg_data & 0xFF;
		modbus_slave->send_buffer_len += 4;
		break;
	}
	case 0x10:
	{
		if (len != 9 + data[6])
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x01;
			modbus_slave->send_buffer_len += 2;
			break;
		}
		uint16_t reg_addr = (data[2] << 8) + data[3];
		uint16_t reg_num = (data[4] << 8) + data[5];
		uint16_t *tmp = pvPortMalloc(sizeof(uint16_t) * reg_num);
		for (uint8_t i = 0; i < reg_num; i++)
		{
			tmp[i] = (data[7 + i * 2] << 8) + data[8 + i * 2];
		}
		int8_t res = modbus_slave_reg_write_mult(reg_addr, reg_num, tmp);
		vPortFree(tmp);
		if (res != 0)
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x02;
			modbus_slave->send_buffer_len += 2;
			break;
		}
		modbus_slave->send_buffer[2] = reg_addr >> 8;
		modbus_slave->send_buffer[3] = reg_addr & 0xFF;
		modbus_slave->send_buffer[4] = reg_num >> 8;
		modbus_slave->send_buffer[5] = reg_num & 0xFF;
		modbus_slave->send_buffer_len += 4;
		break;
	}
	case 0x64 ... 0x6e:
	{
		if (len != 5 + data[2])
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x01;
			modbus_slave->send_buffer_len += 2;
			break;
		}
		uint8_t cmd = data[1];
		modbus_cmd_t *box = modbus_slave_cmd_get(cmd);
		if (box == NULL)
		{
			modbus_slave->send_buffer[1] |= 0x80;
			modbus_slave->send_buffer[2] = 0x01;
			modbus_slave->send_buffer_len += 2;
			break;
		}
		uint8_t res_len = 0;
		box->cb(data + 3, data[2], modbus_slave->send_buffer + 3, &res_len);
		modbus_slave->send_buffer[2] = res_len;
		modbus_slave->send_buffer_len += res_len +1;
		break;
	}
	default:
		modbus_slave->send_buffer[1] |= 0x80;
		modbus_slave->send_buffer[2] = 0x01;
		modbus_slave->send_buffer_len += 2;
		break;
	}
	crc = crc16_modbus(modbus_slave->send_buffer, modbus_slave->send_buffer_len);
	modbus_slave->send_buffer[modbus_slave->send_buffer_len++] = crc & 0xFF;
	modbus_slave->send_buffer[modbus_slave->send_buffer_len++] = crc >> 8;
	trans_fun(modbus_slave->send_buffer, modbus_slave->send_buffer_len);
}
	
void modbus_rtu_cmd_echo(uint8_t* data,uint8_t len,uint8_t* outData,uint8_t* outLen)
{
	memcpy(outData,data,len);
	*outLen = len;
}