@@ -1,349 +1,25 @@
/*
* protocol.c - 协议层实现
* T2.1, T2.2, T1.3
* ESP-IDF 6.0 适配版本 - 使用软 件 AES 实现 + 硬件 RNG
* ESP-IDF 6.0 适配版本 - 使用硬 件 AES-128-CBC + 硬件 RNG
*
* 加密实现使用 ESP32-C3 硬件 AES 加速器(通过 mbedtls port)
*/
# include <string.h>
# include <stdio.h>
# include "esp_random.h"
# include "aes/esp_aes.h"
# include "app_config.h"
# include "protocol.h"
# include "monitor/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) ... " ) ;
DEBUG_INFO ( " PROT " , " 初始化协议层(CRC16、硬 件AES-CBC ) ... " ) ;
s_protocol_initialized = true ;
DEBUG_INFO ( " PROT " , " 协议层就绪 " ) ;
@@ -442,44 +118,94 @@ int frame_parse(const uint8_t *frame, size_t frame_len,
return 0 ;
}
// AES-128 CBC 加密
// 注意: 函数名带 protocol_ 前缀以避免与 ESP-IDF wpa_supplicant 组件冲突
// AES-128 CBC 加密(使用 ESP32-C3 硬件 AES)
int protocol_aes_encrypt ( const uint8_t * plaintext , size_t len ,
const uint8_t * key , const uint8_t * iv ,
uint8_t * ciphertext_out )
{
if ( ! plaintext | | ! key | | ! iv | | ! ciphertext_out ) {
return - 1 ;
}
// PKCS#7 填充
size_t padded_len = ( ( len + 15 ) / 16 ) * 16 ;
uint8_t padded [ 256 ] ;
memset ( padded , 0 , sizeof ( padded ) ) ;
memcpy ( padded , plaintext , len ) ;
uint8_t pad = ( uint8_t ) ( 16 - ( len % 16 ) ) ;
if ( pad = = 0 ) pad = 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 ) ;
// 硬件 AES-CBC 加密(使用 ESP32-C3 硬件 AES 加速器)
esp_aes_context aes_ctx ;
esp_aes_init ( & aes_ctx ) ;
return padded_len ;
int ret = esp_aes_setkey ( & aes_ctx , key , 128 ) ;
if ( ret ! = 0 ) {
DEBUG_ERR ( " PROT " , " esp_aes_setkey 失败: %d " , ret ) ;
esp_aes_free ( & aes_ctx ) ;
return - 1 ;
}
uint8_t iv_copy [ 16 ] ;
memcpy ( iv_copy , iv , 16 ) ;
ret = esp_aes_crypt_cbc ( & aes_ctx , ESP_AES_ENCRYPT , padded_len , iv_copy , padded , ciphertext_out ) ;
esp_aes_free ( & aes_ctx ) ;
if ( ret = = 0 ) {
return padded_len ;
} else {
DEBUG_ERR ( " PROT " , " esp_aes_crypt_cbc 加密失败: %d " , ret ) ;
return - 1 ;
}
}
// AES-128 CBC 解密
// 注意: 函数名带 protocol_ 前缀以避免与 ESP-IDF wpa_supplicant 组件冲突
// AES-128 CBC 解密(使用 ESP32-C3 硬件 AES)
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 ) ;
if ( ! ciphertext | | ! key | | ! iv | | ! plaintext_out ) {
return - 1 ;
}
aes128_context_t ctx ;
aes128_key_expansion ( key , & ctx ) ;
aes128_cbc_decrypt ( ciphertext , plaintext_out , len , iv , & ctx ) ;
if ( len % 16 ! = 0 | | len = = 0 ) {
DEBUG_ERR ( " PROT " , " Ciphertext 长度无效: %zu " , len ) ;
return - 1 ;
}
// 硬件 AES-CBC 解密(使用 ESP32-C3 硬件 AES 加速器)
esp_aes_context aes_ctx ;
esp_aes_init ( & aes_ctx ) ;
int ret = esp_aes_setkey ( & aes_ctx , key , 128 ) ;
if ( ret ! = 0 ) {
DEBUG_ERR ( " PROT " , " esp_aes_setkey 失败: %d " , ret ) ;
esp_aes_free ( & aes_ctx ) ;
return - 1 ;
}
uint8_t iv_copy [ 16 ] ;
memcpy ( iv_copy , iv , 16 ) ;
ret = esp_aes_crypt_cbc ( & aes_ctx , ESP_AES_DECRYPT , len , iv_copy , ciphertext , plaintext_out ) ;
esp_aes_free ( & aes_ctx ) ;
if ( ret ! = 0 ) {
DEBUG_ERR ( " PROT " , " esp_aes_crypt_cbc 解密失败: %d " , ret ) ;
return - 1 ;
}
// 移除 PKCS#7 填充
uint8_t pad = plaintext_out [ len - 1 ] ;
if ( pad > 16 ) return - 1 ;
if ( pad > 16 ) {
DEBUG_ERR ( " PROT " , " 无效填充: %d " , pad ) ;
return - 1 ;
}
return len - pad ;
}