121 lines
2.7 KiB
C
121 lines
2.7 KiB
C
#include "protocol.h"
|
|
#include "string.h"
|
|
#include "utils.h"
|
|
|
|
int protocol_pack_frame(
|
|
uint16_t slave_addr,
|
|
uint8_t command,const
|
|
uint8_t *payload,
|
|
uint16_t payload_len,
|
|
uint8_t *buffer,uint16_t buffer_size,
|
|
uint16_t *packed_len)
|
|
{
|
|
|
|
// 构建基础帧头
|
|
|
|
buffer[0] = PROTOCOL_HEAD & 0xFF;
|
|
buffer[1] = (PROTOCOL_HEAD >> 8) & 0xFF;
|
|
buffer[2] = slave_addr & 0xFF;
|
|
buffer[3] = (slave_addr >> 8) & 0xFF;
|
|
|
|
buffer[4] = command;
|
|
|
|
buffer[5] = payload_len & 0xFF;
|
|
buffer[6] = (payload_len >> 8) & 0xFF;
|
|
|
|
// 复制有效载荷（保持小端格式）
|
|
if (payload_len > 0 && payload != NULL) {
|
|
memcpy(buffer + PROTOCOL_FRAME_HEAD_SIZE , payload, payload_len);
|
|
}
|
|
|
|
// 设置实际长度
|
|
int fram_len = PROTOCOL_FRAME_MIN_SIZE + payload_len;
|
|
|
|
|
|
// 计算CRC（覆盖地址、命令、长度和有效载荷）
|
|
uint16_t crc = 0xFFFF;
|
|
|
|
crc = modbus_crc16(buffer,fram_len -2);
|
|
|
|
// 写入CRC（保持小端格式）
|
|
|
|
buffer[fram_len-2] = (uint8_t)(crc & 0xFF);
|
|
buffer[fram_len-1] = (uint8_t)(crc >> 8);
|
|
|
|
*packed_len =fram_len;
|
|
|
|
return PROTOCOL_SUCCESS;
|
|
|
|
}
|
|
|
|
int protocol_unpack_frame(
|
|
const uint8_t *frame,
|
|
uint16_t frame_len,
|
|
uint16_t *slave_addr,
|
|
uint8_t *command,
|
|
uint8_t *payload,
|
|
uint16_t *payload_len
|
|
) {
|
|
// 参数校验
|
|
if (frame == NULL || slave_addr == NULL || command == NULL || payload_len == NULL) {
|
|
return PROTOCOL_ERR_INVALID_PARAM;
|
|
}
|
|
if (frame_len < PROTOCOL_FRAME_MIN_SIZE) {
|
|
return PROTOCOL_ERR_INVALID_PARAM;
|
|
}
|
|
|
|
// 帧头校验
|
|
uint16_t header = (frame[0] | frame[1] <<8);
|
|
|
|
if (header != PROTOCOL_HEAD) {
|
|
|
|
return PROTOCOL_ERR_INVALID_PARAM;
|
|
}
|
|
|
|
// 基础字段解析
|
|
*slave_addr = (frame[2] | frame[3] << 8);
|
|
|
|
|
|
*command = frame[4];
|
|
|
|
|
|
*payload_len = (frame[5] | frame[6] << 8);
|
|
|
|
// 数据完整性校验
|
|
if (*payload_len > (frame_len - PROTOCOL_FRAME_MIN_SIZE)) {
|
|
|
|
return PROTOCOL_ERR_BUFFER_OVERFLOW;
|
|
}
|
|
|
|
int frame_size = *payload_len + 9;
|
|
|
|
uint16_t calc_crc =0xffff;
|
|
|
|
calc_crc = modbus_crc16((uint8_t *)frame, frame_size -2 );
|
|
|
|
// 校验CRC（保持小端格式）
|
|
union{
|
|
|
|
uint16_t value;
|
|
struct{
|
|
uint8_t crcl;
|
|
uint8_t crch;
|
|
}bytes;
|
|
|
|
}crc_data;
|
|
|
|
crc_data.bytes.crcl =(frame[frame_size - 2] ) ;
|
|
crc_data.bytes.crch =(frame[frame_size - 1] ) ;
|
|
|
|
if (calc_crc != crc_data.value) {
|
|
return PROTOCOL_ERR_CRC;
|
|
}
|
|
|
|
// 复制有效载荷（保持小端格式）
|
|
if (*payload_len > 0 && payload != NULL) {
|
|
memcpy(payload, frame + PROTOCOL_FRAME_HEAD_SIZE, *payload_len);
|
|
}
|
|
|
|
return PROTOCOL_SUCCESS;
|
|
}
|