ch32v307_camera/Middle/QDXnetworkStack/qdx_protocol.c

/**
 * @file qdx_protocol.c
 * @brief Communication Protocol 2.0 Utility Implementations
 */

#include "qdx_protocol.h"
#include <string.h>

/* ============================================================
 * Shift-Safe Serialization (Little-Endian)
 * ============================================================ */

void qdx_write_u16_le(uint8_t *buf, uint16_t val) {
  buf[0] = (uint8_t)(val & 0xFF);
  buf[1] = (uint8_t)((val >> 8) & 0xFF);
}

void qdx_write_u32_le(uint8_t *buf, uint32_t val) {
  buf[0] = (uint8_t)(val & 0xFF);
  buf[1] = (uint8_t)((val >> 8) & 0xFF);
  buf[2] = (uint8_t)((val >> 16) & 0xFF);
  buf[3] = (uint8_t)((val >> 24) & 0xFF);
}

uint16_t qdx_read_u16_le(const uint8_t *buf) {
  return (uint16_t)(buf[0] | (buf[1] << 8));
}

uint32_t qdx_read_u32_le(const uint8_t *buf) {
  /* 显式转型防止高位字节的符号位扩展 */
  return ((uint32_t)buf[0]) | ((uint32_t)buf[1] << 8) |
         ((uint32_t)buf[2] << 16) | ((uint32_t)buf[3] << 24);
}

/* ============================================================
 * CRC16-MODBUS Checksum (Polynomial: 0xA001, Init: 0xFFFF)
 * ============================================================ */
uint16_t qdx_crc16_modbus(const uint8_t *data, int len) {
  uint16_t crc = 0xFFFF;
  for (int i = 0; i < len; i++) {
    crc ^= data[i];
    for (int j = 0; j < 8; j++) {
      if (crc & 0x0001) {
        crc >>= 1;
        crc ^= 0xA001;
      } else {
        crc >>= 1;
      }
    }
  }
  return crc;
}

/* ============================================================
 * Build Frame (Header + TLV + Payload + CRC16)
 * Uses strict byte shifting to prevent unaligned struct access on MCU.
 * ============================================================ */
int qdx_build_frame(uint8_t *buf, uint8_t msg_class, uint8_t tlv_type,
                    const uint8_t *payload, uint16_t payload_len,
                    uint16_t dev_id, uint16_t seq, uint32_t timestamp,
                    uint8_t flags) {

  flags |= FLAG_LAST_FRAGMENT;
  uint16_t total_len = HEADER_SIZE + TLV_HEADER_SIZE + payload_len + CRC_SIZE;

  /* 1. Fill FrameHeader (16 bytes) safely */
  qdx_write_u16_le(buf + 0, PROTO_MAGIC); /* Magic */
  buf[2] = PROTO_VERSION;                 /* Version */
  qdx_write_u16_le(buf + 3, total_len);   /* Length */
  qdx_write_u16_le(buf + 5, seq);         /* Sequence */
  qdx_write_u32_le(buf + 7, timestamp);   /* Timestamp */
  buf[11] = 0x01;                         /* Source = MCU (0x01) */
  qdx_write_u16_le(buf + 12, dev_id);     /* DevID */
  buf[14] = msg_class;                    /* Class */
  buf[15] = flags;                        /* Flags */

  /* 2. Fill TLV Header (3 bytes) safely */
  buf[16] = tlv_type;                      /* TLV Type */
  qdx_write_u16_le(buf + 17, payload_len); /* TLV Length */

  /* 3. Copy Payload（使用 memmove 防止 payload 与 buf 重叠时的未定义行为） */
  if (payload && payload_len > 0) {
    memmove(buf + HEADER_SIZE + TLV_HEADER_SIZE, payload, payload_len);
  }

  /* 4. Append CRC16 */
  uint16_t crc = qdx_crc16_modbus(buf, total_len - CRC_SIZE);
  qdx_write_u16_le(buf + total_len - CRC_SIZE, crc);

  return total_len;
}

/* ============================================================
 * In-Place Frame Build (Header + CRC only, payload already in place)
 * 用于零拷贝场景：payload（含 TLV + 数据）已填充在 buf[HEADER_SIZE] 处，
 * 本函数仅写入 16 字节帧头并在尾部追加 CRC16。
 * ============================================================ */
int qdx_build_frame_inplace(uint8_t *buf, uint8_t msg_class,
                            uint16_t payload_len, uint16_t dev_id, uint16_t seq,
                            uint32_t timestamp, uint8_t flags) {

  flags |= FLAG_LAST_FRAGMENT;
  uint16_t total_len = HEADER_SIZE + payload_len + CRC_SIZE;

  /* 填写 16 字节帧头 */
  qdx_write_u16_le(buf + 0, PROTO_MAGIC);
  buf[2] = PROTO_VERSION;
  qdx_write_u16_le(buf + 3, total_len);
  qdx_write_u16_le(buf + 5, seq);
  qdx_write_u32_le(buf + 7, timestamp);
  buf[11] = 0x01; /* Source = MCU */
  qdx_write_u16_le(buf + 12, dev_id);
  buf[14] = msg_class;
  buf[15] = flags;

  /* payload 已在 buf[16..16+payload_len-1]，无需拷贝 */

  /* 追加 CRC16 */
  uint16_t crc = qdx_crc16_modbus(buf, total_len - CRC_SIZE);
  qdx_write_u16_le(buf + total_len - CRC_SIZE, crc);

  return total_len;
}

/* ============================================================
 * Build Fragmented Frame (Header + Chunk + CRC16, NO TLV)
 * ============================================================ */
int qdx_build_fragment_frame(uint8_t *buf, uint8_t msg_class,
                             const uint8_t *chunk, uint16_t chunk_len,
                             uint16_t dev_id, uint16_t seq, uint32_t timestamp,
                             uint8_t flags) {

  uint16_t total_len = HEADER_SIZE + chunk_len + CRC_SIZE;

  /* 1. Fill FrameHeader (16 bytes) safely */
  qdx_write_u16_le(buf + 0, PROTO_MAGIC);
  buf[2] = PROTO_VERSION;
  qdx_write_u16_le(buf + 3, total_len);
  qdx_write_u16_le(buf + 5, seq);
  qdx_write_u32_le(buf + 7, timestamp);
  buf[11] = 0x01; /* Source MCU */
  qdx_write_u16_le(buf + 12, dev_id);
  buf[14] = msg_class;
  buf[15] = flags;

  /* 2. Copy Payload Chunk（使用 memmove 应对潜在重叠） */
  if (chunk && chunk_len > 0) {
    memmove(buf + HEADER_SIZE, chunk, chunk_len);
  }

  /* 3. Append CRC16 */
  uint16_t crc = qdx_crc16_modbus(buf, total_len - CRC_SIZE);
  qdx_write_u16_le(buf + total_len - CRC_SIZE, crc);

  return total_len;
}