From b37cd045d5f3f4169255e147229d6eba8ece47f9 Mon Sep 17 00:00:00 2001 From: matzman666 Date: Fri, 16 Mar 2018 22:10:19 +0100 Subject: [PATCH] Added XTEA and Pearson Hashing. Improved message encryption for RF24. --- MyConfig.h | 2 +- core/MyMessage.h | 16 +++++ core/MySensorsCore.cpp | 14 ++++- core/MySensorsCore.h | 4 ++ core/MyTransport.cpp | 33 +++++++++-- drivers/CRC16/CRC16.cpp | 16 +++++ drivers/CRC16/CRC16.h | 15 +++++ drivers/PearsonHash/PearsonHash.cpp | 27 +++++++++ drivers/PearsonHash/PearsonHash.h | 17 ++++++ drivers/XTEA/XTEA.cpp | 82 ++++++++++++++++++++++++++ drivers/XTEA/XTEA.h | 29 +++++++++ hal/transport/RF24/MyTransportRF24.cpp | 22 +++++++ 12 files changed, 270 insertions(+), 7 deletions(-) create mode 100644 drivers/CRC16/CRC16.cpp create mode 100644 drivers/CRC16/CRC16.h create mode 100644 drivers/PearsonHash/PearsonHash.cpp create mode 100644 drivers/PearsonHash/PearsonHash.h create mode 100644 drivers/XTEA/XTEA.cpp create mode 100644 drivers/XTEA/XTEA.h diff --git a/MyConfig.h b/MyConfig.h index 5c20987..e7fde19 100644 --- a/MyConfig.h +++ b/MyConfig.h @@ -1869,7 +1869,7 @@ * @brief Helper flag to indicate that some encryption feature is enabled, set automatically * @see MY_RF24_ENABLE_ENCRYPTION, MY_RFM69_ENABLE_ENCRYPTION, MY_NRF5_ESB_ENABLE_ENCRYPTION, MY_RFM95_ENABLE_ENCRYPTION */ -#if defined(MY_RF24_ENABLE_ENCRYPTION) || defined(MY_RFM69_ENABLE_ENCRYPTION) || defined(MY_NRF5_ESB_ENABLE_ENCRYPTION) || defined(MY_RFM95_ENABLE_ENCRYPTION) +#if defined(MY_RF24_ENABLE_ENCRYPTION) || defined(MY_RF24_ENABLE_XTEA) || defined(MY_RFM69_ENABLE_ENCRYPTION) || defined(MY_NRF5_ESB_ENABLE_ENCRYPTION) || defined(MY_RFM95_ENABLE_ENCRYPTION) #define MY_ENCRYPTION_FEATURE #endif /** @}*/ // End of EncryptionSettingGrpPub group diff --git a/core/MyMessage.h b/core/MyMessage.h index bdd4654..9320551 100644 --- a/core/MyMessage.h +++ b/core/MyMessage.h @@ -36,9 +36,16 @@ #include #endif + #define PROTOCOL_VERSION (2u) //!< The version of the protocol #define MAX_MESSAGE_LENGTH (32u) //!< The maximum size of a message (including header) +#if (defined (MY_MESSAGE_ENABLE_HASH) && defined(MY_MESSAGE_ENABLE_NONCE)) +#define HEADER_SIZE (11u) //!< The size of the header +#elif (defined (MY_MESSAGE_ENABLE_HASH) || defined(MY_MESSAGE_ENABLE_NONCE)) +#define HEADER_SIZE (9u) //!< The size of the header +#else #define HEADER_SIZE (7u) //!< The size of the header +#endif #define MAX_PAYLOAD (MAX_MESSAGE_LENGTH - HEADER_SIZE) //!< The maximum size of a payload depends on #MAX_MESSAGE_LENGTH and #HEADER_SIZE /// @brief The command field (message-type) defines the overall properties of a message @@ -495,6 +502,15 @@ typedef union { * 3 bit - Payload data type */ uint8_t command_ack_payload; + +#if defined (MY_MESSAGE_ENABLE_NONCE) + // The salt/nonce need to be in the first XTEA block + uint16_t _nounce; // 16 bit - nounce +#endif + +#if defined (MY_MESSAGE_ENABLE_HASH) + uint16_t _hash; // 16 bit - CRC16 checksum +#endif uint8_t type; ///< 8 bit - Type varies depending on command uint8_t sensor; ///< 8 bit - Id of sensor that this message concerns. diff --git a/core/MySensorsCore.cpp b/core/MySensorsCore.cpp index eb1f1ab..244e318 100644 --- a/core/MySensorsCore.cpp +++ b/core/MySensorsCore.cpp @@ -40,7 +40,7 @@ MyMessage _msgTmp; // Buffer for temporary messages (acks and nonces among othe static coreConfig_t _coreConfig; #if defined(DEBUG_OUTPUT_ENABLED) -char _convBuf[MAX_PAYLOAD*2+1]; +char _convBuf[MAX_PAYLOAD*2+10]; #endif // Callback for transport=ok transition @@ -313,12 +313,24 @@ bool _sendRoute(MyMessage &message) #endif } +#if defined (MY_MESSAGE_ENABLE_NONCE) +uint16_t _nounce = 0; +bool send(MyMessage &message, const bool enableAck) { + return send(message, enableAck, _nounce++); +} +bool send(MyMessage &message, const bool enableAck, uint16_t nounce) +#else bool send(MyMessage &message, const bool enableAck) +#endif { message.sender = getNodeId(); mSetCommand(message, C_SET); mSetRequestAck(message, enableAck); +#if defined(MY_MESSAGE_ENABLE_NONCE) + message._nounce = nounce; +#endif + #if defined(MY_REGISTRATION_FEATURE) && !defined(MY_GATEWAY_FEATURE) if (_coreConfig.nodeRegistered) { return _sendRoute(message); diff --git a/core/MySensorsCore.h b/core/MySensorsCore.h index 721cbde..6dcfa8c 100644 --- a/core/MySensorsCore.h +++ b/core/MySensorsCore.h @@ -159,6 +159,10 @@ bool sendSketchInfo(const __FlashStringHelper *name, const __FlashStringHelper * * @return true Returns true if message reached the first stop on its way to destination. */ bool send(MyMessage &msg, const bool ack = false); +#if defined(MY_MESSAGE_ENABLE_NONCE) +bool send(MyMessage &message, const bool enableAck, uint16_t nounce); +#endif + /** * Send this nodes battery level to gateway. diff --git a/core/MyTransport.cpp b/core/MyTransport.cpp index 06f0c14..048ccb4 100644 --- a/core/MyTransport.cpp +++ b/core/MyTransport.cpp @@ -19,10 +19,14 @@ #include "MyTransport.h" +#if defined(MY_MESSAGE_ENABLE_HASH) +#include "drivers/PearsonHash/PearsonHash.cpp" +#endif + // debug #if defined(MY_DEBUG_VERBOSE_TRANSPORT) #define TRANSPORT_DEBUG(x,...) DEBUG_OUTPUT(x, ##__VA_ARGS__) //!< debug -extern char _convBuf[MAX_PAYLOAD * 2 + 1]; +extern char _convBuf[MAX_PAYLOAD * 2 + 10]; #else #define TRANSPORT_DEBUG(x,...) //!< debug NULL #endif @@ -647,7 +651,7 @@ void transportProcessMessage(void) const uint8_t msgLength = min(mGetLength(_msg), (uint8_t)MAX_PAYLOAD); // calculate expected length const uint8_t expectedMessageLength = HEADER_SIZE + (mGetSigned(_msg) ? MAX_PAYLOAD : msgLength); -#if defined(MY_RF24_ENABLE_ENCRYPTION) || defined(MY_NRF5_ESB_ENABLE_ENCRYPTION) || defined(MY_RFM95_ENABLE_ENCRYPTION) +#if defined(MY_RF24_ENABLE_ENCRYPTION) || defined(MY_RF24_ENABLE_XTEA) || defined(MY_NRF5_ESB_ENABLE_ENCRYPTION) || defined(MY_RFM95_ENABLE_ENCRYPTION) // payload length = a multiple of blocksize length for decrypted messages, i.e. cannot be used for payload length check payloadLength = expectedMessageLength; #endif @@ -663,6 +667,18 @@ void transportProcessMessage(void) mGetSigned(_msg), ((command == C_INTERNAL && type == I_NONCE_RESPONSE) ? "" : _msg.getString(_convBuf))); +#if defined (MY_MESSAGE_ENABLE_HASH) + // Reject messages that do not pass CRC check + auto hash = _msg._hash; + _msg._hash = 0; + if (!PearsonHash::checkHash16(hash, &_msg.last+1, expectedMessageLength-sizeof(_msg.last))) { // do NOT include _msg.last + setIndication(INDICATION_ERR_SIGN); + TRANSPORT_DEBUG(PSTR("!TSF:MSG:CRC VERIFY FAIL\n")); + return; + } + _msg._hash = hash; +#endif + // Reject payloads with incorrect length if (payloadLength != expectedMessageLength) { setIndication(INDICATION_ERR_LENGTH); @@ -932,7 +948,7 @@ void transportProcessMessage(void) (void)transportRouteMessage(_msg); } #else - TRANSPORT_DEBUG(PSTR("!TSF:MSG:REL MSG,NREP\n")); // message relaying request, but not a repeater + TRANSPORT_DEBUG(PSTR("!TSF:MSG:REL MSG,NREP,D=%" PRIu8 ", Id=%" PRIu8 "\n"), destination, _transportConfig.nodeId); // message relaying request, but not a repeater #endif } (void)last; //avoid cppcheck warning @@ -979,6 +995,7 @@ void transportProcessFIFO(void) bool transportSendWrite(const uint8_t to, MyMessage &message) { + message.last = _transportConfig.nodeId; // Update last // sign message if required if (!signerSignMsg(message)) { @@ -988,8 +1005,14 @@ bool transportSendWrite(const uint8_t to, MyMessage &message) } // msg length changes if signed - const uint8_t totalMsgLength = HEADER_SIZE + ( mGetSigned(message) ? MAX_PAYLOAD : mGetLength( - message) ); + const uint8_t totalMsgLength = min(HEADER_SIZE + ( mGetSigned(message) ? MAX_PAYLOAD : mGetLength( + message) ), MAX_MESSAGE_LENGTH); + +#if defined (MY_MESSAGE_ENABLE_HASH) + //calculate crc + message._hash = 0; + message._hash = PearsonHash::calcHash16(&message.last+1, totalMsgLength-sizeof(message.last)); // Do not include message.last; +#endif // send setIndication(INDICATION_TX); diff --git a/drivers/CRC16/CRC16.cpp b/drivers/CRC16/CRC16.cpp new file mode 100644 index 0000000..c3d5f55 --- /dev/null +++ b/drivers/CRC16/CRC16.cpp @@ -0,0 +1,16 @@ +#include "CRC16.h" + + +uint16_t CRC16::calcCRC(const void* data, uint16_t len) { + uint16_t _crc = 0xffff; + for (uint16_t i = 0; i < len; i++) { + _crc = _crc16_update(_crc, ((uint8_t*)data)[i]); + } + return _crc; +} + +bool CRC16::checkCRC(uint16_t crc, const void* data, uint16_t len) { + return crc == calcCRC(data, len); +} + + diff --git a/drivers/CRC16/CRC16.h b/drivers/CRC16/CRC16.h new file mode 100644 index 0000000..c3dc481 --- /dev/null +++ b/drivers/CRC16/CRC16.h @@ -0,0 +1,15 @@ +#ifndef _CRC16_H_ +#define _CRC16_H_ + +#include +#include + + +class CRC16 { +public: + uint16_t calcCRC(const void* data, uint16_t len); + inline bool checkCRC(uint16_t crc, const void* data, uint16_t len); +}; + + +#endif /* _CRC16_H_ */ diff --git a/drivers/PearsonHash/PearsonHash.cpp b/drivers/PearsonHash/PearsonHash.cpp new file mode 100644 index 0000000..6d2809d --- /dev/null +++ b/drivers/PearsonHash/PearsonHash.cpp @@ -0,0 +1,27 @@ + +#include "PearsonHash.h" + + + +// Permutation function for Pearson hashing (uses less space than a lookup table) +// Needs to be bijective, which it is +uint8_t PearsonHash::_pearsonPermutation(uint8_t n) { + return (n*121) % 256; +} + + +uint16_t PearsonHash::calcHash16(const void* data, uint16_t len) { + uint8_t h[2] = { _pearsonPermutation((((uint8_t*)data)[0]+1)%256), _pearsonPermutation(((uint8_t*)data)[0]) }; + for (uint16_t i = 0; i < len; i++) { + auto _tmp = h[0]; + h[0] = _pearsonPermutation(h[1] ^ ((uint8_t*)data)[i]); + h[1] = _pearsonPermutation(_tmp ^ ((uint8_t*)data)[i]); + } + void* _tmp = h; // To avoid the type punning warning + return *((uint16_t*)_tmp); +} + + +bool PearsonHash::checkHash16(uint16_t crc, const void* data, uint16_t len) { + return crc == calcHash16(data, len); +} diff --git a/drivers/PearsonHash/PearsonHash.h b/drivers/PearsonHash/PearsonHash.h new file mode 100644 index 0000000..3360ee4 --- /dev/null +++ b/drivers/PearsonHash/PearsonHash.h @@ -0,0 +1,17 @@ +#ifndef _PEARSONHASH_H_ +#define _PEARSONHASH_H_ + + +#include + +class PearsonHash { +public: + static uint16_t calcHash16(const void* data, uint16_t len); + inline static bool checkHash16(uint16_t crc, const void* data, uint16_t len); + +private: + static uint8_t _pearsonPermutation(uint8_t n); +}; + + +#endif diff --git a/drivers/XTEA/XTEA.cpp b/drivers/XTEA/XTEA.cpp new file mode 100644 index 0000000..abf8bcc --- /dev/null +++ b/drivers/XTEA/XTEA.cpp @@ -0,0 +1,82 @@ + +#include +#include "XTEA.h" + + + +void XTEA::setKey(void* key) { + memcpy(_key, key, 16); +} + + +void XTEA::setIV(void* iv) { + memcpy(_vector, iv, 8); +} + + +void XTEA::setIV(uint32_t ivh = 0, uint32_t ivl = 0) { + memcpy(_vector, &ivh, 4); + memcpy(_vector+4, &ivl, 4); +} + + +void XTEA::cbc_encrypt(void *dest, const void *plain, uint16_t nr_blocks) { + for (uint32_t i = 0; i < nr_blocks; i++) { + + // XOR the current vector with the block before encrypting + for (auto j = 0; j < 8; j++) { + ((uint8_t*)plain)[i*8 + j] ^= _vector[j]; + } + + // Encrypt the block + encrypt_block((uint8_t*)dest + i*8, (uint8_t*)plain + i*8); + + // Copy the cipher output to the current vector + memcpy(_vector, (uint8_t*)dest + i*8, 8); + } +} + + +void XTEA::cbc_decrypt(void *dest, const void *cipher, uint16_t nr_blocks) { + for (uint32_t i = 0; i < nr_blocks; i++) { + + // Decrypt the block + decrypt_block((uint8_t*)dest + i*8, (uint8_t*)cipher + i*8); + + // XOR the output with the current vector to fully decrypt + for (auto j = 0; j < 8; j++) { + ((uint8_t*)dest)[i*8 + j] ^= _vector[j]; + } + + // Copy the cipher text to the current vector + memcpy(_vector, (uint8_t*)cipher + i*8, 8); + } +} + + +void XTEA::encrypt_block(void *dest, const void *block) { + uint8_t i; + uint32_t v0=((uint32_t*)block)[0], v1=((uint32_t*)block)[1]; + uint32_t sum=0, delta=0x9E3779B9; + for(i=0; i> 5) + v1) ^ (sum + ((uint32_t*)_key)[sum & 3]); + sum += delta; + v1 += ((v0 << 4 ^ v0 >> 5) + v0) ^ (sum + ((uint32_t*)_key)[sum>>11 & 3]); + } + ((uint32_t*)dest)[0]=v0; ((uint32_t*)dest)[1]=v1; +} + + +void XTEA::decrypt_block(void *dest, const void *block) { + uint8_t i; + uint32_t v0=((uint32_t*)block)[0], v1=((uint32_t*)block)[1]; + uint32_t delta=0x9E3779B9, sum=delta*XTEA_CYCLES; + for(i=0; i> 5) + v0) ^ (sum + ((uint32_t*)_key)[sum>>11 & 3]); + sum -= delta; + v0 -= ((v1 << 4 ^ v1 >> 5) + v1) ^ (sum + ((uint32_t*)_key)[sum & 3]); + } + ((uint32_t*)dest)[0]=v0; ((uint32_t*)dest)[1]=v1; +} + + diff --git a/drivers/XTEA/XTEA.h b/drivers/XTEA/XTEA.h new file mode 100644 index 0000000..194667f --- /dev/null +++ b/drivers/XTEA/XTEA.h @@ -0,0 +1,29 @@ +#ifndef XTEA_H_ +#define XTEA_H_ + +#include + + +#define XTEA_CYCLES 32 + + +class XTEA { +public: + void setKey(void* key); + + void setIV(void* iv); + void setIV(uint32_t ivh, uint32_t ivl); + + void cbc_encrypt(void *dest, const void *plain, uint16_t nr_blocks); + void cbc_decrypt(void *dest, const void *cipher, uint16_t nr_blocks); + + void encrypt_block(void *dest, const void *block); + void decrypt_block(void *dest, const void *block); + +private: + uint8_t _key[16] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10 }; + uint8_t _vector[8] = {0}; +}; + + +#endif /*XTEA_H_*/ diff --git a/hal/transport/RF24/MyTransportRF24.cpp b/hal/transport/RF24/MyTransportRF24.cpp index 85b1b0b..2643583 100644 --- a/hal/transport/RF24/MyTransportRF24.cpp +++ b/hal/transport/RF24/MyTransportRF24.cpp @@ -19,10 +19,15 @@ #include "drivers/RF24/RF24.h" + #if defined(MY_RF24_ENABLE_ENCRYPTION) #include "drivers/AES/AES.cpp" AES RF24_aes; uint8_t RF24_dataenc[32] = {0}; +#elif defined(MY_RF24_ENABLE_XTEA) +#include "drivers/XTEA/XTEA.cpp" +XTEA RF24_xtea; +uint8_t RF24_dataenc[32] = {0}; #endif #if defined(MY_RX_MESSAGE_BUFFER_FEATURE) @@ -74,6 +79,9 @@ bool transportInit(void) RF24_aes.set_key(RF24_psk, 16); // Make sure it is purged from memory when set (void)memset(RF24_psk, 0, 16); +#elif defined(MY_RF24_ENABLE_XTEA) + uint8_t key[16] = MY_RF24_XTEA_KEY; + RF24_xtea.setKey(key); #endif #if defined(MY_RX_MESSAGE_BUFFER_FEATURE) @@ -104,6 +112,13 @@ bool transportSend(const uint8_t to, const void *data, const uint8_t len, const //encrypt data RF24_aes.cbc_encrypt(RF24_dataenc, RF24_dataenc, finalLength / 16); return RF24_sendMessage(to, RF24_dataenc, finalLength, noACK); +#elif defined(MY_RF24_ENABLE_XTEA) + // has to be adjusted, WIP! + RF24_xtea.setIV(0, 0); + const uint8_t finalLength = len > 16 ? 32 : 16; + //encrypt data + RF24_xtea.cbc_encrypt(RF24_dataenc, data, finalLength / 8); + return RF24_sendMessage(to, RF24_dataenc, finalLength, noACK); #else return RF24_sendMessage(to, data, len, noACK); #endif @@ -144,6 +159,13 @@ uint8_t transportReceive(void *data) if (RF24_aes.cbc_decrypt((uint8_t *)data, (uint8_t *)data, len > 16 ? 2 : 1) != AES_SUCCESS) { len = 0; } +#elif defined(MY_RF24_ENABLE_XTEA) + // has to be adjusted, WIP! + RF24_xtea.setIV(0, 0); + const uint8_t finalLength = len > 16 ? 32 : 16; + // decrypt data + RF24_xtea.cbc_decrypt((uint8_t*)(RF24_dataenc), (uint8_t*)(data), finalLength / 8); + (void)memcpy(data,RF24_dataenc,len); #endif return len; }