feat: ESP32-C3 OTA 中继项目 initial commit
This commit is contained in:
小龙虾
@@ -0,0 +1,572 @@
|
||||
/*
|
||||
* protocol.c - 协议层实现
|
||||
* T2.1, T2.2, T1.3
|
||||
* ESP-IDF 6.0 适配版本 - 使用软件 AES 实现 + 硬件 RNG
|
||||
*/
|
||||
#include <string.h>
|
||||
#include "esp_random.h"
|
||||
#include "app_config.h"
|
||||
#include "protocol.h"
|
||||
#include "debug_log.h"
|
||||
|
||||
static bool s_protocol_initialized = false;
|
||||
|
||||
// ========== 软件 AES-128 实现 ==========
|
||||
|
||||
// AES S-Box
|
||||
static const uint8_t sbox[256] = {
|
||||
0x63,0x7c,0x77,0x7b,0xf2,0x6b,0x6f,0xc5,0x30,0x01,0x67,0x2b,0xfe,0xd7,0xab,0x76,
|
||||
0xca,0x82,0xc9,0x7d,0xfa,0x59,0x47,0xf0,0xad,0xd4,0xa2,0xaf,0x9c,0xa4,0x72,0xc0,
|
||||
0xb7,0xfd,0x93,0x26,0x36,0x3f,0xf7,0xcc,0x34,0xa5,0xe5,0xf1,0x71,0xd8,0x31,0x15,
|
||||
0x04,0xc7,0x23,0xc3,0x18,0x96,0x05,0x9a,0x07,0x12,0x80,0xe2,0xeb,0x27,0xb2,0x75,
|
||||
0x09,0x83,0x2c,0x1a,0x1b,0x6e,0x5a,0xa0,0x52,0x3b,0xd6,0xb3,0x29,0xe3,0x2f,0x84,
|
||||
0x53,0xd1,0x00,0xed,0x20,0xfc,0xb1,0x5b,0x6a,0xcb,0xbe,0x39,0x4a,0x4c,0x58,0xcf,
|
||||
0xd0,0xef,0xaa,0xfb,0x43,0x4d,0x33,0x85,0x45,0xf9,0x02,0x7f,0x50,0x3c,0x9f,0xa8,
|
||||
0x51,0xa3,0x40,0x8f,0x92,0x9d,0x38,0xf5,0xbc,0xb6,0xda,0x21,0x10,0xff,0xf3,0xd2,
|
||||
0xcd,0x0c,0x13,0xec,0x5f,0x97,0x44,0x17,0xc4,0xa7,0x7e,0x3d,0x64,0x5d,0x19,0x73,
|
||||
0x60,0x81,0x4f,0xdc,0x22,0x2a,0x90,0x88,0x46,0xee,0xb8,0x14,0xde,0x5e,0x0b,0xdb,
|
||||
0xe0,0x32,0x3a,0x0a,0x49,0x06,0x24,0x5c,0xc2,0xd3,0xac,0x62,0x91,0x95,0xe4,0x79,
|
||||
0xe7,0xc8,0x37,0x6d,0x8d,0xd5,0x4e,0xa9,0x6c,0x56,0xf4,0xea,0x65,0x7a,0xae,0x08,
|
||||
0xba,0x78,0x25,0x2e,0x1c,0xa6,0xb4,0xc6,0xe8,0xdd,0x74,0x1f,0x4b,0xbd,0x8b,0x8a,
|
||||
0x70,0x3e,0xb5,0x66,0x48,0x03,0xf6,0x0e,0x61,0x35,0x57,0xb9,0x86,0xc1,0x1d,0x9e,
|
||||
0xe1,0xf8,0x98,0x11,0x69,0xd9,0x8e,0x94,0x9b,0x1e,0x87,0xe9,0xce,0x55,0x28,0xdf,
|
||||
0x8c,0xa1,0x89,0x0d,0xbf,0xe6,0x42,0x68,0x41,0x99,0x2d,0x0f,0xb0,0x54,0xbb,0x16
|
||||
};
|
||||
|
||||
// AES 逆 S-Box
|
||||
static const uint8_t rsbox[256] = {
|
||||
0x52,0x09,0x6a,0xd5,0x30,0x36,0xa5,0x38,0xbf,0x40,0xa3,0x9e,0x81,0xf3,0xd7,0xfb,
|
||||
0x7c,0xe3,0x39,0x82,0x9b,0x2f,0xff,0x87,0x34,0x8e,0x43,0x44,0xc4,0xde,0xe9,0xcb,
|
||||
0x54,0x7b,0x94,0x32,0xa6,0xc2,0x23,0x3d,0xee,0x4c,0x95,0x0b,0x42,0xfa,0xc3,0x4e,
|
||||
0x08,0x2e,0xa1,0x66,0x28,0xd9,0x24,0xb2,0x76,0x5b,0xa2,0x49,0x6d,0x8b,0xd1,0x25,
|
||||
0x72,0xf8,0xf6,0x64,0x86,0x68,0x98,0x16,0xd4,0xa4,0x5c,0xcc,0x5d,0x65,0xb6,0x92,
|
||||
0x6c,0x70,0x48,0x50,0xfd,0xed,0xb9,0xda,0x5e,0x15,0x46,0x57,0xa7,0x8d,0x9d,0x84,
|
||||
0x90,0xd8,0xab,0x00,0x8c,0xbc,0xd3,0x0a,0xf7,0xe4,0x58,0x05,0xb8,0xb3,0x45,0x06,
|
||||
0xd0,0x2c,0x1e,0x8f,0xca,0x3f,0x0f,0x02,0xc1,0xaf,0xbd,0x03,0x01,0x13,0x8a,0x6b,
|
||||
0x3a,0x91,0x11,0x41,0x4f,0x67,0xdc,0xea,0x97,0xf2,0xcf,0xce,0xf0,0xb4,0xe6,0x73,
|
||||
0x96,0xac,0x74,0x22,0xe7,0xad,0x35,0x85,0xe2,0xf9,0x37,0xe8,0x1c,0x75,0xdf,0x6e,
|
||||
0x47,0xf1,0x1a,0x71,0x1d,0x29,0xc5,0x89,0x6f,0xb7,0x62,0x0e,0xaa,0x18,0xbe,0x1b,
|
||||
0xfc,0x56,0x3e,0x4b,0xc6,0xd2,0x79,0x20,0x9a,0xdb,0xc0,0xfe,0x78,0xcd,0x5a,0xf4,
|
||||
0x1f,0xdd,0xa8,0x33,0x88,0x07,0xc7,0x31,0xb1,0x12,0x10,0x59,0x27,0x80,0xec,0x5f,
|
||||
0x60,0x51,0x7f,0xa9,0x19,0xb5,0x4a,0x0d,0x2d,0xe5,0x7a,0x9f,0x93,0xc9,0x9c,0xef,
|
||||
0xa0,0xe0,0x3b,0x4d,0xae,0x2a,0xf5,0xb0,0xc8,0xeb,0xbb,0x3c,0x83,0x53,0x99,0x61,
|
||||
0x17,0x2b,0x04,0x7e,0xba,0x77,0xd6,0x26,0xe1,0x69,0x14,0x63,0x55,0x21,0x0c,0x7d
|
||||
};
|
||||
|
||||
// AES Rcon
|
||||
static const uint8_t Rcon[11] = {
|
||||
0x8d,0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x1b,0x36
|
||||
};
|
||||
|
||||
// AES 状态操作
|
||||
#define AES_BLOCK_SIZE 16
|
||||
|
||||
typedef struct {
|
||||
uint8_t round_key[176]; // 10 rounds * 16 bytes + initial key
|
||||
} aes128_context_t;
|
||||
|
||||
// GF(2^8) 乘法
|
||||
static uint8_t gmul(uint8_t a, uint8_t b)
|
||||
{
|
||||
uint8_t p = 0, hbit;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (b & 1) {
|
||||
p ^= a;
|
||||
}
|
||||
hbit = a & 0x80;
|
||||
a <<= 1;
|
||||
if (hbit) {
|
||||
a ^= 0x1b; // 模不可约多项式 x^8 + x^4 + x^3 + x + 1
|
||||
}
|
||||
b >>= 1;
|
||||
}
|
||||
return p;
|
||||
}
|
||||
|
||||
// 密钥扩展
|
||||
static void aes128_key_expansion(const uint8_t *key, aes128_context_t *ctx)
|
||||
{
|
||||
uint8_t temp[4];
|
||||
memcpy(ctx->round_key, key, 16);
|
||||
|
||||
for (int i = 4; i < 44; i++) {
|
||||
for (int j = 0; j < 4; j++) {
|
||||
temp[j] = ctx->round_key[(i - 1) * 4 + j];
|
||||
}
|
||||
|
||||
if (i % 4 == 0) {
|
||||
// RotWord
|
||||
uint8_t t = temp[0];
|
||||
temp[0] = temp[1];
|
||||
temp[1] = temp[2];
|
||||
temp[2] = temp[3];
|
||||
temp[3] = t;
|
||||
|
||||
// SubWord
|
||||
temp[0] = sbox[temp[0]];
|
||||
temp[1] = sbox[temp[1]];
|
||||
temp[2] = sbox[temp[2]];
|
||||
temp[3] = sbox[temp[3]];
|
||||
|
||||
temp[0] ^= Rcon[i / 4];
|
||||
}
|
||||
|
||||
for (int j = 0; j < 4; j++) {
|
||||
ctx->round_key[i * 4 + j] = ctx->round_key[(i - 4) * 4 + j] ^ temp[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 字节替换
|
||||
static void sub_bytes(uint8_t *state)
|
||||
{
|
||||
for (int i = 0; i < 16; i++) {
|
||||
state[i] = sbox[state[i]];
|
||||
}
|
||||
}
|
||||
|
||||
// 逆字节替换
|
||||
static void inv_sub_bytes(uint8_t *state)
|
||||
{
|
||||
for (int i = 0; i < 16; i++) {
|
||||
state[i] = rsbox[state[i]];
|
||||
}
|
||||
}
|
||||
|
||||
// 行移位
|
||||
static void shift_rows(uint8_t *state)
|
||||
{
|
||||
uint8_t temp;
|
||||
|
||||
// 第1行左移1位
|
||||
temp = state[1];
|
||||
state[1] = state[5];
|
||||
state[5] = state[9];
|
||||
state[9] = state[13];
|
||||
state[13] = temp;
|
||||
|
||||
// 第2行左移2位
|
||||
temp = state[2];
|
||||
state[2] = state[10];
|
||||
state[10] = temp;
|
||||
temp = state[6];
|
||||
state[6] = state[14];
|
||||
state[14] = temp;
|
||||
|
||||
// 第3行左移3位
|
||||
temp = state[15];
|
||||
state[15] = state[11];
|
||||
state[11] = state[7];
|
||||
state[7] = state[3];
|
||||
state[3] = temp;
|
||||
}
|
||||
|
||||
// 逆行移位
|
||||
static void inv_shift_rows(uint8_t *state)
|
||||
{
|
||||
uint8_t temp;
|
||||
|
||||
// 第1行右移1位
|
||||
temp = state[13];
|
||||
state[13] = state[9];
|
||||
state[9] = state[5];
|
||||
state[5] = state[1];
|
||||
state[1] = temp;
|
||||
|
||||
// 第2行右移2位
|
||||
temp = state[2];
|
||||
state[2] = state[10];
|
||||
state[10] = temp;
|
||||
temp = state[6];
|
||||
state[6] = state[14];
|
||||
state[14] = temp;
|
||||
|
||||
// 第3行右移3位
|
||||
temp = state[3];
|
||||
state[3] = state[7];
|
||||
state[7] = state[11];
|
||||
state[11] = state[15];
|
||||
state[15] = temp;
|
||||
}
|
||||
|
||||
// 列混合
|
||||
static void mix_columns(uint8_t *state)
|
||||
{
|
||||
uint8_t temp[16];
|
||||
memcpy(temp, state, 16);
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
int c = i * 4;
|
||||
state[c] = gmul(2, temp[c]) ^ gmul(3, temp[c + 1]) ^ temp[c + 2] ^ temp[c + 3];
|
||||
state[c + 1] = temp[c] ^ gmul(2, temp[c + 1]) ^ gmul(3, temp[c + 2]) ^ temp[c + 3];
|
||||
state[c + 2] = temp[c] ^ temp[c + 1] ^ gmul(2, temp[c + 2]) ^ gmul(3, temp[c + 3]);
|
||||
state[c + 3] = gmul(3, temp[c]) ^ temp[c + 1] ^ temp[c + 2] ^ gmul(2, temp[c + 3]);
|
||||
}
|
||||
}
|
||||
|
||||
// 逆列混合
|
||||
static void inv_mix_columns(uint8_t *state)
|
||||
{
|
||||
uint8_t temp[16];
|
||||
memcpy(temp, state, 16);
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
int c = i * 4;
|
||||
state[c] = gmul(14, temp[c]) ^ gmul(11, temp[c + 1]) ^ gmul(13, temp[c + 2]) ^ gmul(9, temp[c + 3]);
|
||||
state[c + 1] = gmul(9, temp[c]) ^ gmul(14, temp[c + 1]) ^ gmul(11, temp[c + 2]) ^ gmul(13, temp[c + 3]);
|
||||
state[c + 2] = gmul(13, temp[c]) ^ gmul(9, temp[c + 1]) ^ gmul(14, temp[c + 2]) ^ gmul(11, temp[c + 3]);
|
||||
state[c + 3] = gmul(11, temp[c]) ^ gmul(13, temp[c + 1]) ^ gmul(9, temp[c + 2]) ^ gmul(14, temp[c + 3]);
|
||||
}
|
||||
}
|
||||
|
||||
// 轮密钥加
|
||||
static void add_round_key(uint8_t *state, const uint8_t *round_key)
|
||||
{
|
||||
for (int i = 0; i < 16; i++) {
|
||||
state[i] ^= round_key[i];
|
||||
}
|
||||
}
|
||||
|
||||
// AES-128 加密单个块
|
||||
static void aes128_encrypt_block(const uint8_t *input, uint8_t *output, const aes128_context_t *ctx)
|
||||
{
|
||||
uint8_t state[16];
|
||||
|
||||
// 复制输入到状态（列优先顺序）
|
||||
for (int i = 0; i < 4; i++) {
|
||||
for (int j = 0; j < 4; j++) {
|
||||
state[i * 4 + j] = input[j * 4 + i];
|
||||
}
|
||||
}
|
||||
|
||||
// 初始轮密钥加
|
||||
add_round_key(state, ctx->round_key);
|
||||
|
||||
// 9轮加密
|
||||
for (int round = 1; round < 10; round++) {
|
||||
sub_bytes(state);
|
||||
shift_rows(state);
|
||||
mix_columns(state);
|
||||
add_round_key(state, ctx->round_key + round * 16);
|
||||
}
|
||||
|
||||
// 最后一轮（无列混合）
|
||||
sub_bytes(state);
|
||||
shift_rows(state);
|
||||
add_round_key(state, ctx->round_key + 10 * 16);
|
||||
|
||||
// 复制状态到输出（列优先顺序）
|
||||
for (int i = 0; i < 4; i++) {
|
||||
for (int j = 0; j < 4; j++) {
|
||||
output[j * 4 + i] = state[i * 4 + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// AES-128 解密单个块
|
||||
static void aes128_decrypt_block(const uint8_t *input, uint8_t *output, const aes128_context_t *ctx)
|
||||
{
|
||||
uint8_t state[16];
|
||||
|
||||
// 复制输入到状态（列优先顺序）
|
||||
for (int i = 0; i < 4; i++) {
|
||||
for (int j = 0; j < 4; j++) {
|
||||
state[i * 4 + j] = input[j * 4 + i];
|
||||
}
|
||||
}
|
||||
|
||||
// 初始轮密钥加
|
||||
add_round_key(state, ctx->round_key + 10 * 16);
|
||||
|
||||
// 9轮解密
|
||||
for (int round = 9; round >= 1; round--) {
|
||||
inv_shift_rows(state);
|
||||
inv_sub_bytes(state);
|
||||
add_round_key(state, ctx->round_key + round * 16);
|
||||
inv_mix_columns(state);
|
||||
}
|
||||
|
||||
// 最后一轮（无逆列混合）
|
||||
inv_shift_rows(state);
|
||||
inv_sub_bytes(state);
|
||||
add_round_key(state, ctx->round_key);
|
||||
|
||||
// 复制状态到输出（列优先顺序）
|
||||
for (int i = 0; i < 4; i++) {
|
||||
for (int j = 0; j < 4; j++) {
|
||||
output[j * 4 + i] = state[i * 4 + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// CBC 模式加密
|
||||
static void aes128_cbc_encrypt(const uint8_t *input, uint8_t *output, size_t length,
|
||||
const uint8_t *iv, const aes128_context_t *ctx)
|
||||
{
|
||||
uint8_t prev[16];
|
||||
memcpy(prev, iv, 16);
|
||||
|
||||
for (size_t i = 0; i < length; i += 16) {
|
||||
uint8_t block[16];
|
||||
memcpy(block, input + i, 16);
|
||||
|
||||
// XOR with previous ciphertext (or IV for first block)
|
||||
for (int j = 0; j < 16; j++) {
|
||||
block[j] ^= prev[j];
|
||||
}
|
||||
|
||||
aes128_encrypt_block(block, output + i, ctx);
|
||||
memcpy(prev, output + i, 16);
|
||||
}
|
||||
}
|
||||
|
||||
// CBC 模式解密
|
||||
static void aes128_cbc_decrypt(const uint8_t *input, uint8_t *output, size_t length,
|
||||
const uint8_t *iv, const aes128_context_t *ctx)
|
||||
{
|
||||
uint8_t prev[16];
|
||||
memcpy(prev, iv, 16);
|
||||
|
||||
for (size_t i = 0; i < length; i += 16) {
|
||||
aes128_decrypt_block(input + i, output + i, ctx);
|
||||
|
||||
// XOR with previous ciphertext (or IV for first block)
|
||||
for (int j = 0; j < 16; j++) {
|
||||
output[i + j] ^= prev[j];
|
||||
}
|
||||
|
||||
memcpy(prev, input + i, 16);
|
||||
}
|
||||
}
|
||||
|
||||
void protocol_init(void)
|
||||
{
|
||||
if (s_protocol_initialized) return;
|
||||
|
||||
DEBUG_INFO("PROT", "初始化协议层（CRC16、软件AES）...");
|
||||
|
||||
s_protocol_initialized = true;
|
||||
DEBUG_INFO("PROT", "协议层就绪");
|
||||
}
|
||||
|
||||
// CRC16 Modbus（LE，低字节在前）
|
||||
uint16_t crc16_modbus(const uint8_t *data, size_t len)
|
||||
{
|
||||
uint16_t crc = 0xFFFF;
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
crc ^= data[i];
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if (crc & 0x0001) {
|
||||
crc = (crc >> 1) ^ 0xA001;
|
||||
} else {
|
||||
crc >>= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return (crc >> 8) | (crc << 8);
|
||||
}
|
||||
|
||||
// 构建 SPI 帧: SOF + CMD + LEN(2LE) + PAYLOAD + CRC16(2LE)
|
||||
int frame_build(uint8_t cmd, const uint8_t *payload, size_t len, uint8_t *out_frame)
|
||||
{
|
||||
if (len > FRAME_MAX_PAYLOAD) {
|
||||
DEBUG_ERR("PROT", "Payload 太大: %zu > %d", len, FRAME_MAX_PAYLOAD);
|
||||
return -1;
|
||||
}
|
||||
|
||||
size_t idx = 0;
|
||||
out_frame[idx++] = FRAME_SOF;
|
||||
out_frame[idx++] = cmd;
|
||||
out_frame[idx++] = (uint8_t)(len & 0xFF);
|
||||
out_frame[idx++] = (uint8_t)((len >> 8) & 0xFF);
|
||||
|
||||
if (payload && len > 0) {
|
||||
memcpy(&out_frame[idx], payload, len);
|
||||
idx += len;
|
||||
}
|
||||
|
||||
uint16_t crc = crc16_modbus(out_frame, idx);
|
||||
out_frame[idx++] = (uint8_t)(crc & 0xFF);
|
||||
out_frame[idx++] = (uint8_t)((crc >> 8) & 0xFF);
|
||||
|
||||
return idx;
|
||||
}
|
||||
|
||||
// 解析 SPI 帧
|
||||
int frame_parse(const uint8_t *frame, size_t frame_len,
|
||||
uint8_t *out_cmd, uint8_t *out_payload, size_t *out_len)
|
||||
{
|
||||
if (frame_len < 6) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
size_t idx = 0;
|
||||
|
||||
if (frame[idx++] != FRAME_SOF) {
|
||||
DEBUG_WARN("PROT", "无效 SOF: 0x%02X", frame[0]);
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t cmd = frame[idx++];
|
||||
|
||||
size_t payload_len = frame[idx] | (frame[idx + 1] << 8);
|
||||
idx += 2;
|
||||
|
||||
if (payload_len > FRAME_MAX_PAYLOAD) {
|
||||
DEBUG_ERR("PROT", "Payload 长度太大: %zu", payload_len);
|
||||
return -1;
|
||||
}
|
||||
|
||||
size_t expected_len = 1 + 1 + 2 + payload_len + 2;
|
||||
if (frame_len < expected_len) {
|
||||
DEBUG_WARN("PROT", "帧太短: %zu < %zu", frame_len, expected_len);
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint16_t received_crc = frame[idx + payload_len] | (frame[idx + payload_len + 1] << 8);
|
||||
uint16_t calc_crc = crc16_modbus(frame, idx + payload_len);
|
||||
|
||||
if (received_crc != calc_crc) {
|
||||
DEBUG_WARN("PROT", "CRC 不匹配: 0x%04X vs 0x%04X", received_crc, calc_crc);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (out_payload && out_len) {
|
||||
memcpy(out_payload, &frame[idx], payload_len);
|
||||
*out_len = payload_len;
|
||||
}
|
||||
if (out_cmd) {
|
||||
*out_cmd = cmd;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// AES-128 CBC 加密
|
||||
// 注意: 函数名带 protocol_ 前缀以避免与 ESP-IDF wpa_supplicant 组件冲突
|
||||
int protocol_aes_encrypt(const uint8_t *plaintext, size_t len,
|
||||
const uint8_t *key, const uint8_t *iv,
|
||||
uint8_t *ciphertext_out)
|
||||
{
|
||||
size_t padded_len = ((len + 15) / 16) * 16;
|
||||
uint8_t padded[256];
|
||||
|
||||
memcpy(padded, plaintext, len);
|
||||
uint8_t pad = (uint8_t)(16 - (len % 16));
|
||||
memset(&padded[len], pad, pad);
|
||||
|
||||
aes128_context_t ctx;
|
||||
aes128_key_expansion(key, &ctx);
|
||||
aes128_cbc_encrypt(padded, ciphertext_out, padded_len, iv, &ctx);
|
||||
|
||||
return padded_len;
|
||||
}
|
||||
|
||||
// AES-128 CBC 解密
|
||||
// 注意: 函数名带 protocol_ 前缀以避免与 ESP-IDF wpa_supplicant 组件冲突
|
||||
int protocol_aes_decrypt(const uint8_t *ciphertext, size_t len,
|
||||
const uint8_t *key, const uint8_t *iv,
|
||||
uint8_t *plaintext_out)
|
||||
{
|
||||
if (len % 16 != 0) {
|
||||
DEBUG_ERR("PROT", "Ciphertext 长度不是 16 的倍数: %zu", len);
|
||||
return -1;
|
||||
}
|
||||
|
||||
aes128_context_t ctx;
|
||||
aes128_key_expansion(key, &ctx);
|
||||
aes128_cbc_decrypt(ciphertext, plaintext_out, len, iv, &ctx);
|
||||
|
||||
// 移除 PKCS#7 填充
|
||||
uint8_t pad = plaintext_out[len - 1];
|
||||
if (pad > 16) return -1;
|
||||
return len - pad;
|
||||
}
|
||||
|
||||
// 搅伴算法 - 生成随机数
|
||||
void scramble_generate(uint32_t timestamp, uint32_t mem_addr,
|
||||
uint32_t rand_seed, uint8_t *nonce_out)
|
||||
{
|
||||
uint8_t state[16];
|
||||
|
||||
memcpy(&state[0], ×tamp, 4);
|
||||
memcpy(&state[4], &mem_addr, 4);
|
||||
memcpy(&state[8], &rand_seed, 4);
|
||||
memcpy(&state[12], SCRAMBLE_CONST, 4);
|
||||
|
||||
for (int round = 0; round < 3; round++) {
|
||||
for (int i = 0; i < 16; i++) {
|
||||
state[i] ^= SCRAMBLE_CONST[i];
|
||||
state[i] = (state[i] << 1) | (state[i] >> 7);
|
||||
}
|
||||
}
|
||||
|
||||
for (int round = 0; round < 2; round++) {
|
||||
uint32_t *words = (uint32_t *)state;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
words[i] ^= SCRAMBLE_CONST[i * 4 % 16] | ((uint32_t)SCRAMBLE_CONST[i * 4 + 1] << 8);
|
||||
}
|
||||
}
|
||||
|
||||
memcpy(nonce_out, state, 16);
|
||||
}
|
||||
|
||||
// 会话密钥初始化
|
||||
int session_key_init(session_ctx_t *sess, const uint8_t *k_session)
|
||||
{
|
||||
if (!sess || !k_session) return -1;
|
||||
|
||||
memset(sess, 0, sizeof(session_ctx_t));
|
||||
memcpy(sess->key, k_session, 16);
|
||||
memset(sess->iv, 0, 16);
|
||||
sess->used = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void session_key_clear(session_ctx_t *sess)
|
||||
{
|
||||
if (sess) {
|
||||
memset(sess->key, 0, 16);
|
||||
memset(sess->iv, 0, 16);
|
||||
sess->used = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int session_encrypt(session_ctx_t *sess, const uint8_t *in, size_t len, uint8_t *out)
|
||||
{
|
||||
if (!sess || !in || !out) return -1;
|
||||
|
||||
uint8_t iv_copy[16];
|
||||
memcpy(iv_copy, sess->iv, 16);
|
||||
|
||||
int ret = protocol_aes_encrypt(in, len, sess->key, iv_copy, out);
|
||||
|
||||
if (ret > 0) {
|
||||
memcpy(sess->iv, &out[ret - 16], 16);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int session_decrypt(session_ctx_t *sess, const uint8_t *in, size_t len, uint8_t *out)
|
||||
{
|
||||
if (!sess || !in || !out) return -1;
|
||||
|
||||
uint8_t iv_copy[16];
|
||||
memcpy(iv_copy, sess->iv, 16);
|
||||
|
||||
int ret = protocol_aes_decrypt(in, len, sess->key, iv_copy, out);
|
||||
|
||||
if (ret > 0) {
|
||||
memcpy(sess->iv, &in[len - 16], 16);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
// 使用 ESP-IDF 硬件 RNG 生成随机数
|
||||
void generate_random_nonce(uint8_t *nonce, size_t len)
|
||||
{
|
||||
esp_fill_random(nonce, len);
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
* protocol.h - 帧格式、CRC16、AES 加密解密
|
||||
* T2.2, T1.3
|
||||
* ESP-IDF 6.0 适配版本 - 使用软件 AES 实现 + 硬件 RNG
|
||||
*/
|
||||
#ifndef PROTOCOL_H
|
||||
#define PROTOCOL_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "app_config.h"
|
||||
|
||||
// 帧格式: SOF(1B) + CMD(1B) + LEN(2B LE) + PAYLOAD(LEN) + CRC16(2B LE)
|
||||
#define FRAME_SOF (0xEB) // 帧起始标记
|
||||
#define FRAME_MAX_PAYLOAD (64) // 安全最大 payload
|
||||
#define FRAME_OVERHEAD (6) // SOF + CMD + LEN(2) + CRC16(2)
|
||||
|
||||
// 命令码（对应 GD32 OTA 协议）
|
||||
#define CMD_HANDSHAKE_REQ (0xF0) // 握手请求
|
||||
#define CMD_CHALLENGE (0xF1) // 挑战/随机数
|
||||
#define CMD_SESSION_KEY (0xF2) // 会话密钥
|
||||
#define CMD_SEND_SIZE (0xF3) // 发送固件大小
|
||||
#define CMD_DATA_PACKET (0xF4) // 数据包
|
||||
#define CMD_DATA_END (0xF5) // 数据结束
|
||||
#define CMD_TRANSFER_DONE (0xF6) // 传输完成
|
||||
#define CMD_FINISH_OTA (0xF7) // 结束 OTA
|
||||
#define CMD_ACK (0x80) // 通用 ACK
|
||||
#define CMD_ERROR (0x81) // 通用错误
|
||||
|
||||
// ACK 状态码
|
||||
#define ACK_OK (0x00) // 成功
|
||||
#define ACK_UNSUPPORTED (0x01) // 命令不支持
|
||||
#define ACK_AUTH_FAILED (0x02) // 认证失败
|
||||
#define ACK_SIZE_OOR (0x03) // 大小超出范围
|
||||
#define ACK_CRC_FAILED (0x04) // CRC 校验失败
|
||||
#define ACK_RETRY_EXCEED (0x05) // 重试次数超限
|
||||
#define ACK_SESSION_INVALID (0x06) // 会话密钥无效
|
||||
#define ACK_FLASH_FAILED (0x07) // Flash 写入失败
|
||||
|
||||
// 安全常量（定义在 app_config.h 中）
|
||||
inline constexpr size_t MASTER_KEY_HERE_LEN = 16;
|
||||
inline constexpr size_t SCRAMBLE_CONST_HERE_LEN = 16;
|
||||
inline constexpr size_t CHIP_UID_LEN = 12;
|
||||
|
||||
// AES 会话上下文
|
||||
typedef struct {
|
||||
uint8_t key[16]; // 会话密钥 K_session
|
||||
uint8_t iv[16]; // CBC 模式 IV
|
||||
int used;
|
||||
} session_ctx_t;
|
||||
|
||||
// 初始化协议层
|
||||
void protocol_init(void);
|
||||
|
||||
// CRC16 Modbus 计算（低字节在前，LE）
|
||||
uint16_t crc16_modbus(const uint8_t *data, size_t len);
|
||||
|
||||
// 构建帧: 返回总帧长度
|
||||
// 输出格式: SOF + CMD + LEN(2) + PAYLOAD + CRC16(2)
|
||||
int frame_build(uint8_t cmd, const uint8_t *payload, size_t len, uint8_t *out_frame);
|
||||
|
||||
// 解析帧: 返回 payload 起始偏移，失败返回 -1
|
||||
// 检查 SOF，验证 CRC16，提取 CMD 和 LEN
|
||||
int frame_parse(const uint8_t *frame, size_t frame_len,
|
||||
uint8_t *out_cmd, uint8_t *out_payload, size_t *out_len);
|
||||
|
||||
// T2.2: AES-128 CBC 加密
|
||||
// 使用预设主密钥或会话密钥
|
||||
// 注意: 函数名带 protocol_ 前缀以避免与 ESP-IDF wpa_supplicant 组件冲突
|
||||
int protocol_aes_encrypt(const uint8_t *plaintext, size_t len,
|
||||
const uint8_t *key, const uint8_t *iv,
|
||||
uint8_t *ciphertext_out);
|
||||
|
||||
int protocol_aes_decrypt(const uint8_t *ciphertext, size_t len,
|
||||
const uint8_t *key, const uint8_t *iv,
|
||||
uint8_t *plaintext_out);
|
||||
|
||||
// T2.1: 搅伴算法 - 生成随机数
|
||||
// 输入: 时间戳 + 内存地址 + rand_seed
|
||||
// 输出: 16 字节 nonce_server
|
||||
void scramble_generate(uint32_t timestamp, uint32_t mem_addr,
|
||||
uint32_t rand_seed, uint8_t *nonce_out);
|
||||
|
||||
// T2.4: 会话密钥管理
|
||||
int session_key_init(session_ctx_t *sess, const uint8_t *k_session);
|
||||
void session_key_clear(session_ctx_t *sess);
|
||||
int session_encrypt(session_ctx_t *sess, const uint8_t *in, size_t len, uint8_t *out);
|
||||
int session_decrypt(session_ctx_t *sess, const uint8_t *in, size_t len, uint8_t *out);
|
||||
|
||||
// 使用 ESP-IDF 硬件 RNG 生成随机数
|
||||
void generate_random_nonce(uint8_t *nonce, size_t len);
|
||||
|
||||
#endif // PROTOCOL_H
|
||||
@@ -0,0 +1,124 @@
|
||||
/*
|
||||
* spi_master.c - SPI Master 驱动实现
|
||||
* T1.1 - T1.4
|
||||
* ESP-IDF 6.0 适配版本
|
||||
*/
|
||||
#include <string.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "driver/spi_master.h"
|
||||
#include "soc/spi_struct.h"
|
||||
|
||||
#include "spi_master.h"
|
||||
#include "debug_log.h"
|
||||
|
||||
spi_device_handle_t g_spi_device = NULL;
|
||||
|
||||
// SPI2 总线初始化
|
||||
esp_err_t spi_master_init(void)
|
||||
{
|
||||
DEBUG_INFO("SPI", "初始化 SPI2 (VSPI)...");
|
||||
|
||||
spi_bus_config_t bus_cfg = {};
|
||||
bus_cfg.mosi_io_num = 8;
|
||||
bus_cfg.miso_io_num = 7;
|
||||
bus_cfg.sclk_io_num = 6;
|
||||
bus_cfg.quadwp_io_num = -1;
|
||||
bus_cfg.quadhd_io_num = -1;
|
||||
bus_cfg.max_transfer_sz = 256;
|
||||
|
||||
esp_err_t ret = spi_bus_initialize(SPI2_HOST, &bus_cfg, SPI_DMA_CH_AUTO);
|
||||
if (ret != ESP_OK) {
|
||||
DEBUG_ERR("SPI", "SPI 总线初始化失败: %s", esp_err_to_name(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
spi_device_interface_config_t dev_cfg = {};
|
||||
dev_cfg.clock_speed_hz = SPI_CLOCK_HZ;
|
||||
dev_cfg.mode = SPI_MODE;
|
||||
dev_cfg.spics_io_num = SPI_MASTER_CS_GPIO;
|
||||
dev_cfg.queue_size = SPI_QUEUE_SIZE;
|
||||
dev_cfg.command_bits = 0;
|
||||
dev_cfg.address_bits = 0;
|
||||
dev_cfg.dummy_bits = 0;
|
||||
dev_cfg.cs_ena_posttrans = 0;
|
||||
dev_cfg.cs_ena_pretrans = 0;
|
||||
|
||||
ret = spi_bus_add_device(SPI2_HOST, &dev_cfg, &g_spi_device);
|
||||
if (ret != ESP_OK) {
|
||||
DEBUG_ERR("SPI", "添加 SPI 设备失败: %s", esp_err_to_name(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
DEBUG_INFO("SPI", "SPI2 已初始化: CLK=40MHz, CS=GPIO4");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// 带超时的同步 SPI 传输
|
||||
// ESP-IDF 6.0: spi_device_transmit() 不再接受超时参数
|
||||
// 使用 spi_device_queue_trans() + spi_device_get_trans_result() 实现超时
|
||||
esp_err_t spi_sync_transfer(uint8_t cmd, const uint8_t *payload,
|
||||
size_t payload_len, uint8_t *recv_buf,
|
||||
size_t recv_len, uint32_t timeout_ms)
|
||||
{
|
||||
if (g_spi_device == NULL) {
|
||||
DEBUG_ERR("SPI", "SPI 设备未初始化");
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
spi_transaction_t t = {};
|
||||
t.cmd = cmd;
|
||||
t.length = payload_len * 8;
|
||||
t.rxlength = recv_len * 8;
|
||||
t.tx_buffer = payload;
|
||||
t.rx_buffer = recv_buf;
|
||||
t.user = (void *)(uintptr_t)cmd;
|
||||
|
||||
/* ESP-IDF 6.0: 使用 spi_device_transmit() 无超时参数
|
||||
* 如需超时控制，使用 spi_device_queue_trans() + spi_device_get_trans_result()
|
||||
*/
|
||||
esp_err_t ret = spi_device_transmit(g_spi_device, &t);
|
||||
if (ret != ESP_OK) {
|
||||
DEBUG_WARN("SPI", "SPI 发送 cmd=0x%02X 失败: %s", cmd, esp_err_to_name(ret));
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
// 仅发送
|
||||
esp_err_t spi_send(uint8_t cmd, const uint8_t *payload, size_t len)
|
||||
{
|
||||
return spi_sync_transfer(cmd, payload, len, NULL, 0, 3000);
|
||||
}
|
||||
|
||||
// 仅接收
|
||||
esp_err_t spi_recv(uint8_t cmd, uint8_t *recv_buf, size_t len, uint32_t timeout_ms)
|
||||
{
|
||||
uint8_t dummy[64] = {0};
|
||||
memset(dummy, 0xFF, sizeof(dummy));
|
||||
return spi_sync_transfer(cmd, dummy, len, recv_buf, len, timeout_ms);
|
||||
}
|
||||
|
||||
// 带重试的 SPI 传输
|
||||
esp_err_t spi_transfer_with_retry(uint8_t cmd, const uint8_t *payload,
|
||||
size_t payload_len, uint8_t *recv_buf,
|
||||
size_t recv_len, int max_retries)
|
||||
{
|
||||
esp_err_t ret;
|
||||
int retry_count = 0;
|
||||
|
||||
while (retry_count < max_retries) {
|
||||
ret = spi_sync_transfer(cmd, payload, payload_len,
|
||||
recv_buf, recv_len, 3000);
|
||||
if (ret == ESP_OK) {
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
retry_count++;
|
||||
DEBUG_WARN("SPI", "SPI 重试 %d/%d, cmd=0x%02X", retry_count, max_retries, cmd);
|
||||
vTaskDelay(pdMS_TO_TICKS(100));
|
||||
}
|
||||
|
||||
DEBUG_ERR("SPI", "SPI 传输 %d 次重试后失败", max_retries);
|
||||
return ESP_ERR_TIMEOUT;
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* spi_master.h - SPI Master 驱动头文件
|
||||
*/
|
||||
#ifndef SPI_MASTER_H
|
||||
#define SPI_MASTER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "driver/spi_master.h"
|
||||
|
||||
// SPI 配置常量
|
||||
#define SPI_MASTER_CS_GPIO (4) // CS 片选引脚
|
||||
#define SPI_CLOCK_HZ (40 * 1000 * 1000) // 40MHz
|
||||
#define SPI_MODE (0) // CPOL=0, CPHA=0
|
||||
#define SPI_QUEUE_SIZE (1)
|
||||
|
||||
// SPI 设备句柄（外部引用）
|
||||
extern spi_device_handle_t g_spi_device;
|
||||
|
||||
// 初始化 SPI2 (VSPI) 总线和设备
|
||||
esp_err_t spi_master_init(void);
|
||||
|
||||
// 同步 SPI 传输（阻塞式）
|
||||
// 发送 cmd + payload，接收响应到 recv_buf
|
||||
// 返回: ESP_OK 成功, ESP_ERR_TIMEOUT 超时
|
||||
esp_err_t spi_sync_transfer(uint8_t cmd, const uint8_t *payload,
|
||||
size_t payload_len, uint8_t *recv_buf,
|
||||
size_t recv_len, uint32_t timeout_ms);
|
||||
|
||||
// 仅发送（不期望接收）
|
||||
esp_err_t spi_send(uint8_t cmd, const uint8_t *payload, size_t len);
|
||||
|
||||
// 仅接收（发送哑数据）
|
||||
esp_err_t spi_recv(uint8_t cmd, uint8_t *recv_buf, size_t len, uint32_t timeout_ms);
|
||||
|
||||
#endif // SPI_MASTER_H
|
||||
Reference in New Issue
Block a user