246 lines
8.5 KiB
C
246 lines
8.5 KiB
C
/**
|
||
* @file ble_proto.c
|
||
* @brief BLE-网关通信协议 V1.3 序列化 / 反序列化实现
|
||
*
|
||
* 结构：
|
||
* [Cmd:2][Reserved:4][gateway_int_cnt:1][gateway_int32[]:4*N]
|
||
* [device_int_len:2][device_str_len:2][device_bytes_len:2]
|
||
* [Payload][CRC16:2]
|
||
*
|
||
* CRC 计算范围：Cmd ~ Payload 末尾（整个包除 CRC 字段）
|
||
*/
|
||
|
||
#include "ble_proto.h"
|
||
#include <string.h>
|
||
|
||
/* -----------------------------------------------------------------------
|
||
* 内部辅助：Big Endian 读写
|
||
* --------------------------------------------------------------------- */
|
||
|
||
/** 写入 uint16_t（Big Endian） */
|
||
static inline void put_be16(uint8_t *p, uint16_t v)
|
||
{
|
||
p[0] = (uint8_t)(v >> 8);
|
||
p[1] = (uint8_t)(v & 0xFFu);
|
||
}
|
||
|
||
/** 写入 uint32_t（Big Endian） */
|
||
static inline void put_be32(uint8_t *p, uint32_t v)
|
||
{
|
||
p[0] = (uint8_t)(v >> 24);
|
||
p[1] = (uint8_t)((v >> 16) & 0xFFu);
|
||
p[2] = (uint8_t)((v >> 8) & 0xFFu);
|
||
p[3] = (uint8_t)( v & 0xFFu);
|
||
}
|
||
|
||
/** 读取 uint16_t（Big Endian） */
|
||
static inline uint16_t get_be16(const uint8_t *p)
|
||
{
|
||
return (uint16_t)(((uint16_t)p[0] << 8) | p[1]);
|
||
}
|
||
|
||
/** 读取 uint32_t（Big Endian） */
|
||
static inline uint32_t get_be32(const uint8_t *p)
|
||
{
|
||
return ((uint32_t)p[0] << 24)
|
||
| ((uint32_t)p[1] << 16)
|
||
| ((uint32_t)p[2] << 8)
|
||
| (uint32_t)p[3];
|
||
}
|
||
|
||
/* -----------------------------------------------------------------------
|
||
* CRC16（多项式 0x8005，初值 0xFFFF —— MODBUS 变体）
|
||
* --------------------------------------------------------------------- */
|
||
uint16_t ble_proto_crc16(const uint8_t *data, size_t len)
|
||
{
|
||
uint16_t crc = 0xFFFFu;
|
||
for (size_t i = 0; i < len; i++) {
|
||
crc ^= (uint16_t)data[i];
|
||
for (int bit = 0; bit < 8; bit++) {
|
||
if (crc & 0x0001u) {
|
||
crc = (crc >> 1) ^ 0xA001u;
|
||
} else {
|
||
crc >>= 1;
|
||
}
|
||
}
|
||
}
|
||
return crc;
|
||
}
|
||
|
||
/* -----------------------------------------------------------------------
|
||
* 序列化（pack）
|
||
* --------------------------------------------------------------------- */
|
||
int ble_proto_pack(const ble_proto_packet_t *pkt, uint8_t *buf, size_t buf_size)
|
||
{
|
||
if (!pkt || !buf) return BLE_PROTO_ERR_NULL;
|
||
|
||
/* ----- 参数合法性检查 ----- */
|
||
if (pkt->gateway_int_cnt > BLE_PROTO_GW_INT_MAX) return BLE_PROTO_ERR_GW_CNT;
|
||
if (pkt->device_int_len % 4 != 0) return BLE_PROTO_ERR_INT_ALIGN;
|
||
|
||
/* ----- 计算所需总长度 ----- */
|
||
size_t payload_len = (size_t)pkt->device_int_len
|
||
+ (size_t)pkt->device_str_len
|
||
+ (size_t)pkt->device_bytes_len;
|
||
|
||
size_t total = BLE_PROTO_HEADER_LEN /* Cmd(2) + Reserved(4) */
|
||
+ BLE_PROTO_GW_CNT_LEN /* gateway_int_cnt (1) */
|
||
+ (size_t)pkt->gateway_int_cnt * 4u /* gateway_int[] */
|
||
+ BLE_PROTO_DEV_PARAM_LEN /* 3 x device_*_len */
|
||
+ payload_len /* Payload */
|
||
+ BLE_PROTO_CRC_LEN; /* CRC16 */
|
||
|
||
if (buf_size < total) return BLE_PROTO_ERR_BUF_SMALL;
|
||
|
||
uint8_t *p = buf;
|
||
|
||
/* ----- Protocol Header ----- */
|
||
put_be16(p, pkt->cmd); p += 2;
|
||
put_be32(p, 0); p += 4; /* Reserved 固定填 0 */
|
||
|
||
/* ----- Gateway Parameter ----- */
|
||
*p++ = pkt->gateway_int_cnt; /* uint8 */
|
||
for (uint8_t i = 0; i < pkt->gateway_int_cnt; i++) {
|
||
put_be32(p, (uint32_t)pkt->gateway_int[i]);
|
||
p += 4;
|
||
}
|
||
|
||
/* ----- Device Parameter ----- */
|
||
put_be16(p, pkt->device_int_len); p += 2;
|
||
put_be16(p, pkt->device_str_len); p += 2;
|
||
put_be16(p, pkt->device_bytes_len); p += 2;
|
||
|
||
/* ----- Payload ----- */
|
||
if (pkt->device_int_len > 0 && pkt->int_data) {
|
||
for (uint16_t i = 0; i < pkt->device_int_len / 4u; i++) {
|
||
put_be32(p, (uint32_t)pkt->int_data[i]);
|
||
p += 4;
|
||
}
|
||
}
|
||
if (pkt->device_str_len > 0 && pkt->str_data) {
|
||
memcpy(p, pkt->str_data, pkt->device_str_len);
|
||
p += pkt->device_str_len;
|
||
}
|
||
if (pkt->device_bytes_len > 0 && pkt->bytes_data) {
|
||
memcpy(p, pkt->bytes_data, pkt->device_bytes_len);
|
||
p += pkt->device_bytes_len;
|
||
}
|
||
|
||
/* ----- CRC16（从 buf[0] 开始，到 Payload 末尾） ----- */
|
||
uint16_t crc = ble_proto_crc16(buf, (size_t)(p - buf));
|
||
put_be16(p, crc);
|
||
p += 2;
|
||
|
||
|
||
return (int)(p - buf);
|
||
}
|
||
|
||
/* -----------------------------------------------------------------------
|
||
* 反序列化（unpack）
|
||
* --------------------------------------------------------------------- */
|
||
int ble_proto_unpack(const uint8_t *buf, size_t len, ble_proto_packet_t *pkt)
|
||
{
|
||
if (!buf || !pkt) return BLE_PROTO_ERR_NULL;
|
||
|
||
/* 最小包长：Header(6) + gw_cnt(1) + dev_param(6) + CRC(2) = 15 */
|
||
const size_t min_len = BLE_PROTO_HEADER_LEN
|
||
+ BLE_PROTO_GW_CNT_LEN
|
||
+ BLE_PROTO_DEV_PARAM_LEN
|
||
+ BLE_PROTO_CRC_LEN;
|
||
if (len < min_len) return BLE_PROTO_ERR_LEN;
|
||
|
||
const uint8_t *p = buf;
|
||
|
||
/* ----- Protocol Header ----- */
|
||
pkt->cmd = get_be16(p); p += 2;
|
||
pkt->reserved = get_be32(p); p += 4;
|
||
|
||
/* ----- Gateway Parameter ----- */
|
||
pkt->gateway_int_cnt = *p++;
|
||
if (pkt->gateway_int_cnt > BLE_PROTO_GW_INT_MAX) return BLE_PROTO_ERR_GW_CNT;
|
||
|
||
/* 检查剩余字节是否足够容纳 gateway_int[] + dev_param + CRC */
|
||
size_t consumed = (size_t)(p - buf)
|
||
+ (size_t)pkt->gateway_int_cnt * 4u
|
||
+ BLE_PROTO_DEV_PARAM_LEN
|
||
+ BLE_PROTO_CRC_LEN;
|
||
if (len < consumed) return BLE_PROTO_ERR_LEN;
|
||
|
||
for (uint8_t i = 0; i < pkt->gateway_int_cnt; i++) {
|
||
pkt->gateway_int[i] = (int32_t)get_be32(p);
|
||
p += 4;
|
||
}
|
||
|
||
/* ----- Device Parameter ----- */
|
||
pkt->device_int_len = get_be16(p); p += 2;
|
||
pkt->device_str_len = get_be16(p); p += 2;
|
||
pkt->device_bytes_len = get_be16(p); p += 2;
|
||
|
||
if (pkt->device_int_len % 4 != 0) return BLE_PROTO_ERR_INT_ALIGN;
|
||
|
||
size_t payload_len = (size_t)pkt->device_int_len
|
||
+ (size_t)pkt->device_str_len
|
||
+ (size_t)pkt->device_bytes_len;
|
||
|
||
/* 验证总长度（含 CRC） */
|
||
if ((size_t)(p - buf) + payload_len + BLE_PROTO_CRC_LEN != len) {
|
||
return BLE_PROTO_ERR_LEN;
|
||
}
|
||
|
||
/* ----- CRC16 校验（从 buf[0] 到 CRC 前一字节） ----- */
|
||
uint16_t crc_calc = ble_proto_crc16(buf, len - BLE_PROTO_CRC_LEN);
|
||
uint16_t crc_recv = get_be16(buf + len - BLE_PROTO_CRC_LEN);
|
||
if (crc_calc != crc_recv) return BLE_PROTO_ERR_CRC;
|
||
|
||
/* ----- Payload（零拷贝，直接指向 buf 内部） ----- */
|
||
if (pkt->device_int_len > 0) {
|
||
pkt->int_data = (const int32_t *)p;
|
||
} else {
|
||
pkt->int_data = NULL;
|
||
}
|
||
p += pkt->device_int_len;
|
||
|
||
pkt->str_data = (pkt->device_str_len > 0) ? (const char *)p : NULL;
|
||
p += pkt->device_str_len;
|
||
|
||
pkt->bytes_data = (pkt->device_bytes_len > 0) ? p : NULL;
|
||
|
||
// printf("cmd = %d", pkt->cmd);
|
||
// printf("reserved = %d", pkt->reserved);
|
||
// printf("gateway_int_cnt = %d", pkt->gateway_int_cnt);
|
||
// printf("device_int_len = %d", pkt->device_int_len);
|
||
// printf("device_str_len = %d", pkt->device_str_len);
|
||
// printf("device_bytes_len = %d", pkt->device_bytes_len);
|
||
|
||
// /* int_data 指向 buf 内部，地址未必 4 字节对齐，
|
||
// * 不能直接解引用 int32_t*（对齐访问会触发硬件异常）；
|
||
// * 必须按字节读取，用 get_be32 还原数值 */
|
||
// if (pkt->int_data && pkt->device_int_len >= 4) {
|
||
// const uint8_t *ip = (const uint8_t *)pkt->int_data;
|
||
// printf("int_data = ");
|
||
// for (uint16_t i = 0; i < pkt->device_int_len / 4u; i++) {
|
||
// printf("%08X ", (unsigned int)get_be32(ip + (uint32_t)i * 4u));
|
||
// }
|
||
// printf("\n");
|
||
// }
|
||
// /* str_data / bytes_data 不保证有 '\0'，用 printf_buf 打印 hex */
|
||
// if (pkt->str_data && pkt->device_str_len > 0) {
|
||
// printf("str_data = ");
|
||
// for (size_t i = 0; i < pkt->device_str_len; i++) {
|
||
// printf("%02X ", *(pkt->str_data + i));
|
||
// }
|
||
// printf("\n");
|
||
// }
|
||
// if (pkt->bytes_data && pkt->device_bytes_len > 0) {
|
||
// printf("bytes_data = ");
|
||
// for (size_t i = 0; i < pkt->device_bytes_len; i++) {
|
||
// printf("%02X ", *(pkt->bytes_data + i));
|
||
// }
|
||
// printf("\n");
|
||
// }
|
||
// printf("crc = %04X", crc_recv);
|
||
// printf("\n");
|
||
|
||
return BLE_PROTO_OK;
|
||
}
|