Tasmota/lib/lib_div/ams/GcmParser.cpp

105 lines
2.9 KiB
C++
Raw Normal View History

#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(((*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++;
}
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