mirror of https://github.com/arendst/Tasmota.git
120 lines
3.3 KiB
C++
120 lines
3.3 KiB
C++
|
|
#include "GcmParser.h"
|
|
#include "tasmota_options.h"
|
|
|
|
#ifdef USE_TLS
|
|
#include "lwip/def.h"
|
|
#include <t_bearssl.h>
|
|
|
|
GCMParser::GCMParser(uint8_t *encryption_key, uint8_t *authentication_key) {
|
|
memcpy(this->encryption_key, encryption_key, 16);
|
|
memcpy(this->authentication_key, authentication_key, 16);
|
|
use_auth = 0;
|
|
for (uint16_t cnt = 0; cnt < 16; cnt++) {
|
|
if (authentication_key[cnt]) {
|
|
use_auth |= 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
int8_t GCMParser::parse(uint8_t *d, DataParserContext &ctx) {
|
|
if(ctx.length < 12) return DATA_PARSE_INCOMPLETE;
|
|
|
|
uint8_t* ptr = (uint8_t*) d;
|
|
if(*ptr != GCM_TAG) return DATA_PARSE_BOUNDRY_FLAG_MISSING;
|
|
ptr++;
|
|
// Encrypted APDU
|
|
// http://www.weigu.lu/tutorials/sensors2bus/04_encryption/index.html
|
|
|
|
uint8_t systemTitleLength = *ptr;
|
|
ptr++;
|
|
|
|
uint8_t initialization_vector[12];
|
|
memcpy(ctx.system_title, ptr, systemTitleLength);
|
|
memcpy(initialization_vector, ctx.system_title, systemTitleLength);
|
|
|
|
int len = 0;
|
|
int headersize = 2 + systemTitleLength;
|
|
ptr += systemTitleLength;
|
|
|
|
if (ctx.flags & 1) {
|
|
len = *ptr;
|
|
ptr++;
|
|
headersize++;
|
|
} else {
|
|
if(((*ptr) & 0xFF) == 0x81) {
|
|
ptr++;
|
|
len = *ptr;
|
|
// 1-byte payload length
|
|
ptr++;
|
|
headersize += 2;
|
|
} else if(((*ptr) & 0xFF) == 0x82) {
|
|
GCMSizeDef* h = (GCMSizeDef*) ptr;
|
|
|
|
// 2-byte payload length
|
|
len = (ntohs(h->format) & 0xFFFF);
|
|
|
|
ptr += 3;
|
|
headersize += 3;
|
|
} else if(((*ptr) & 0xFF) == 0x4f) {
|
|
// ???????? single frame did only decode with this compare
|
|
ptr++;
|
|
headersize++;
|
|
} else if(((*ptr) & 0xFF) == 0x5e) {
|
|
// ???????? single frame did only decode with this compare
|
|
ptr++;
|
|
headersize++;
|
|
} else {
|
|
len = *ptr;
|
|
ptr++;
|
|
headersize++;
|
|
}
|
|
}
|
|
if(len + headersize > ctx.length)
|
|
return DATA_PARSE_INCOMPLETE;
|
|
|
|
uint8_t additional_authenticated_data[17];
|
|
memcpy(additional_authenticated_data, ptr, 1);
|
|
|
|
// Security tag
|
|
uint8_t sec = *ptr;
|
|
ptr++;
|
|
headersize++;
|
|
|
|
// Frame counter
|
|
memcpy(initialization_vector + 8, ptr, 4);
|
|
ptr += 4;
|
|
headersize += 4;
|
|
|
|
int footersize = 0;
|
|
|
|
// Authentication enabled
|
|
uint8_t authentication_tag[12];
|
|
uint8_t authkeylen = 0, aadlen = 0;
|
|
if((sec & 0x10) == 0x10) {
|
|
authkeylen = 12;
|
|
aadlen = 17;
|
|
footersize += authkeylen;
|
|
memcpy(additional_authenticated_data + 1, authentication_key, 16);
|
|
memcpy(authentication_tag, ptr + len - footersize - 5, authkeylen);
|
|
}
|
|
|
|
br_gcm_context gcm_ctx;
|
|
br_aes_small_ctr_keys ctr_ctx;
|
|
br_aes_small_ctr_init(&ctr_ctx, encryption_key, 16);
|
|
br_gcm_init(&gcm_ctx, &ctr_ctx.vtable, &br_ghash_ctmul32);
|
|
br_gcm_reset(&gcm_ctx, initialization_vector, 12);
|
|
if (use_auth && authkeylen > 0) {
|
|
br_gcm_aad_inject(&gcm_ctx, additional_authenticated_data, aadlen);
|
|
}
|
|
br_gcm_flip(&gcm_ctx);
|
|
br_gcm_run(&gcm_ctx, 0, ptr , ctx.length - headersize);
|
|
if (use_auth && authkeylen > 0 && br_gcm_check_tag_trunc(&gcm_ctx, authentication_tag, authkeylen) != 1) {
|
|
return GCM_AUTH_FAILED;
|
|
}
|
|
|
|
ctx.length -= footersize + headersize;
|
|
return ptr - d;
|
|
}
|
|
#endif // USE_TLS
|