diff --git a/src/esp32/ESP32CryptoEngine.cpp b/src/esp32/ESP32CryptoEngine.cpp index d7eaa46bf..e80d59661 100644 --- a/src/esp32/ESP32CryptoEngine.cpp +++ b/src/esp32/ESP32CryptoEngine.cpp @@ -49,12 +49,13 @@ class ESP32CryptoEngine : public CryptoEngine */ virtual void encrypt(uint32_t fromNode, uint64_t packetId, size_t numBytes, uint8_t *bytes) override { + hexDump("before", bytes, numBytes, 16); if (key.length > 0) { uint8_t stream_block[16]; static uint8_t scratch[MAX_BLOCKSIZE]; size_t nc_off = 0; - // DEBUG_MSG("ESP32 crypt fr=%x, num=%x, numBytes=%d!\n", fromNode, (uint32_t) packetId, numBytes); + DEBUG_MSG("ESP32 crypt fr=%x, num=%x, numBytes=%d!\n", fromNode, (uint32_t) packetId, numBytes); initNonce(fromNode, packetId); assert(numBytes <= MAX_BLOCKSIZE); memcpy(scratch, bytes, numBytes); @@ -64,12 +65,11 @@ class ESP32CryptoEngine : public CryptoEngine auto res = mbedtls_aes_crypt_ctr(&aes, numBytes, &nc_off, nonce, stream_block, scratch, bytes); assert(!res); } + hexDump("after", bytes, numBytes, 16); } virtual void decrypt(uint32_t fromNode, uint64_t packetId, size_t numBytes, uint8_t *bytes) override { - // DEBUG_MSG("ESP32 decrypt!\n"); - // For CTR, the implementation is the same encrypt(fromNode, packetId, numBytes, bytes); } diff --git a/src/mesh/CryptoEngine.cpp b/src/mesh/CryptoEngine.cpp index 971080b23..8a5ea795d 100644 --- a/src/mesh/CryptoEngine.cpp +++ b/src/mesh/CryptoEngine.cpp @@ -3,7 +3,7 @@ void CryptoEngine::setKey(const CryptoKey &k) { - DEBUG_MSG("Installing AES%d key!\n", k.length * 8); + DEBUG_MSG("Using AES%d key!\n", k.length * 8); /* for(uint8_t i = 0; i < k.length; i++) DEBUG_MSG("%02x ", k.bytes[i]); DEBUG_MSG("\n"); */ @@ -26,6 +26,78 @@ void CryptoEngine::decrypt(uint32_t fromNode, uint64_t packetId, size_t numBytes DEBUG_MSG("WARNING: noop decryption!\n"); } +// Usage: +// hexDump(desc, addr, len, perLine); +// desc: if non-NULL, printed as a description before hex dump. +// addr: the address to start dumping from. +// len: the number of bytes to dump. +// perLine: number of bytes on each output line. + +void CryptoEngine::hexDump (const char * desc, const void * addr, const int len, int perLine) +{ + // Silently ignore silly per-line values. + + if (perLine < 4 || perLine > 64) perLine = 16; + + int i; + unsigned char buff[perLine+1]; + const unsigned char * pc = (const unsigned char *)addr; + + // Output description if given. + + if (desc != NULL) DEBUG_MSG ("%s:\n", desc); + + // Length checks. + + if (len == 0) { + DEBUG_MSG(" ZERO LENGTH\n"); + return; + } + if (len < 0) { + DEBUG_MSG(" NEGATIVE LENGTH: %d\n", len); + return; + } + + // Process every byte in the data. + + for (i = 0; i < len; i++) { + // Multiple of perLine means new or first line (with line offset). + + if ((i % perLine) == 0) { + // Only print previous-line ASCII buffer for lines beyond first. + + if (i != 0) DEBUG_MSG (" %s\n", buff); + + // Output the offset of current line. + + DEBUG_MSG (" %04x ", i); + } + + // Now the hex code for the specific character. + + DEBUG_MSG (" %02x", pc[i]); + + // And buffer a printable ASCII character for later. + + if ((pc[i] < 0x20) || (pc[i] > 0x7e)) // isprint() may be better. + buff[i % perLine] = '.'; + else + buff[i % perLine] = pc[i]; + buff[(i % perLine) + 1] = '\0'; + } + + // Pad out last line if not exactly perLine characters. + + while ((i % perLine) != 0) { + DEBUG_MSG (" "); + i++; + } + + // And print the final ASCII buffer. + + DEBUG_MSG (" %s\n", buff); +} + /** * Init our 128 bit nonce for a new packet */ diff --git a/src/mesh/CryptoEngine.h b/src/mesh/CryptoEngine.h index 1dda7ce31..39b30a727 100644 --- a/src/mesh/CryptoEngine.h +++ b/src/mesh/CryptoEngine.h @@ -56,6 +56,8 @@ class CryptoEngine * a 32 bit block counter (starts at zero) */ void initNonce(uint32_t fromNode, uint64_t packetId); + + void hexDump(const char * desc, const void * addr, const int len, int perLine); }; extern CryptoEngine *crypto; diff --git a/src/nrf52/NRF52CryptoEngine.cpp b/src/nrf52/NRF52CryptoEngine.cpp index b508ed554..287defdda 100644 --- a/src/nrf52/NRF52CryptoEngine.cpp +++ b/src/nrf52/NRF52CryptoEngine.cpp @@ -16,48 +16,54 @@ class NRF52CryptoEngine : public CryptoEngine */ virtual void encrypt(uint32_t fromNode, uint64_t packetId, size_t numBytes, uint8_t *bytes) override { -// DEBUG_MSG("NRF52 encrypt!\n"); - + hexDump("before", bytes, numBytes, 16); if (key.length > 16) { + DEBUG_MSG("Software encrypt fr=%x, num=%x, numBytes=%d!\n", fromNode, (uint32_t) packetId, numBytes); AES_ctx ctx; initNonce(fromNode, packetId); AES_init_ctx_iv(&ctx, key.bytes, nonce); AES_CTR_xcrypt_buffer(&ctx, bytes, numBytes); } else if (key.length > 0) { + DEBUG_MSG("nRF52 encrypt fr=%x, num=%x, numBytes=%d!\n", fromNode, (uint32_t) packetId, numBytes); nRFCrypto.begin(); nRFCrypto_AES ctx; uint8_t myLen = ctx.blockLen(numBytes); + DEBUG_MSG("nRF52 encBuf myLen=%d!\n", myLen); char encBuf[myLen] = {0}; - memcpy(encBuf, bytes, numBytes); initNonce(fromNode, packetId); ctx.begin(); - ctx.Process(encBuf, numBytes, nonce, key.bytes, key.length, (char*)bytes, ctx.encryptFlag, ctx.ctrMode); + ctx.Process((char*)bytes, numBytes, nonce, key.bytes, key.length, encBuf, ctx.encryptFlag, ctx.ctrMode); ctx.end(); nRFCrypto.end(); + memcpy(bytes, encBuf, numBytes); } + hexDump("after", bytes, numBytes, 16); } virtual void decrypt(uint32_t fromNode, uint64_t packetId, size_t numBytes, uint8_t *bytes) override { -// DEBUG_MSG("NRF52 decrypt!\n"); - + hexDump("before", bytes, numBytes, 16); if (key.length > 16) { + DEBUG_MSG("Software decrypt fr=%x, num=%x, numBytes=%d!\n", fromNode, (uint32_t) packetId, numBytes); AES_ctx ctx; initNonce(fromNode, packetId); AES_init_ctx_iv(&ctx, key.bytes, nonce); AES_CTR_xcrypt_buffer(&ctx, bytes, numBytes); } else if (key.length > 0) { + DEBUG_MSG("nRF52 decrypt fr=%x, num=%x, numBytes=%d!\n", fromNode, (uint32_t) packetId, numBytes); nRFCrypto.begin(); nRFCrypto_AES ctx; uint8_t myLen = ctx.blockLen(numBytes); + DEBUG_MSG("nRF52 decBuf myLen=%d!\n", myLen); char decBuf[myLen] = {0}; - memcpy(decBuf, bytes, numBytes); initNonce(fromNode, packetId); ctx.begin(); - ctx.Process(decBuf, numBytes, nonce, key.bytes, key.length, (char*)bytes, ctx.decryptFlag, ctx.ctrMode); + ctx.Process((char*)bytes, numBytes, nonce, key.bytes, key.length, decBuf, ctx.decryptFlag, ctx.ctrMode); ctx.end(); nRFCrypto.end(); + memcpy(bytes, decBuf, numBytes); } + hexDump("after", bytes, numBytes, 16); } private: