From 38d8ecaf228e881d20ae4f7a1fe7673faa243334 Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Thu, 8 Oct 2020 02:30:52 +0100 Subject: [PATCH] Start with LoRaWAN connectivity --- main/LoRaMacCrypto.c | 248 ++++++++++++ main/LoRaMacCrypto.h | 127 ++++++ main/aes.c | 936 +++++++++++++++++++++++++++++++++++++++++++ main/aes.h | 160 ++++++++ main/cmac.c | 153 +++++++ main/cmac.h | 63 +++ main/cmacutil.c | 86 ++++ main/cmacutil.h | 109 +++++ main/config.h | 35 +- main/disp.cpp | 3 +- main/disp_oled.cpp | 21 +- main/disp_oled.h | 2 +- main/hal.cpp | 8 + main/hal.h | 5 + main/lorawan.h | 240 +++++++++++ main/main.cpp | 7 + main/parameters.h | 20 +- main/rf.cpp | 4 + main/rfm.h | 27 +- 19 files changed, 2225 insertions(+), 29 deletions(-) create mode 100644 main/LoRaMacCrypto.c create mode 100644 main/LoRaMacCrypto.h create mode 100644 main/aes.c create mode 100644 main/aes.h create mode 100644 main/cmac.c create mode 100644 main/cmac.h create mode 100644 main/cmacutil.c create mode 100644 main/cmacutil.h create mode 100644 main/lorawan.h diff --git a/main/LoRaMacCrypto.c b/main/LoRaMacCrypto.c new file mode 100644 index 0000000..7c36f18 --- /dev/null +++ b/main/LoRaMacCrypto.c @@ -0,0 +1,248 @@ +/* + * Copyright (C) 2015-2017 Alibaba Group Holding Limited + */ + +/* + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2013 Semtech + ___ _____ _ ___ _ _____ ___ ___ ___ ___ +/ __|_ _/_\ / __| |/ / __/ _ \| _ \/ __| __| +\__ \ | |/ _ \ (__| ' <| _| (_) | / (__| _| +|___/ |_/_/ \_\___|_|\_\_| \___/|_|_\\___|___| +embedded.connectivity.solutions=============== + +Description: LoRa MAC layer implementation + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis ( Semtech ), Gregory Cristian ( Semtech ) and Daniel Jaeckle ( STACKFORCE ) +*/ +#include +#include +#include "cmacutil.h" + +#include "aes.h" +#include "cmac.h" + +#include "LoRaMacCrypto.h" + +/*! + * CMAC/AES Message Integrity Code (MIC) Block B0 size + */ +#define LORAMAC_MIC_BLOCK_B0_SIZE 16 + +/*! + * MIC field computation initial data + */ +static uint8_t MicBlockB0[] = { 0x49, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + +/*! + * Contains the computed MIC field. + * + * \remark Only the 4 first bytes are used + */ +static uint8_t Mic[16]; + +/*! + * Encryption aBlock and sBlock + */ +static uint8_t aBlock[] = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; +static uint8_t sBlock[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + +/*! + * AES computation context variable + */ +static aes_context AesContext; + +/*! + * CMAC computation context variable + */ +static AES_CMAC_CTX AesCmacCtx[1]; + +/*! + * \brief Computes the LoRaMAC frame MIC field + * + * \param [IN] buffer Data buffer + * \param [IN] size Data buffer size + * \param [IN] key AES key to be used + * \param [IN] address Frame address + * \param [IN] dir Frame direction [0: uplink, 1: downlink] + * \param [IN] sequenceCounter Frame sequence counter + * \param [OUT] mic Computed MIC field + */ +void LoRaMacComputeMic( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t address, uint8_t dir, uint32_t sequenceCounter, uint32_t *mic ) +{ + MicBlockB0[0] = 0x49; // these are already preloade but you never know... + MicBlockB0[1] = 0x00; + MicBlockB0[2] = 0x00; + MicBlockB0[3] = 0x00; + MicBlockB0[4] = 0x00; + + MicBlockB0[5] = dir; + + MicBlockB0[6] = ( address ) & 0xFF; + MicBlockB0[7] = ( address >> 8 ) & 0xFF; + MicBlockB0[8] = ( address >> 16 ) & 0xFF; + MicBlockB0[9] = ( address >> 24 ) & 0xFF; + + MicBlockB0[10] = ( sequenceCounter ) & 0xFF; + MicBlockB0[11] = ( sequenceCounter >> 8 ) & 0xFF; + MicBlockB0[12] = ( sequenceCounter >> 16 ) & 0xFF; + MicBlockB0[13] = ( sequenceCounter >> 24 ) & 0xFF; + + MicBlockB0[14] = 0x00; // preloaded in principle, but ... + + MicBlockB0[15] = size & 0xFF; + + AES_CMAC_Init( AesCmacCtx ); + + AES_CMAC_SetKey( AesCmacCtx, key ); + + AES_CMAC_Update( AesCmacCtx, MicBlockB0, LORAMAC_MIC_BLOCK_B0_SIZE ); + + AES_CMAC_Update( AesCmacCtx, buffer, size & 0xFF ); + + AES_CMAC_Final( Mic, AesCmacCtx ); + + *mic = ( uint32_t )( ( uint32_t )Mic[3] << 24 | ( uint32_t )Mic[2] << 16 | ( uint32_t )Mic[1] << 8 | ( uint32_t )Mic[0] ); +} + +void LoRaMacPayloadEncrypt( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t address, uint8_t dir, uint32_t sequenceCounter, uint8_t *encBuffer ) +{ + uint16_t i; + uint8_t bufferIndex = 0; + uint16_t ctr = 1; + + memset1( AesContext.ksch, '\0', 240 ); + lorawan_aes_set_key( key, 16, &AesContext ); + + aBlock[5] = dir; + + aBlock[6] = ( address ) & 0xFF; + aBlock[7] = ( address >> 8 ) & 0xFF; + aBlock[8] = ( address >> 16 ) & 0xFF; + aBlock[9] = ( address >> 24 ) & 0xFF; + + aBlock[10] = ( sequenceCounter ) & 0xFF; + aBlock[11] = ( sequenceCounter >> 8 ) & 0xFF; + aBlock[12] = ( sequenceCounter >> 16 ) & 0xFF; + aBlock[13] = ( sequenceCounter >> 24 ) & 0xFF; + + while( size >= 16 ) + { + aBlock[15] = ( ( ctr ) & 0xFF ); + ctr++; + lora_aes_encrypt( aBlock, sBlock, &AesContext ); + for( i = 0; i < 16; i++ ) + { + encBuffer[bufferIndex + i] = buffer[bufferIndex + i] ^ sBlock[i]; + } + size -= 16; + bufferIndex += 16; + } + + if( size > 0 ) + { + aBlock[15] = ( ( ctr ) & 0xFF ); + lora_aes_encrypt( aBlock, sBlock, &AesContext ); + for( i = 0; i < size; i++ ) + { + encBuffer[bufferIndex + i] = buffer[bufferIndex + i] ^ sBlock[i]; + } + } +} + +void LoRaMacPayloadDecrypt( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t address, uint8_t dir, uint32_t sequenceCounter, uint8_t *decBuffer ) +{ + LoRaMacPayloadEncrypt( buffer, size, key, address, dir, sequenceCounter, decBuffer ); +} + +void LoRaMacJoinComputeMic( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t *mic ) +{ + AES_CMAC_Init( AesCmacCtx ); + + AES_CMAC_SetKey( AesCmacCtx, key ); + + AES_CMAC_Update( AesCmacCtx, buffer, size & 0xFF ); + + AES_CMAC_Final( Mic, AesCmacCtx ); + + *mic = ( uint32_t )( ( uint32_t )Mic[3] << 24 | ( uint32_t )Mic[2] << 16 | ( uint32_t )Mic[1] << 8 | ( uint32_t )Mic[0] ); +} + +void LoRaMacJoinDecrypt( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint8_t *decBuffer ) +{ + memset1( AesContext.ksch, '\0', 240 ); + lorawan_aes_set_key( key, 16, &AesContext ); + lora_aes_encrypt( buffer, decBuffer, &AesContext ); + // Check if optional CFList is included + if( size >= 16 ) + { + lora_aes_encrypt( buffer + 16, decBuffer + 16, &AesContext ); + } +} + +void LoRaMacJoinComputeSKeys( const uint8_t *key, const uint8_t *appNonce, uint16_t devNonce, uint8_t *nwkSKey, uint8_t *appSKey ) +{ + uint8_t nonce[16]; + uint8_t *pDevNonce = ( uint8_t * )&devNonce; + + memset1( AesContext.ksch, '\0', 240 ); + lorawan_aes_set_key( key, 16, &AesContext ); + + memset1( nonce, 0, sizeof( nonce ) ); + nonce[0] = 0x01; + memcpy1( nonce + 1, appNonce, 6 ); + memcpy1( nonce + 7, pDevNonce, 2 ); + lora_aes_encrypt( nonce, nwkSKey, &AesContext ); + + memset1( nonce, 0, sizeof( nonce ) ); + nonce[0] = 0x02; + memcpy1( nonce + 1, appNonce, 6 ); + memcpy1( nonce + 7, pDevNonce, 2 ); + lora_aes_encrypt( nonce, appSKey, &AesContext ); +} + +void LoRaMacBeaconComputePingOffset( uint64_t beaconTime, uint32_t address, uint16_t pingPeriod, uint16_t *pingOffset ) +{ + uint8_t zeroKey[16]; + uint8_t buffer[16]; + uint8_t cipher[16]; + uint32_t result = 0; + /* Refer to chapter 15.2 of the LoRaWAN specification v1.1. The beacon time + * GPS time in seconds modulo 2^32 + */ + uint32_t time = ( beaconTime % ( ( ( uint64_t ) 1 ) << 32 ) ); + + memset1( zeroKey, 0, 16 ); + memset1( buffer, 0, 16 ); + memset1( cipher, 0, 16 ); + memset1( AesContext.ksch, '\0', 240 ); + + buffer[0] = ( time ) & 0xFF; + buffer[1] = ( time >> 8 ) & 0xFF; + buffer[2] = ( time >> 16 ) & 0xFF; + buffer[3] = ( time >> 24 ) & 0xFF; + + buffer[4] = ( address ) & 0xFF; + buffer[5] = ( address >> 8 ) & 0xFF; + buffer[6] = ( address >> 16 ) & 0xFF; + buffer[7] = ( address >> 24 ) & 0xFF; + + lorawan_aes_set_key( zeroKey, 16, &AesContext ); + lora_aes_encrypt( buffer, cipher, &AesContext ); + + result = ( ( ( uint32_t ) cipher[0] ) + ( ( ( uint32_t ) cipher[1] ) * 256 ) ); + + *pingOffset = ( uint16_t )( result % pingPeriod ); +} diff --git a/main/LoRaMacCrypto.h b/main/LoRaMacCrypto.h new file mode 100644 index 0000000..0c579fa --- /dev/null +++ b/main/LoRaMacCrypto.h @@ -0,0 +1,127 @@ +/* + * Copyright (C) 2015-2017 Alibaba Group Holding Limited + */ + +/*! + * \file LoRaMacCrypto.h + * + * \brief LoRa MAC layer cryptography implementation + * + * \copyright Revised BSD License, see section \ref LICENSE. + * + * \code + * ______ _ + * / _____) _ | | + * ( (____ _____ ____ _| |_ _____ ____| |__ + * \____ \| ___ | (_ _) ___ |/ ___) _ \ + * _____) ) ____| | | || |_| ____( (___| | | | + * (______/|_____)_|_|_| \__)_____)\____)_| |_| + * (C)2013 Semtech + * + * ___ _____ _ ___ _ _____ ___ ___ ___ ___ + * / __|_ _/_\ / __| |/ / __/ _ \| _ \/ __| __| + * \__ \ | |/ _ \ (__| ' <| _| (_) | / (__| _| + * |___/ |_/_/ \_\___|_|\_\_| \___/|_|_\\___|___| + * embedded.connectivity.solutions=============== + * + * \endcode + * + * \author Miguel Luis ( Semtech ) + * + * \author Gregory Cristian ( Semtech ) + * + * \author Daniel Jaeckle ( STACKFORCE ) + * + * \defgroup LORAMAC_CRYPTO LoRa MAC layer cryptography implementation + * This module covers the implementation of cryptographic functions + * of the LoRaMAC layer. + * \{ + */ +#ifndef __LORAMAC_CRYPTO_H__ +#define __LORAMAC_CRYPTO_H__ + +#include + +/*! + * Computes the LoRaMAC frame MIC field + * + * \param [IN] buffer - Data buffer + * \param [IN] size - Data buffer size + * \param [IN] key - AES key to be used + * \param [IN] address - Frame address + * \param [IN] dir - Frame direction [0: uplink, 1: downlink] + * \param [IN] sequenceCounter - Frame sequence counter + * \param [OUT] mic - Computed MIC field + */ +void LoRaMacComputeMic( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t address, uint8_t dir, uint32_t sequenceCounter, uint32_t *mic ); + +/*! + * Computes the LoRaMAC payload encryption + * + * \param [IN] buffer - Data buffer + * \param [IN] size - Data buffer size + * \param [IN] key - AES key to be used + * \param [IN] address - Frame address + * \param [IN] dir - Frame direction [0: uplink, 1: downlink] + * \param [IN] sequenceCounter - Frame sequence counter + * \param [OUT] encBuffer - Encrypted buffer + */ +void LoRaMacPayloadEncrypt( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t address, uint8_t dir, uint32_t sequenceCounter, uint8_t *encBuffer ); + +/*! + * Computes the LoRaMAC payload decryption + * + * \param [IN] buffer - Data buffer + * \param [IN] size - Data buffer size + * \param [IN] key - AES key to be used + * \param [IN] address - Frame address + * \param [IN] dir - Frame direction [0: uplink, 1: downlink] + * \param [IN] sequenceCounter - Frame sequence counter + * \param [OUT] decBuffer - Decrypted buffer + */ +void LoRaMacPayloadDecrypt( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t address, uint8_t dir, uint32_t sequenceCounter, uint8_t *decBuffer ); + +/*! + * Computes the LoRaMAC Join Request frame MIC field + * + * \param [IN] buffer - Data buffer + * \param [IN] size - Data buffer size + * \param [IN] key - AES key to be used + * \param [OUT] mic - Computed MIC field + */ +void LoRaMacJoinComputeMic( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t *mic ); + +/*! + * Computes the LoRaMAC join frame decryption + * + * \param [IN] buffer - Data buffer + * \param [IN] size - Data buffer size + * \param [IN] key - AES key to be used + * \param [OUT] decBuffer - Decrypted buffer + */ +void LoRaMacJoinDecrypt( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint8_t *decBuffer ); + +/*! + * Computes the LoRaMAC join frame decryption + * + * \param [IN] key - AES key to be used + * \param [IN] appNonce - Application nonce + * \param [IN] devNonce - Device nonce + * \param [OUT] nwkSKey - Network session key + * \param [OUT] appSKey - Application session key + */ +void LoRaMacJoinComputeSKeys( const uint8_t *key, const uint8_t *appNonce, uint16_t devNonce, uint8_t *nwkSKey, uint8_t *appSKey ); + +/*! + * Computes the LoRaMAC join frame decryption + * + * \param [IN] beaconTime - Time of the recent received beacon + * \param [IN] address - Frame address + * \param [IN] pingPeriod - Ping period of the node + * \param [OUT] pingOffset - Pseudo random ping offset + */ +void LoRaMacBeaconComputePingOffset( uint64_t beaconTime, uint32_t address, uint16_t pingPeriod, uint16_t *pingOffset ); + +/*! \} defgroup LORAMAC */ + +#endif // __LORAMAC_CRYPTO_H__ diff --git a/main/aes.c b/main/aes.c new file mode 100644 index 0000000..cc98569 --- /dev/null +++ b/main/aes.c @@ -0,0 +1,936 @@ +/* + --------------------------------------------------------------------------- + Copyright (c) 1998-2008, Brian Gladman, Worcester, UK. All rights reserved. + + LICENSE TERMS + + The redistribution and use of this software (with or without changes) + is allowed without the payment of fees or royalties provided that: + + 1. source code distributions include the above copyright notice, this + list of conditions and the following disclaimer; + + 2. binary distributions include the above copyright notice, this list + of conditions and the following disclaimer in their documentation; + + 3. the name of the copyright holder is not used to endorse products + built using this software without specific written permission. + + DISCLAIMER + + This software is provided 'as is' with no explicit or implied warranties + in respect of its properties, including, but not limited to, correctness + and/or fitness for purpose. + --------------------------------------------------------------------------- + Issue 09/09/2006 + + This is an AES implementation that uses only 8-bit byte operations on the + cipher state (there are options to use 32-bit types if available). + + The combination of mix columns and byte substitution used here is based on + that developed by Karl Malbrain. His contribution is acknowledged. + */ + +/* define if you have a fast memcpy function on your system */ +#if 0 +# define HAVE_MEMCPY +# include +# if defined( _MSC_VER ) +# include +# pragma intrinsic( memcpy ) +# endif +#endif + + +#include +#include + +/* define if you have fast 32-bit types on your system */ +#if ( __CORTEX_M != 0 ) // if Cortex is different from M0/M0+ +# define HAVE_UINT_32T +#endif + +/* define if you don't want any tables */ +#if 1 +# define USE_TABLES +#endif + +/* On Intel Core 2 duo VERSION_1 is faster */ + +/* alternative versions (test for performance on your system) */ +#if 1 +# define VERSION_1 +#endif + +#include "aes.h" + +//#if defined( HAVE_UINT_32T ) +// typedef unsigned long uint32_t; +//#endif + +/* functions for finite field multiplication in the AES Galois field */ + +#define WPOLY 0x011b +#define BPOLY 0x1b +#define DPOLY 0x008d + +#define f1(x) (x) +#define f2(x) ((x << 1) ^ (((x >> 7) & 1) * WPOLY)) +#define f4(x) ((x << 2) ^ (((x >> 6) & 1) * WPOLY) ^ (((x >> 6) & 2) * WPOLY)) +#define f8(x) ((x << 3) ^ (((x >> 5) & 1) * WPOLY) ^ (((x >> 5) & 2) * WPOLY) \ + ^ (((x >> 5) & 4) * WPOLY)) +#define d2(x) (((x) >> 1) ^ ((x) & 1 ? DPOLY : 0)) + +#define f3(x) (f2(x) ^ x) +#define f9(x) (f8(x) ^ x) +#define fb(x) (f8(x) ^ f2(x) ^ x) +#define fd(x) (f8(x) ^ f4(x) ^ x) +#define fe(x) (f8(x) ^ f4(x) ^ f2(x)) + +#if defined( USE_TABLES ) + +#define sb_data(w) { /* S Box data values */ \ + w(0x63), w(0x7c), w(0x77), w(0x7b), w(0xf2), w(0x6b), w(0x6f), w(0xc5),\ + w(0x30), w(0x01), w(0x67), w(0x2b), w(0xfe), w(0xd7), w(0xab), w(0x76),\ + w(0xca), w(0x82), w(0xc9), w(0x7d), w(0xfa), w(0x59), w(0x47), w(0xf0),\ + w(0xad), w(0xd4), w(0xa2), w(0xaf), w(0x9c), w(0xa4), w(0x72), w(0xc0),\ + w(0xb7), w(0xfd), w(0x93), w(0x26), w(0x36), w(0x3f), w(0xf7), w(0xcc),\ + w(0x34), w(0xa5), w(0xe5), w(0xf1), w(0x71), w(0xd8), w(0x31), w(0x15),\ + w(0x04), w(0xc7), w(0x23), w(0xc3), w(0x18), w(0x96), w(0x05), w(0x9a),\ + w(0x07), w(0x12), w(0x80), w(0xe2), w(0xeb), w(0x27), w(0xb2), w(0x75),\ + w(0x09), w(0x83), w(0x2c), w(0x1a), w(0x1b), w(0x6e), w(0x5a), w(0xa0),\ + w(0x52), w(0x3b), w(0xd6), w(0xb3), w(0x29), w(0xe3), w(0x2f), w(0x84),\ + w(0x53), w(0xd1), w(0x00), w(0xed), w(0x20), w(0xfc), w(0xb1), w(0x5b),\ + w(0x6a), w(0xcb), w(0xbe), w(0x39), w(0x4a), w(0x4c), w(0x58), w(0xcf),\ + w(0xd0), w(0xef), w(0xaa), w(0xfb), w(0x43), w(0x4d), w(0x33), w(0x85),\ + w(0x45), w(0xf9), w(0x02), w(0x7f), w(0x50), w(0x3c), w(0x9f), w(0xa8),\ + w(0x51), w(0xa3), w(0x40), w(0x8f), w(0x92), w(0x9d), w(0x38), w(0xf5),\ + w(0xbc), w(0xb6), w(0xda), w(0x21), w(0x10), w(0xff), w(0xf3), w(0xd2),\ + w(0xcd), w(0x0c), w(0x13), w(0xec), w(0x5f), w(0x97), w(0x44), w(0x17),\ + w(0xc4), w(0xa7), w(0x7e), w(0x3d), w(0x64), w(0x5d), w(0x19), w(0x73),\ + w(0x60), w(0x81), w(0x4f), w(0xdc), w(0x22), w(0x2a), w(0x90), w(0x88),\ + w(0x46), w(0xee), w(0xb8), w(0x14), w(0xde), w(0x5e), w(0x0b), w(0xdb),\ + w(0xe0), w(0x32), w(0x3a), w(0x0a), w(0x49), w(0x06), w(0x24), w(0x5c),\ + w(0xc2), w(0xd3), w(0xac), w(0x62), w(0x91), w(0x95), w(0xe4), w(0x79),\ + w(0xe7), w(0xc8), w(0x37), w(0x6d), w(0x8d), w(0xd5), w(0x4e), w(0xa9),\ + w(0x6c), w(0x56), w(0xf4), w(0xea), w(0x65), w(0x7a), w(0xae), w(0x08),\ + w(0xba), w(0x78), w(0x25), w(0x2e), w(0x1c), w(0xa6), w(0xb4), w(0xc6),\ + w(0xe8), w(0xdd), w(0x74), w(0x1f), w(0x4b), w(0xbd), w(0x8b), w(0x8a),\ + w(0x70), w(0x3e), w(0xb5), w(0x66), w(0x48), w(0x03), w(0xf6), w(0x0e),\ + w(0x61), w(0x35), w(0x57), w(0xb9), w(0x86), w(0xc1), w(0x1d), w(0x9e),\ + w(0xe1), w(0xf8), w(0x98), w(0x11), w(0x69), w(0xd9), w(0x8e), w(0x94),\ + w(0x9b), w(0x1e), w(0x87), w(0xe9), w(0xce), w(0x55), w(0x28), w(0xdf),\ + w(0x8c), w(0xa1), w(0x89), w(0x0d), w(0xbf), w(0xe6), w(0x42), w(0x68),\ + w(0x41), w(0x99), w(0x2d), w(0x0f), w(0xb0), w(0x54), w(0xbb), w(0x16) } + +#define isb_data(w) { /* inverse S Box data values */ \ + w(0x52), w(0x09), w(0x6a), w(0xd5), w(0x30), w(0x36), w(0xa5), w(0x38),\ + w(0xbf), w(0x40), w(0xa3), w(0x9e), w(0x81), w(0xf3), w(0xd7), w(0xfb),\ + w(0x7c), w(0xe3), w(0x39), w(0x82), w(0x9b), w(0x2f), w(0xff), w(0x87),\ + w(0x34), w(0x8e), w(0x43), w(0x44), w(0xc4), w(0xde), w(0xe9), w(0xcb),\ + w(0x54), w(0x7b), w(0x94), w(0x32), w(0xa6), w(0xc2), w(0x23), w(0x3d),\ + w(0xee), w(0x4c), w(0x95), w(0x0b), w(0x42), w(0xfa), w(0xc3), w(0x4e),\ + w(0x08), w(0x2e), w(0xa1), w(0x66), w(0x28), w(0xd9), w(0x24), w(0xb2),\ + w(0x76), w(0x5b), w(0xa2), w(0x49), w(0x6d), w(0x8b), w(0xd1), w(0x25),\ + w(0x72), w(0xf8), w(0xf6), w(0x64), w(0x86), w(0x68), w(0x98), w(0x16),\ + w(0xd4), w(0xa4), w(0x5c), w(0xcc), w(0x5d), w(0x65), w(0xb6), w(0x92),\ + w(0x6c), w(0x70), w(0x48), w(0x50), w(0xfd), w(0xed), w(0xb9), w(0xda),\ + w(0x5e), w(0x15), w(0x46), w(0x57), w(0xa7), w(0x8d), w(0x9d), w(0x84),\ + w(0x90), w(0xd8), w(0xab), w(0x00), w(0x8c), w(0xbc), w(0xd3), w(0x0a),\ + w(0xf7), w(0xe4), w(0x58), w(0x05), w(0xb8), w(0xb3), w(0x45), w(0x06),\ + w(0xd0), w(0x2c), w(0x1e), w(0x8f), w(0xca), w(0x3f), w(0x0f), w(0x02),\ + w(0xc1), w(0xaf), w(0xbd), w(0x03), w(0x01), w(0x13), w(0x8a), w(0x6b),\ + w(0x3a), w(0x91), w(0x11), w(0x41), w(0x4f), w(0x67), w(0xdc), w(0xea),\ + w(0x97), w(0xf2), w(0xcf), w(0xce), w(0xf0), w(0xb4), w(0xe6), w(0x73),\ + w(0x96), w(0xac), w(0x74), w(0x22), w(0xe7), w(0xad), w(0x35), w(0x85),\ + w(0xe2), w(0xf9), w(0x37), w(0xe8), w(0x1c), w(0x75), w(0xdf), w(0x6e),\ + w(0x47), w(0xf1), w(0x1a), w(0x71), w(0x1d), w(0x29), w(0xc5), w(0x89),\ + w(0x6f), w(0xb7), w(0x62), w(0x0e), w(0xaa), w(0x18), w(0xbe), w(0x1b),\ + w(0xfc), w(0x56), w(0x3e), w(0x4b), w(0xc6), w(0xd2), w(0x79), w(0x20),\ + w(0x9a), w(0xdb), w(0xc0), w(0xfe), w(0x78), w(0xcd), w(0x5a), w(0xf4),\ + w(0x1f), w(0xdd), w(0xa8), w(0x33), w(0x88), w(0x07), w(0xc7), w(0x31),\ + w(0xb1), w(0x12), w(0x10), w(0x59), w(0x27), w(0x80), w(0xec), w(0x5f),\ + w(0x60), w(0x51), w(0x7f), w(0xa9), w(0x19), w(0xb5), w(0x4a), w(0x0d),\ + w(0x2d), w(0xe5), w(0x7a), w(0x9f), w(0x93), w(0xc9), w(0x9c), w(0xef),\ + w(0xa0), w(0xe0), w(0x3b), w(0x4d), w(0xae), w(0x2a), w(0xf5), w(0xb0),\ + w(0xc8), w(0xeb), w(0xbb), w(0x3c), w(0x83), w(0x53), w(0x99), w(0x61),\ + w(0x17), w(0x2b), w(0x04), w(0x7e), w(0xba), w(0x77), w(0xd6), w(0x26),\ + w(0xe1), w(0x69), w(0x14), w(0x63), w(0x55), w(0x21), w(0x0c), w(0x7d) } + +#define mm_data(w) { /* basic data for forming finite field tables */ \ + w(0x00), w(0x01), w(0x02), w(0x03), w(0x04), w(0x05), w(0x06), w(0x07),\ + w(0x08), w(0x09), w(0x0a), w(0x0b), w(0x0c), w(0x0d), w(0x0e), w(0x0f),\ + w(0x10), w(0x11), w(0x12), w(0x13), w(0x14), w(0x15), w(0x16), w(0x17),\ + w(0x18), w(0x19), w(0x1a), w(0x1b), w(0x1c), w(0x1d), w(0x1e), w(0x1f),\ + w(0x20), w(0x21), w(0x22), w(0x23), w(0x24), w(0x25), w(0x26), w(0x27),\ + w(0x28), w(0x29), w(0x2a), w(0x2b), w(0x2c), w(0x2d), w(0x2e), w(0x2f),\ + w(0x30), w(0x31), w(0x32), w(0x33), w(0x34), w(0x35), w(0x36), w(0x37),\ + w(0x38), w(0x39), w(0x3a), w(0x3b), w(0x3c), w(0x3d), w(0x3e), w(0x3f),\ + w(0x40), w(0x41), w(0x42), w(0x43), w(0x44), w(0x45), w(0x46), w(0x47),\ + w(0x48), w(0x49), w(0x4a), w(0x4b), w(0x4c), w(0x4d), w(0x4e), w(0x4f),\ + w(0x50), w(0x51), w(0x52), w(0x53), w(0x54), w(0x55), w(0x56), w(0x57),\ + w(0x58), w(0x59), w(0x5a), w(0x5b), w(0x5c), w(0x5d), w(0x5e), w(0x5f),\ + w(0x60), w(0x61), w(0x62), w(0x63), w(0x64), w(0x65), w(0x66), w(0x67),\ + w(0x68), w(0x69), w(0x6a), w(0x6b), w(0x6c), w(0x6d), w(0x6e), w(0x6f),\ + w(0x70), w(0x71), w(0x72), w(0x73), w(0x74), w(0x75), w(0x76), w(0x77),\ + w(0x78), w(0x79), w(0x7a), w(0x7b), w(0x7c), w(0x7d), w(0x7e), w(0x7f),\ + w(0x80), w(0x81), w(0x82), w(0x83), w(0x84), w(0x85), w(0x86), w(0x87),\ + w(0x88), w(0x89), w(0x8a), w(0x8b), w(0x8c), w(0x8d), w(0x8e), w(0x8f),\ + w(0x90), w(0x91), w(0x92), w(0x93), w(0x94), w(0x95), w(0x96), w(0x97),\ + w(0x98), w(0x99), w(0x9a), w(0x9b), w(0x9c), w(0x9d), w(0x9e), w(0x9f),\ + w(0xa0), w(0xa1), w(0xa2), w(0xa3), w(0xa4), w(0xa5), w(0xa6), w(0xa7),\ + w(0xa8), w(0xa9), w(0xaa), w(0xab), w(0xac), w(0xad), w(0xae), w(0xaf),\ + w(0xb0), w(0xb1), w(0xb2), w(0xb3), w(0xb4), w(0xb5), w(0xb6), w(0xb7),\ + w(0xb8), w(0xb9), w(0xba), w(0xbb), w(0xbc), w(0xbd), w(0xbe), w(0xbf),\ + w(0xc0), w(0xc1), w(0xc2), w(0xc3), w(0xc4), w(0xc5), w(0xc6), w(0xc7),\ + w(0xc8), w(0xc9), w(0xca), w(0xcb), w(0xcc), w(0xcd), w(0xce), w(0xcf),\ + w(0xd0), w(0xd1), w(0xd2), w(0xd3), w(0xd4), w(0xd5), w(0xd6), w(0xd7),\ + w(0xd8), w(0xd9), w(0xda), w(0xdb), w(0xdc), w(0xdd), w(0xde), w(0xdf),\ + w(0xe0), w(0xe1), w(0xe2), w(0xe3), w(0xe4), w(0xe5), w(0xe6), w(0xe7),\ + w(0xe8), w(0xe9), w(0xea), w(0xeb), w(0xec), w(0xed), w(0xee), w(0xef),\ + w(0xf0), w(0xf1), w(0xf2), w(0xf3), w(0xf4), w(0xf5), w(0xf6), w(0xf7),\ + w(0xf8), w(0xf9), w(0xfa), w(0xfb), w(0xfc), w(0xfd), w(0xfe), w(0xff) } + +static const uint8_t sbox[256] = sb_data(f1); + +#if defined( AES_DEC_PREKEYED ) +static const uint8_t isbox[256] = isb_data(f1); +#endif + +static const uint8_t gfm2_sbox[256] = sb_data(f2); +static const uint8_t gfm3_sbox[256] = sb_data(f3); + +#if defined( AES_DEC_PREKEYED ) +static const uint8_t gfmul_9[256] = mm_data(f9); +static const uint8_t gfmul_b[256] = mm_data(fb); +static const uint8_t gfmul_d[256] = mm_data(fd); +static const uint8_t gfmul_e[256] = mm_data(fe); +#endif + +#define s_box(x) sbox[(x)] +#if defined( AES_DEC_PREKEYED ) +#define is_box(x) isbox[(x)] +#endif +#define gfm2_sb(x) gfm2_sbox[(x)] +#define gfm3_sb(x) gfm3_sbox[(x)] +#if defined( AES_DEC_PREKEYED ) +#define gfm_9(x) gfmul_9[(x)] +#define gfm_b(x) gfmul_b[(x)] +#define gfm_d(x) gfmul_d[(x)] +#define gfm_e(x) gfmul_e[(x)] +#endif +#else + +/* this is the high bit of x right shifted by 1 */ +/* position. Since the starting polynomial has */ +/* 9 bits (0x11b), this right shift keeps the */ +/* values of all top bits within a byte */ + +static uint8_t hibit(const uint8_t x) +{ uint8_t r = (uint8_t)((x >> 1) | (x >> 2)); + + r |= (r >> 2); + r |= (r >> 4); + return (r + 1) >> 1; +} + +/* return the inverse of the finite field element x */ + +static uint8_t gf_inv(const uint8_t x) +{ uint8_t p1 = x, p2 = BPOLY, n1 = hibit(x), n2 = 0x80, v1 = 1, v2 = 0; + + if(x < 2) + return x; + + for( ; ; ) + { + if(n1) + while(n2 >= n1) /* divide polynomial p2 by p1 */ + { + n2 /= n1; /* shift smaller polynomial left */ + p2 ^= (p1 * n2) & 0xff; /* and remove from larger one */ + v2 ^= (v1 * n2); /* shift accumulated value and */ + n2 = hibit(p2); /* add into result */ + } + else + return v1; + + if(n2) /* repeat with values swapped */ + while(n1 >= n2) + { + n1 /= n2; + p1 ^= p2 * n1; + v1 ^= v2 * n1; + n1 = hibit(p1); + } + else + return v2; + } +} + +/* The forward and inverse affine transformations used in the S-box */ +uint8_t fwd_affine(const uint8_t x) +{ +#if defined( HAVE_UINT_32T ) + uint32_t w = x; + w ^= (w << 1) ^ (w << 2) ^ (w << 3) ^ (w << 4); + return 0x63 ^ ((w ^ (w >> 8)) & 0xff); +#else + return 0x63 ^ x ^ (x << 1) ^ (x << 2) ^ (x << 3) ^ (x << 4) + ^ (x >> 7) ^ (x >> 6) ^ (x >> 5) ^ (x >> 4); +#endif +} + +uint8_t inv_affine(const uint8_t x) +{ +#if defined( HAVE_UINT_32T ) + uint32_t w = x; + w = (w << 1) ^ (w << 3) ^ (w << 6); + return 0x05 ^ ((w ^ (w >> 8)) & 0xff); +#else + return 0x05 ^ (x << 1) ^ (x << 3) ^ (x << 6) + ^ (x >> 7) ^ (x >> 5) ^ (x >> 2); +#endif +} + +#define s_box(x) fwd_affine(gf_inv(x)) +#define is_box(x) gf_inv(inv_affine(x)) +#define gfm2_sb(x) f2(s_box(x)) +#define gfm3_sb(x) f3(s_box(x)) +#define gfm_9(x) f9(x) +#define gfm_b(x) fb(x) +#define gfm_d(x) fd(x) +#define gfm_e(x) fe(x) + +#endif + +#if defined( HAVE_MEMCPY ) +# define block_copy_nn(d, s, l) memcpy(d, s, l) +# define block_copy(d, s) memcpy(d, s, N_BLOCK) +#else +# define block_copy_nn(d, s, l) copy_block_nn(d, s, l) +# define block_copy(d, s) copy_block(d, s) +#endif + +static void copy_block( void *d, const void *s ) +{ +#if defined( HAVE_UINT_32T ) + ((uint32_t*)d)[ 0] = ((uint32_t*)s)[ 0]; + ((uint32_t*)d)[ 1] = ((uint32_t*)s)[ 1]; + ((uint32_t*)d)[ 2] = ((uint32_t*)s)[ 2]; + ((uint32_t*)d)[ 3] = ((uint32_t*)s)[ 3]; +#else + ((uint8_t*)d)[ 0] = ((uint8_t*)s)[ 0]; + ((uint8_t*)d)[ 1] = ((uint8_t*)s)[ 1]; + ((uint8_t*)d)[ 2] = ((uint8_t*)s)[ 2]; + ((uint8_t*)d)[ 3] = ((uint8_t*)s)[ 3]; + ((uint8_t*)d)[ 4] = ((uint8_t*)s)[ 4]; + ((uint8_t*)d)[ 5] = ((uint8_t*)s)[ 5]; + ((uint8_t*)d)[ 6] = ((uint8_t*)s)[ 6]; + ((uint8_t*)d)[ 7] = ((uint8_t*)s)[ 7]; + ((uint8_t*)d)[ 8] = ((uint8_t*)s)[ 8]; + ((uint8_t*)d)[ 9] = ((uint8_t*)s)[ 9]; + ((uint8_t*)d)[10] = ((uint8_t*)s)[10]; + ((uint8_t*)d)[11] = ((uint8_t*)s)[11]; + ((uint8_t*)d)[12] = ((uint8_t*)s)[12]; + ((uint8_t*)d)[13] = ((uint8_t*)s)[13]; + ((uint8_t*)d)[14] = ((uint8_t*)s)[14]; + ((uint8_t*)d)[15] = ((uint8_t*)s)[15]; +#endif +} + +static void copy_block_nn( uint8_t * d, const uint8_t *s, uint8_t nn ) +{ + while( nn-- ) + //*((uint8_t*)d)++ = *((uint8_t*)s)++; + *d++ = *s++; +} + +static void xor_block( void *d, const void *s ) +{ +#if defined( HAVE_UINT_32T ) + ((uint32_t*)d)[ 0] ^= ((uint32_t*)s)[ 0]; + ((uint32_t*)d)[ 1] ^= ((uint32_t*)s)[ 1]; + ((uint32_t*)d)[ 2] ^= ((uint32_t*)s)[ 2]; + ((uint32_t*)d)[ 3] ^= ((uint32_t*)s)[ 3]; +#else + ((uint8_t*)d)[ 0] ^= ((uint8_t*)s)[ 0]; + ((uint8_t*)d)[ 1] ^= ((uint8_t*)s)[ 1]; + ((uint8_t*)d)[ 2] ^= ((uint8_t*)s)[ 2]; + ((uint8_t*)d)[ 3] ^= ((uint8_t*)s)[ 3]; + ((uint8_t*)d)[ 4] ^= ((uint8_t*)s)[ 4]; + ((uint8_t*)d)[ 5] ^= ((uint8_t*)s)[ 5]; + ((uint8_t*)d)[ 6] ^= ((uint8_t*)s)[ 6]; + ((uint8_t*)d)[ 7] ^= ((uint8_t*)s)[ 7]; + ((uint8_t*)d)[ 8] ^= ((uint8_t*)s)[ 8]; + ((uint8_t*)d)[ 9] ^= ((uint8_t*)s)[ 9]; + ((uint8_t*)d)[10] ^= ((uint8_t*)s)[10]; + ((uint8_t*)d)[11] ^= ((uint8_t*)s)[11]; + ((uint8_t*)d)[12] ^= ((uint8_t*)s)[12]; + ((uint8_t*)d)[13] ^= ((uint8_t*)s)[13]; + ((uint8_t*)d)[14] ^= ((uint8_t*)s)[14]; + ((uint8_t*)d)[15] ^= ((uint8_t*)s)[15]; +#endif +} + +static void copy_and_key( void *d, const void *s, const void *k ) +{ +#if defined( HAVE_UINT_32T ) + ((uint32_t*)d)[ 0] = ((uint32_t*)s)[ 0] ^ ((uint32_t*)k)[ 0]; + ((uint32_t*)d)[ 1] = ((uint32_t*)s)[ 1] ^ ((uint32_t*)k)[ 1]; + ((uint32_t*)d)[ 2] = ((uint32_t*)s)[ 2] ^ ((uint32_t*)k)[ 2]; + ((uint32_t*)d)[ 3] = ((uint32_t*)s)[ 3] ^ ((uint32_t*)k)[ 3]; +#elif 1 + ((uint8_t*)d)[ 0] = ((uint8_t*)s)[ 0] ^ ((uint8_t*)k)[ 0]; + ((uint8_t*)d)[ 1] = ((uint8_t*)s)[ 1] ^ ((uint8_t*)k)[ 1]; + ((uint8_t*)d)[ 2] = ((uint8_t*)s)[ 2] ^ ((uint8_t*)k)[ 2]; + ((uint8_t*)d)[ 3] = ((uint8_t*)s)[ 3] ^ ((uint8_t*)k)[ 3]; + ((uint8_t*)d)[ 4] = ((uint8_t*)s)[ 4] ^ ((uint8_t*)k)[ 4]; + ((uint8_t*)d)[ 5] = ((uint8_t*)s)[ 5] ^ ((uint8_t*)k)[ 5]; + ((uint8_t*)d)[ 6] = ((uint8_t*)s)[ 6] ^ ((uint8_t*)k)[ 6]; + ((uint8_t*)d)[ 7] = ((uint8_t*)s)[ 7] ^ ((uint8_t*)k)[ 7]; + ((uint8_t*)d)[ 8] = ((uint8_t*)s)[ 8] ^ ((uint8_t*)k)[ 8]; + ((uint8_t*)d)[ 9] = ((uint8_t*)s)[ 9] ^ ((uint8_t*)k)[ 9]; + ((uint8_t*)d)[10] = ((uint8_t*)s)[10] ^ ((uint8_t*)k)[10]; + ((uint8_t*)d)[11] = ((uint8_t*)s)[11] ^ ((uint8_t*)k)[11]; + ((uint8_t*)d)[12] = ((uint8_t*)s)[12] ^ ((uint8_t*)k)[12]; + ((uint8_t*)d)[13] = ((uint8_t*)s)[13] ^ ((uint8_t*)k)[13]; + ((uint8_t*)d)[14] = ((uint8_t*)s)[14] ^ ((uint8_t*)k)[14]; + ((uint8_t*)d)[15] = ((uint8_t*)s)[15] ^ ((uint8_t*)k)[15]; +#else + block_copy(d, s); + xor_block(d, k); +#endif +} + +static void add_round_key( uint8_t d[N_BLOCK], const uint8_t k[N_BLOCK] ) +{ + xor_block(d, k); +} + +static void shift_sub_rows( uint8_t st[N_BLOCK] ) +{ uint8_t tt; + + st[ 0] = s_box(st[ 0]); st[ 4] = s_box(st[ 4]); + st[ 8] = s_box(st[ 8]); st[12] = s_box(st[12]); + + tt = st[1]; st[ 1] = s_box(st[ 5]); st[ 5] = s_box(st[ 9]); + st[ 9] = s_box(st[13]); st[13] = s_box( tt ); + + tt = st[2]; st[ 2] = s_box(st[10]); st[10] = s_box( tt ); + tt = st[6]; st[ 6] = s_box(st[14]); st[14] = s_box( tt ); + + tt = st[15]; st[15] = s_box(st[11]); st[11] = s_box(st[ 7]); + st[ 7] = s_box(st[ 3]); st[ 3] = s_box( tt ); +} + +#if defined( AES_DEC_PREKEYED ) + +static void inv_shift_sub_rows( uint8_t st[N_BLOCK] ) +{ uint8_t tt; + + st[ 0] = is_box(st[ 0]); st[ 4] = is_box(st[ 4]); + st[ 8] = is_box(st[ 8]); st[12] = is_box(st[12]); + + tt = st[13]; st[13] = is_box(st[9]); st[ 9] = is_box(st[5]); + st[ 5] = is_box(st[1]); st[ 1] = is_box( tt ); + + tt = st[2]; st[ 2] = is_box(st[10]); st[10] = is_box( tt ); + tt = st[6]; st[ 6] = is_box(st[14]); st[14] = is_box( tt ); + + tt = st[3]; st[ 3] = is_box(st[ 7]); st[ 7] = is_box(st[11]); + st[11] = is_box(st[15]); st[15] = is_box( tt ); +} + +#endif + +#if defined( VERSION_1 ) + static void mix_sub_columns( uint8_t dt[N_BLOCK] ) + { uint8_t st[N_BLOCK]; + block_copy(st, dt); +#else + static void mix_sub_columns( uint8_t dt[N_BLOCK], uint8_t st[N_BLOCK] ) + { +#endif + dt[ 0] = gfm2_sb(st[0]) ^ gfm3_sb(st[5]) ^ s_box(st[10]) ^ s_box(st[15]); + dt[ 1] = s_box(st[0]) ^ gfm2_sb(st[5]) ^ gfm3_sb(st[10]) ^ s_box(st[15]); + dt[ 2] = s_box(st[0]) ^ s_box(st[5]) ^ gfm2_sb(st[10]) ^ gfm3_sb(st[15]); + dt[ 3] = gfm3_sb(st[0]) ^ s_box(st[5]) ^ s_box(st[10]) ^ gfm2_sb(st[15]); + + dt[ 4] = gfm2_sb(st[4]) ^ gfm3_sb(st[9]) ^ s_box(st[14]) ^ s_box(st[3]); + dt[ 5] = s_box(st[4]) ^ gfm2_sb(st[9]) ^ gfm3_sb(st[14]) ^ s_box(st[3]); + dt[ 6] = s_box(st[4]) ^ s_box(st[9]) ^ gfm2_sb(st[14]) ^ gfm3_sb(st[3]); + dt[ 7] = gfm3_sb(st[4]) ^ s_box(st[9]) ^ s_box(st[14]) ^ gfm2_sb(st[3]); + + dt[ 8] = gfm2_sb(st[8]) ^ gfm3_sb(st[13]) ^ s_box(st[2]) ^ s_box(st[7]); + dt[ 9] = s_box(st[8]) ^ gfm2_sb(st[13]) ^ gfm3_sb(st[2]) ^ s_box(st[7]); + dt[10] = s_box(st[8]) ^ s_box(st[13]) ^ gfm2_sb(st[2]) ^ gfm3_sb(st[7]); + dt[11] = gfm3_sb(st[8]) ^ s_box(st[13]) ^ s_box(st[2]) ^ gfm2_sb(st[7]); + + dt[12] = gfm2_sb(st[12]) ^ gfm3_sb(st[1]) ^ s_box(st[6]) ^ s_box(st[11]); + dt[13] = s_box(st[12]) ^ gfm2_sb(st[1]) ^ gfm3_sb(st[6]) ^ s_box(st[11]); + dt[14] = s_box(st[12]) ^ s_box(st[1]) ^ gfm2_sb(st[6]) ^ gfm3_sb(st[11]); + dt[15] = gfm3_sb(st[12]) ^ s_box(st[1]) ^ s_box(st[6]) ^ gfm2_sb(st[11]); + } + +#if defined( AES_DEC_PREKEYED ) + +#if defined( VERSION_1 ) + static void inv_mix_sub_columns( uint8_t dt[N_BLOCK] ) + { uint8_t st[N_BLOCK]; + block_copy(st, dt); +#else + static void inv_mix_sub_columns( uint8_t dt[N_BLOCK], uint8_t st[N_BLOCK] ) + { +#endif + dt[ 0] = is_box(gfm_e(st[ 0]) ^ gfm_b(st[ 1]) ^ gfm_d(st[ 2]) ^ gfm_9(st[ 3])); + dt[ 5] = is_box(gfm_9(st[ 0]) ^ gfm_e(st[ 1]) ^ gfm_b(st[ 2]) ^ gfm_d(st[ 3])); + dt[10] = is_box(gfm_d(st[ 0]) ^ gfm_9(st[ 1]) ^ gfm_e(st[ 2]) ^ gfm_b(st[ 3])); + dt[15] = is_box(gfm_b(st[ 0]) ^ gfm_d(st[ 1]) ^ gfm_9(st[ 2]) ^ gfm_e(st[ 3])); + + dt[ 4] = is_box(gfm_e(st[ 4]) ^ gfm_b(st[ 5]) ^ gfm_d(st[ 6]) ^ gfm_9(st[ 7])); + dt[ 9] = is_box(gfm_9(st[ 4]) ^ gfm_e(st[ 5]) ^ gfm_b(st[ 6]) ^ gfm_d(st[ 7])); + dt[14] = is_box(gfm_d(st[ 4]) ^ gfm_9(st[ 5]) ^ gfm_e(st[ 6]) ^ gfm_b(st[ 7])); + dt[ 3] = is_box(gfm_b(st[ 4]) ^ gfm_d(st[ 5]) ^ gfm_9(st[ 6]) ^ gfm_e(st[ 7])); + + dt[ 8] = is_box(gfm_e(st[ 8]) ^ gfm_b(st[ 9]) ^ gfm_d(st[10]) ^ gfm_9(st[11])); + dt[13] = is_box(gfm_9(st[ 8]) ^ gfm_e(st[ 9]) ^ gfm_b(st[10]) ^ gfm_d(st[11])); + dt[ 2] = is_box(gfm_d(st[ 8]) ^ gfm_9(st[ 9]) ^ gfm_e(st[10]) ^ gfm_b(st[11])); + dt[ 7] = is_box(gfm_b(st[ 8]) ^ gfm_d(st[ 9]) ^ gfm_9(st[10]) ^ gfm_e(st[11])); + + dt[12] = is_box(gfm_e(st[12]) ^ gfm_b(st[13]) ^ gfm_d(st[14]) ^ gfm_9(st[15])); + dt[ 1] = is_box(gfm_9(st[12]) ^ gfm_e(st[13]) ^ gfm_b(st[14]) ^ gfm_d(st[15])); + dt[ 6] = is_box(gfm_d(st[12]) ^ gfm_9(st[13]) ^ gfm_e(st[14]) ^ gfm_b(st[15])); + dt[11] = is_box(gfm_b(st[12]) ^ gfm_d(st[13]) ^ gfm_9(st[14]) ^ gfm_e(st[15])); + } + +#endif + +#if defined( AES_ENC_PREKEYED ) || defined( AES_DEC_PREKEYED ) + +/* Set the cipher key for the pre-keyed version */ + +return_type lorawan_aes_set_key( const uint8_t key[], length_type keylen, aes_context ctx[1] ) +{ + uint8_t cc, rc, hi; + + switch( keylen ) + { + case 16: + case 24: + case 32: + break; + default: + ctx->rnd = 0; + return ( uint8_t )-1; + } + block_copy_nn(ctx->ksch, key, keylen); + hi = (keylen + 28) << 2;//16+28 <<2 10110000(44*4=176) + ctx->rnd = (hi >> 4) - 1;//00001010 + for( cc = keylen, rc = 1; cc < hi; cc += 4 ) + { uint8_t tt, t0, t1, t2, t3; + + t0 = ctx->ksch[cc - 4]; + t1 = ctx->ksch[cc - 3]; + t2 = ctx->ksch[cc - 2]; + t3 = ctx->ksch[cc - 1]; + if( cc % keylen == 0 ) + { + tt = t0; + t0 = s_box(t1) ^ rc; + t1 = s_box(t2); + t2 = s_box(t3); + t3 = s_box(tt); + rc = f2(rc); + } + else if( keylen > 24 && cc % keylen == 16 ) + { + t0 = s_box(t0); + t1 = s_box(t1); + t2 = s_box(t2); + t3 = s_box(t3); + } + tt = cc - keylen; + ctx->ksch[cc + 0] = ctx->ksch[tt + 0] ^ t0; + ctx->ksch[cc + 1] = ctx->ksch[tt + 1] ^ t1; + ctx->ksch[cc + 2] = ctx->ksch[tt + 2] ^ t2; + ctx->ksch[cc + 3] = ctx->ksch[tt + 3] ^ t3; + } + return 0; +} + +#endif + +#if defined( AES_ENC_PREKEYED ) + +/* Encrypt a single block of 16 bytes */ + +return_type lora_aes_encrypt( const uint8_t in[N_BLOCK], uint8_t out[N_BLOCK], const aes_context ctx[1] ) +{ + if( ctx->rnd ) + { + uint8_t s1[N_BLOCK], r; + copy_and_key( s1, in, ctx->ksch ); + + for( r = 1 ; r < ctx->rnd ; ++r ) +#if defined( VERSION_1 ) + { + mix_sub_columns( s1 ); + add_round_key( s1, ctx->ksch + r * N_BLOCK); + } +#else + { uint8_t s2[N_BLOCK]; + mix_sub_columns( s2, s1 ); + copy_and_key( s1, s2, ctx->ksch + r * N_BLOCK); + } +#endif + shift_sub_rows( s1 ); + copy_and_key( out, s1, ctx->ksch + r * N_BLOCK ); + } + else + return ( uint8_t )-1; + return 0; +} + +/* CBC encrypt a number of blocks (input and return an IV) */ + +return_type lorawan_aes_cbc_encrypt( const uint8_t *in, uint8_t *out, + int32_t n_block, uint8_t iv[N_BLOCK], const aes_context ctx[1] ) +{ + + while(n_block--) + { + xor_block(iv, in); + if(lora_aes_encrypt(iv, iv, ctx) != EXIT_SUCCESS) + return EXIT_FAILURE; + //memcpy(out, iv, N_BLOCK); + block_copy(out, iv); + in += N_BLOCK; + out += N_BLOCK; + } + return EXIT_SUCCESS; +} + +#endif + +#if defined( AES_DEC_PREKEYED ) + +/* Decrypt a single block of 16 bytes */ + +return_type aes_decrypt( const uint8_t in[N_BLOCK], uint8_t out[N_BLOCK], const aes_context ctx[1] ) +{ + if( ctx->rnd ) + { + uint8_t s1[N_BLOCK], r; + copy_and_key( s1, in, ctx->ksch + ctx->rnd * N_BLOCK ); + inv_shift_sub_rows( s1 ); + + for( r = ctx->rnd ; --r ; ) +#if defined( VERSION_1 ) + { + add_round_key( s1, ctx->ksch + r * N_BLOCK ); + inv_mix_sub_columns( s1 ); + } +#else + { uint8_t s2[N_BLOCK]; + copy_and_key( s2, s1, ctx->ksch + r * N_BLOCK ); + inv_mix_sub_columns( s1, s2 ); + } +#endif + copy_and_key( out, s1, ctx->ksch ); + } + else + return -1; + return 0; +} + +/* CBC decrypt a number of blocks (input and return an IV) */ + +return_type aes_cbc_decrypt( const uint8_t *in, uint8_t *out, + int32_t n_block, uint8_t iv[N_BLOCK], const aes_context ctx[1] ) +{ + while(n_block--) + { uint8_t tmp[N_BLOCK]; + + //memcpy(tmp, in, N_BLOCK); + block_copy(tmp, in); + if(aes_decrypt(in, out, ctx) != EXIT_SUCCESS) + return EXIT_FAILURE; + xor_block(out, iv); + //memcpy(iv, tmp, N_BLOCK); + block_copy(iv, tmp); + in += N_BLOCK; + out += N_BLOCK; + } + return EXIT_SUCCESS; +} + +#endif + +#if defined( AES_ENC_128_OTFK ) + +/* The 'on the fly' encryption key update for for 128 bit keys */ + +static void update_encrypt_key_128( uint8_t k[N_BLOCK], uint8_t *rc ) +{ uint8_t cc; + + k[0] ^= s_box(k[13]) ^ *rc; + k[1] ^= s_box(k[14]); + k[2] ^= s_box(k[15]); + k[3] ^= s_box(k[12]); + *rc = f2( *rc ); + + for(cc = 4; cc < 16; cc += 4 ) + { + k[cc + 0] ^= k[cc - 4]; + k[cc + 1] ^= k[cc - 3]; + k[cc + 2] ^= k[cc - 2]; + k[cc + 3] ^= k[cc - 1]; + } +} + +/* Encrypt a single block of 16 bytes with 'on the fly' 128 bit keying */ + +void aes_encrypt_128( const uint8_t in[N_BLOCK], uint8_t out[N_BLOCK], + const uint8_t key[N_BLOCK], uint8_t o_key[N_BLOCK] ) +{ uint8_t s1[N_BLOCK], r, rc = 1; + + if(o_key != key) + block_copy( o_key, key ); + copy_and_key( s1, in, o_key ); + + for( r = 1 ; r < 10 ; ++r ) +#if defined( VERSION_1 ) + { + mix_sub_columns( s1 ); + update_encrypt_key_128( o_key, &rc ); + add_round_key( s1, o_key ); + } +#else + { uint8_t s2[N_BLOCK]; + mix_sub_columns( s2, s1 ); + update_encrypt_key_128( o_key, &rc ); + copy_and_key( s1, s2, o_key ); + } +#endif + + shift_sub_rows( s1 ); + update_encrypt_key_128( o_key, &rc ); + copy_and_key( out, s1, o_key ); +} + +#endif + +#if defined( AES_DEC_128_OTFK ) + +/* The 'on the fly' decryption key update for for 128 bit keys */ + +static void update_decrypt_key_128( uint8_t k[N_BLOCK], uint8_t *rc ) +{ uint8_t cc; + + for( cc = 12; cc > 0; cc -= 4 ) + { + k[cc + 0] ^= k[cc - 4]; + k[cc + 1] ^= k[cc - 3]; + k[cc + 2] ^= k[cc - 2]; + k[cc + 3] ^= k[cc - 1]; + } + *rc = d2(*rc); + k[0] ^= s_box(k[13]) ^ *rc; + k[1] ^= s_box(k[14]); + k[2] ^= s_box(k[15]); + k[3] ^= s_box(k[12]); +} + +/* Decrypt a single block of 16 bytes with 'on the fly' 128 bit keying */ + +void aes_decrypt_128( const uint8_t in[N_BLOCK], uint8_t out[N_BLOCK], + const uint8_t key[N_BLOCK], uint8_t o_key[N_BLOCK] ) +{ + uint8_t s1[N_BLOCK], r, rc = 0x6c; + if(o_key != key) + block_copy( o_key, key ); + + copy_and_key( s1, in, o_key ); + inv_shift_sub_rows( s1 ); + + for( r = 10 ; --r ; ) +#if defined( VERSION_1 ) + { + update_decrypt_key_128( o_key, &rc ); + add_round_key( s1, o_key ); + inv_mix_sub_columns( s1 ); + } +#else + { uint8_t s2[N_BLOCK]; + update_decrypt_key_128( o_key, &rc ); + copy_and_key( s2, s1, o_key ); + inv_mix_sub_columns( s1, s2 ); + } +#endif + update_decrypt_key_128( o_key, &rc ); + copy_and_key( out, s1, o_key ); +} + +#endif + +#if defined( AES_ENC_256_OTFK ) + +/* The 'on the fly' encryption key update for for 256 bit keys */ + +static void update_encrypt_key_256( uint8_t k[2 * N_BLOCK], uint8_t *rc ) +{ uint8_t cc; + + k[0] ^= s_box(k[29]) ^ *rc; + k[1] ^= s_box(k[30]); + k[2] ^= s_box(k[31]); + k[3] ^= s_box(k[28]); + *rc = f2( *rc ); + + for(cc = 4; cc < 16; cc += 4) + { + k[cc + 0] ^= k[cc - 4]; + k[cc + 1] ^= k[cc - 3]; + k[cc + 2] ^= k[cc - 2]; + k[cc + 3] ^= k[cc - 1]; + } + + k[16] ^= s_box(k[12]); + k[17] ^= s_box(k[13]); + k[18] ^= s_box(k[14]); + k[19] ^= s_box(k[15]); + + for( cc = 20; cc < 32; cc += 4 ) + { + k[cc + 0] ^= k[cc - 4]; + k[cc + 1] ^= k[cc - 3]; + k[cc + 2] ^= k[cc - 2]; + k[cc + 3] ^= k[cc - 1]; + } +} + +/* Encrypt a single block of 16 bytes with 'on the fly' 256 bit keying */ + +void aes_encrypt_256( const uint8_t in[N_BLOCK], uint8_t out[N_BLOCK], + const uint8_t key[2 * N_BLOCK], uint8_t o_key[2 * N_BLOCK] ) +{ + uint8_t s1[N_BLOCK], r, rc = 1; + if(o_key != key) + { + block_copy( o_key, key ); + block_copy( o_key + 16, key + 16 ); + } + copy_and_key( s1, in, o_key ); + + for( r = 1 ; r < 14 ; ++r ) +#if defined( VERSION_1 ) + { + mix_sub_columns(s1); + if( r & 1 ) + add_round_key( s1, o_key + 16 ); + else + { + update_encrypt_key_256( o_key, &rc ); + add_round_key( s1, o_key ); + } + } +#else + { uint8_t s2[N_BLOCK]; + mix_sub_columns( s2, s1 ); + if( r & 1 ) + copy_and_key( s1, s2, o_key + 16 ); + else + { + update_encrypt_key_256( o_key, &rc ); + copy_and_key( s1, s2, o_key ); + } + } +#endif + + shift_sub_rows( s1 ); + update_encrypt_key_256( o_key, &rc ); + copy_and_key( out, s1, o_key ); +} + +#endif + +#if defined( AES_DEC_256_OTFK ) + +/* The 'on the fly' encryption key update for for 256 bit keys */ + +static void update_decrypt_key_256( uint8_t k[2 * N_BLOCK], uint8_t *rc ) +{ uint8_t cc; + + for(cc = 28; cc > 16; cc -= 4) + { + k[cc + 0] ^= k[cc - 4]; + k[cc + 1] ^= k[cc - 3]; + k[cc + 2] ^= k[cc - 2]; + k[cc + 3] ^= k[cc - 1]; + } + + k[16] ^= s_box(k[12]); + k[17] ^= s_box(k[13]); + k[18] ^= s_box(k[14]); + k[19] ^= s_box(k[15]); + + for(cc = 12; cc > 0; cc -= 4) + { + k[cc + 0] ^= k[cc - 4]; + k[cc + 1] ^= k[cc - 3]; + k[cc + 2] ^= k[cc - 2]; + k[cc + 3] ^= k[cc - 1]; + } + + *rc = d2(*rc); + k[0] ^= s_box(k[29]) ^ *rc; + k[1] ^= s_box(k[30]); + k[2] ^= s_box(k[31]); + k[3] ^= s_box(k[28]); +} + +/* Decrypt a single block of 16 bytes with 'on the fly' + 256 bit keying +*/ +void aes_decrypt_256( const uint8_t in[N_BLOCK], uint8_t out[N_BLOCK], + const uint8_t key[2 * N_BLOCK], uint8_t o_key[2 * N_BLOCK] ) +{ + uint8_t s1[N_BLOCK], r, rc = 0x80; + + if(o_key != key) + { + block_copy( o_key, key ); + block_copy( o_key + 16, key + 16 ); + } + + copy_and_key( s1, in, o_key ); + inv_shift_sub_rows( s1 ); + + for( r = 14 ; --r ; ) +#if defined( VERSION_1 ) + { + if( ( r & 1 ) ) + { + update_decrypt_key_256( o_key, &rc ); + add_round_key( s1, o_key + 16 ); + } + else + add_round_key( s1, o_key ); + inv_mix_sub_columns( s1 ); + } +#else + { uint8_t s2[N_BLOCK]; + if( ( r & 1 ) ) + { + update_decrypt_key_256( o_key, &rc ); + copy_and_key( s2, s1, o_key + 16 ); + } + else + copy_and_key( s2, s1, o_key ); + inv_mix_sub_columns( s1, s2 ); + } +#endif + copy_and_key( out, s1, o_key ); +} + +#endif diff --git a/main/aes.h b/main/aes.h new file mode 100644 index 0000000..95e1a3f --- /dev/null +++ b/main/aes.h @@ -0,0 +1,160 @@ +/* + --------------------------------------------------------------------------- + Copyright (c) 1998-2008, Brian Gladman, Worcester, UK. All rights reserved. + + LICENSE TERMS + + The redistribution and use of this software (with or without changes) + is allowed without the payment of fees or royalties provided that: + + 1. source code distributions include the above copyright notice, this + list of conditions and the following disclaimer; + + 2. binary distributions include the above copyright notice, this list + of conditions and the following disclaimer in their documentation; + + 3. the name of the copyright holder is not used to endorse products + built using this software without specific written permission. + + DISCLAIMER + + This software is provided 'as is' with no explicit or implied warranties + in respect of its properties, including, but not limited to, correctness + and/or fitness for purpose. + --------------------------------------------------------------------------- + Issue 09/09/2006 + + This is an AES implementation that uses only 8-bit byte operations on the + cipher state. + */ + +#ifndef AES_H +#define AES_H + +#if 1 +# define AES_ENC_PREKEYED /* AES encryption with a precomputed key schedule */ +#endif +#if 0 +# define AES_DEC_PREKEYED /* AES decryption with a precomputed key schedule */ +#endif +#if 0 +# define AES_ENC_128_OTFK /* AES encryption with 'on the fly' 128 bit keying */ +#endif +#if 0 +# define AES_DEC_128_OTFK /* AES decryption with 'on the fly' 128 bit keying */ +#endif +#if 0 +# define AES_ENC_256_OTFK /* AES encryption with 'on the fly' 256 bit keying */ +#endif +#if 0 +# define AES_DEC_256_OTFK /* AES decryption with 'on the fly' 256 bit keying */ +#endif + +#define N_ROW 4 +#define N_COL 4 +#define N_BLOCK (N_ROW * N_COL) +#define N_MAX_ROUNDS 14 + +typedef uint8_t return_type; + +/* Warning: The key length for 256 bit keys overflows a byte + (see comment below) +*/ + +typedef uint8_t length_type; + +typedef struct +{ uint8_t ksch[(N_MAX_ROUNDS + 1) * N_BLOCK]; + uint8_t rnd; +} aes_context; + +/* The following calls are for a precomputed key schedule + + NOTE: If the length_type used for the key length is an + unsigned 8-bit character, a key length of 256 bits must + be entered as a length in bytes (valid inputs are hence + 128, 192, 16, 24 and 32). +*/ + +#if defined( AES_ENC_PREKEYED ) || defined( AES_DEC_PREKEYED ) + +return_type lorawan_aes_set_key( const uint8_t key[], + length_type keylen, + aes_context ctx[1] ); +#endif + +#if defined( AES_ENC_PREKEYED ) + +return_type lora_aes_encrypt( const uint8_t in[N_BLOCK], + uint8_t out[N_BLOCK], + const aes_context ctx[1] ); + +return_type lorawan_aes_cbc_encrypt( const uint8_t *in, + uint8_t *out, + int32_t n_block, + uint8_t iv[N_BLOCK], + const aes_context ctx[1] ); +#endif + +#if defined( AES_DEC_PREKEYED ) + +return_type aes_decrypt( const uint8_t in[N_BLOCK], + uint8_t out[N_BLOCK], + const aes_context ctx[1] ); + +return_type aes_cbc_decrypt( const uint8_t *in, + uint8_t *out, + int32_t n_block, + uint8_t iv[N_BLOCK], + const aes_context ctx[1] ); +#endif + +/* The following calls are for 'on the fly' keying. In this case the + encryption and decryption keys are different. + + The encryption subroutines take a key in an array of bytes in + key[L] where L is 16, 24 or 32 bytes for key lengths of 128, + 192, and 256 bits respectively. They then encrypts the input + data, in[] with this key and put the reult in the output array + out[]. In addition, the second key array, o_key[L], is used + to output the key that is needed by the decryption subroutine + to reverse the encryption operation. The two key arrays can + be the same array but in this case the original key will be + overwritten. + + In the same way, the decryption subroutines output keys that + can be used to reverse their effect when used for encryption. + + Only 128 and 256 bit keys are supported in these 'on the fly' + modes. +*/ + +#if defined( AES_ENC_128_OTFK ) +void aes_encrypt_128( const uint8_t in[N_BLOCK], + uint8_t out[N_BLOCK], + const uint8_t key[N_BLOCK], + uint8_t o_key[N_BLOCK] ); +#endif + +#if defined( AES_DEC_128_OTFK ) +void aes_decrypt_128( const uint8_t in[N_BLOCK], + uint8_t out[N_BLOCK], + const uint8_t key[N_BLOCK], + uint8_t o_key[N_BLOCK] ); +#endif + +#if defined( AES_ENC_256_OTFK ) +void aes_encrypt_256( const uint8_t in[N_BLOCK], + uint8_t out[N_BLOCK], + const uint8_t key[2 * N_BLOCK], + uint8_t o_key[2 * N_BLOCK] ); +#endif + +#if defined( AES_DEC_256_OTFK ) +void aes_decrypt_256( const uint8_t in[N_BLOCK], + uint8_t out[N_BLOCK], + const uint8_t key[2 * N_BLOCK], + uint8_t o_key[2 * N_BLOCK] ); +#endif + +#endif diff --git a/main/cmac.c b/main/cmac.c new file mode 100644 index 0000000..41156b1 --- /dev/null +++ b/main/cmac.c @@ -0,0 +1,153 @@ +/************************************************************************** +Copyright (C) 2009 Lander Casado, Philippas Tsigas + +All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining +a copy of this software and associated documentation files +(the "Software"), to deal with the Software without restriction, including +without limitation the rights to use, copy, modify, merge, publish, +distribute, sublicense, and/or sell copies of the Software, and to +permit persons to whom the Software is furnished to do so, subject to +the following conditions: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimers. Redistributions in +binary form must reproduce the above copyright notice, this list of +conditions and the following disclaimers in the documentation and/or +other materials provided with the distribution. + +In no event shall the authors or copyright holders be liable for any special, +incidental, indirect or consequential damages of any kind, or any damages +whatsoever resulting from loss of use, data or profits, whether or not +advised of the possibility of damage, and on any theory of liability, +arising out of or in connection with the use or performance of this software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS WITH THE SOFTWARE + +*****************************************************************************/ +//#include +//#include +#include +#include "aes.h" +#include "cmac.h" +#include "cmacutil.h" + +#define LSHIFT(v, r) do { \ + int32_t i; \ + for (i = 0; i < 15; i++) \ + (r)[i] = (v)[i] << 1 | (v)[i + 1] >> 7; \ + (r)[15] = (v)[15] << 1; \ + } while (0) + +#define XOR(v, r) do { \ + int32_t i; \ + for (i = 0; i < 16; i++) \ + { \ + (r)[i] = (r)[i] ^ (v)[i]; \ + } \ + } while (0) \ + + +void AES_CMAC_Init(AES_CMAC_CTX *ctx) +{ + memset1(ctx->X, 0, sizeof ctx->X); + ctx->M_n = 0; + memset1(ctx->rijndael.ksch, '\0', 240); +} + +void AES_CMAC_SetKey(AES_CMAC_CTX *ctx, const uint8_t key[AES_CMAC_KEY_LENGTH]) +{ + //rijndael_set_key_enc_only(&ctx->rijndael, key, 128); + lorawan_aes_set_key( key, AES_CMAC_KEY_LENGTH, &ctx->rijndael); +} + +void AES_CMAC_Update(AES_CMAC_CTX *ctx, const uint8_t *data, uint32_t len) +{ + uint32_t mlen; + uint8_t in[16]; + + if (ctx->M_n > 0) { + mlen = MIN(16 - ctx->M_n, len); + memcpy1(ctx->M_last + ctx->M_n, data, mlen); + ctx->M_n += mlen; + if (ctx->M_n < 16 || len == mlen) + return; + XOR(ctx->M_last, ctx->X); + //rijndael_encrypt(&ctx->rijndael, ctx->X, ctx->X); + lora_aes_encrypt( ctx->X, ctx->X, &ctx->rijndael); + data += mlen; + len -= mlen; + } + while (len > 16) { /* not last block */ + + XOR(data, ctx->X); + //rijndael_encrypt(&ctx->rijndael, ctx->X, ctx->X); + + memcpy1(in, &ctx->X[0], 16); //Bestela ez du ondo iten + lora_aes_encrypt( in, in, &ctx->rijndael); + memcpy1(&ctx->X[0], in, 16); + + data += 16; + len -= 16; + } + /* potential last block, save it */ + memcpy1(ctx->M_last, data, len); + ctx->M_n = len; +} + +void AES_CMAC_Final(uint8_t digest[AES_CMAC_DIGEST_LENGTH], AES_CMAC_CTX *ctx) +{ + uint8_t K[16]; + uint8_t in[16]; + /* generate subkey K1 */ + memset1(K, '\0', 16); + + //rijndael_encrypt(&ctx->rijndael, K, K); + + lora_aes_encrypt( K, K, &ctx->rijndael); + + if (K[0] & 0x80) { + LSHIFT(K, K); + K[15] ^= 0x87; + } else + LSHIFT(K, K); + + + if (ctx->M_n == 16) { + /* last block was a complete block */ + XOR(K, ctx->M_last); + + } else { + /* generate subkey K2 */ + if (K[0] & 0x80) { + LSHIFT(K, K); + K[15] ^= 0x87; + } else + LSHIFT(K, K); + + /* padding(M_last) */ + ctx->M_last[ctx->M_n] = 0x80; + while (++ctx->M_n < 16) + ctx->M_last[ctx->M_n] = 0; + + XOR(K, ctx->M_last); + + + } + XOR(ctx->M_last, ctx->X); + + //rijndael_encrypt(&ctx->rijndael, ctx->X, digest); + + memcpy1(in, &ctx->X[0], 16); //Bestela ez du ondo iten + lora_aes_encrypt(in, digest, &ctx->rijndael); + memset1(K, 0, sizeof K); + +} + diff --git a/main/cmac.h b/main/cmac.h new file mode 100644 index 0000000..c12e970 --- /dev/null +++ b/main/cmac.h @@ -0,0 +1,63 @@ +/************************************************************************** +Copyright (C) 2009 Lander Casado, Philippas Tsigas + +All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining +a copy of this software and associated documentation files +(the "Software"), to deal with the Software without restriction, including +without limitation the rights to use, copy, modify, merge, publish, +distribute, sublicense, and/or sell copies of the Software, and to +permit persons to whom the Software is furnished to do so, subject to +the following conditions: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimers. Redistributions in +binary form must reproduce the above copyright notice, this list of +conditions and the following disclaimers in the documentation and/or +other materials provided with the distribution. + +In no event shall the authors or copyright holders be liable for any special, +incidental, indirect or consequential damages of any kind, or any damages +whatsoever resulting from loss of use, data or profits, whether or not +advised of the possibility of damage, and on any theory of liability, +arising out of or in connection with the use or performance of this software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS WITH THE SOFTWARE + +*****************************************************************************/ + +#ifndef _CMAC_H_ +#define _CMAC_H_ + +#include "aes.h" + +#define AES_CMAC_KEY_LENGTH 16 +#define AES_CMAC_DIGEST_LENGTH 16 + +typedef struct _AES_CMAC_CTX { + aes_context rijndael; + uint8_t X[16]; + uint8_t M_last[16]; + uint32_t M_n; + } AES_CMAC_CTX; + +//#include + +//__BEGIN_DECLS +void AES_CMAC_Init(AES_CMAC_CTX * ctx); +void AES_CMAC_SetKey(AES_CMAC_CTX * ctx, const uint8_t key[AES_CMAC_KEY_LENGTH]); +void AES_CMAC_Update(AES_CMAC_CTX * ctx, const uint8_t * data, uint32_t len); + // __attribute__((__bounded__(__string__,2,3))); +void AES_CMAC_Final(uint8_t digest[AES_CMAC_DIGEST_LENGTH], AES_CMAC_CTX * ctx); + // __attribute__((__bounded__(__minbytes__,1,AES_CMAC_DIGEST_LENGTH))); +//__END_DECLS + +#endif /* _CMAC_H_ */ + diff --git a/main/cmacutil.c b/main/cmacutil.c new file mode 100644 index 0000000..f5706c6 --- /dev/null +++ b/main/cmacutil.c @@ -0,0 +1,86 @@ +/* + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2013 Semtech + +Description: Helper functions implementation + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis and Gregory Cristian +*/ +#include +#include +#include +// #include "board.h" +#include "cmacutil.h" + +/*! + * Redefinition of rand() and srand() standard C functions. + * These functions are redefined in order to get the same behavior across + * different compiler toolchains implementations. + */ +// Standard random functions redefinition start +#define RAND_LOCAL_MAX 2147483647L + +static uint32_t next = 1; + +int32_t rand1( void ) +{ + return ( ( next = next * 1103515245L + 12345L ) % RAND_LOCAL_MAX ); +} + +void srand1( uint32_t seed ) +{ + next = seed; +} +// Standard random functions redefinition end + +int32_t randr( int32_t min, int32_t max ) +{ + return ( int32_t )rand1( ) % ( max - min + 1 ) + min; +} + +void memcpy1( uint8_t *dst, const uint8_t *src, uint16_t size ) +{ + while( size-- ) + { + *dst++ = *src++; + } +} + +void memcpyr( uint8_t *dst, const uint8_t *src, uint16_t size ) +{ + dst = dst + ( size - 1 ); + while( size-- ) + { + *dst-- = *src++; + } +} + +void memset1( uint8_t *dst, uint8_t value, uint16_t size ) +{ + while( size-- ) + { + *dst++ = value; + } +} + +int8_t Nibble2HexChar( uint8_t a ) +{ + if( a < 10 ) + { + return '0' + a; + } + else if( a < 16 ) + { + return 'A' + ( a - 10 ); + } + else + { + return '?'; + } +} diff --git a/main/cmacutil.h b/main/cmacutil.h new file mode 100644 index 0000000..5d1204c --- /dev/null +++ b/main/cmacutil.h @@ -0,0 +1,109 @@ +/* + / _____) _ | | +( (____ _____ ____ _| |_ _____ ____| |__ + \____ \| ___ | (_ _) ___ |/ ___) _ \ + _____) ) ____| | | || |_| ____( (___| | | | +(______/|_____)_|_|_| \__)_____)\____)_| |_| + (C)2013 Semtech + +Description: Helper functions implementation + +License: Revised BSD License, see LICENSE.TXT file include in the project + +Maintainer: Miguel Luis and Gregory Cristian +*/ +#ifndef __UTILITIES_H__ +#define __UTILITIES_H__ +#include + +#ifdef __cplusplus +extern "C"{ +#endif + +/*! + * \brief Returns the minimum value between a and b + * + * \param [IN] a 1st value + * \param [IN] b 2nd value + * \retval minValue Minimum value + */ +#define MIN( a, b ) ( ( ( a ) < ( b ) ) ? ( a ) : ( b ) ) + +/*! + * \brief Returns the maximum value between a and b + * + * \param [IN] a 1st value + * \param [IN] b 2nd value + * \retval maxValue Maximum value + */ +#define MAX( a, b ) ( ( ( a ) > ( b ) ) ? ( a ) : ( b ) ) + +/*! + * \brief Returns 2 raised to the power of n + * + * \param [IN] n power value + * \retval result of raising 2 to the power n + */ +#define POW2( n ) ( 1 << n ) + +/*! + * \brief Initializes the pseudo random generator initial value + * + * \param [IN] seed Pseudo random generator initial value + */ +void srand1( uint32_t seed ); + +/*! + * \brief Computes a random number between min and max + * + * \param [IN] min range minimum value + * \param [IN] max range maximum value + * \retval random random value in range min..max + */ +int32_t randr( int32_t min, int32_t max ); + +/*! + * \brief Copies size elements of src array to dst array + * + * \remark STM32 Standard memcpy function only works on pointers that are aligned + * + * \param [OUT] dst Destination array + * \param [IN] src Source array + * \param [IN] size Number of bytes to be copied + */ +void memcpy1( uint8_t *dst, const uint8_t *src, uint16_t size ); + +/*! + * \brief Copies size elements of src array to dst array reversing the byte order + * + * \param [OUT] dst Destination array + * \param [IN] src Source array + * \param [IN] size Number of bytes to be copied + */ +void memcpyr( uint8_t *dst, const uint8_t *src, uint16_t size ); + +/*! + * \brief Set size elements of dst array with value + * + * \remark STM32 Standard memset function only works on pointers that are aligned + * + * \param [OUT] dst Destination array + * \param [IN] value Default value + * \param [IN] size Number of bytes to be copied + */ +void memset1( uint8_t *dst, uint8_t value, uint16_t size ); + +/*! + * \brief Converts a nibble to an hexadecimal character + * + * \param [IN] a Nibble to be converted + * \retval hexChar Converted hexadecimal character + */ +int8_t Nibble2HexChar( uint8_t a ); + + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif // __UTILITIES_H__ diff --git a/main/config.h b/main/config.h index ee8806b..72419cf 100644 --- a/main/config.h +++ b/main/config.h @@ -9,34 +9,35 @@ // #define WITH_HELTEC_V2 // HELTEC module v2 // #define WITH_TTGO // TTGO module: PCB LED on GPIO2, GPIO25 free to use as DAC2 output // #define WITH_TBEAM // T-Beam module -#define WITH_TBEAM_V10 // T-Beam module +// #define WITH_TBEAM_V10 // T-Beam module // #define WITH_M5_JACEK // JACEK M5 ESP32 OGN-Tracker -// #define WITH_FollowMe // by Avionix +#define WITH_FollowMe // by Avionix // #define WITH_ILI9341 // 320x240 M5stack // #define WITH_ST7789 // IPS 240x240 ST7789 // #define WITH_TFT_LCD // TFT LCD // #define WITH_OLED // OLED display on the I2C: some TTGO modules are without OLED display // #define WITH_OLED2 // 2nd OLED display, I2C address next higher -// #define WITH_U8G2_OLED // I2C OLED through the U8g2 library -// #define WITH_U8G2_SH1106 +#define WITH_U8G2_OLED // I2C OLED through the U8g2 library +#define WITH_U8G2_SH1106 // correct controller for the bigger OLED +#define WITH_U8G2_FLIP // flip the OLED screen (rotate by 180deg) #define WITH_RFM95 // RF chip selection: both HELTEC and TTGO use sx1276 which is same as RFM95 // #define WITH_SLEEP // with software sleep mode controlled by the long-press on the button -#define WITH_AXP // with AXP192 power controller (T-BEAM V1.0) +// #define WITH_AXP // with AXP192 power controller (T-BEAM V1.0) // #define WITH_BQ // with BQ24295 power controller (FollowMe) // #define WITH_LED_RX // #define WITH_LED_TX -// #define WITH_GPS_ENABLE // use GPS_ENABLE control line to turn the GPS ON/OFF +#define WITH_GPS_ENABLE // use GPS_ENABLE control line to turn the GPS ON/OFF #define WITH_GPS_PPS // use the PPS signal from GPS for precise time-sync. #define WITH_GPS_CONFIG // attempt to configure higher GPS baud rate and airborne mode -#define WITH_GPS_UBX // GPS understands UBX -// #define WITH_GPS_MTK // GPS understands MTK +// #define WITH_GPS_UBX // GPS understands UBX +#define WITH_GPS_MTK // GPS understands MTK // #define WITH_GPS_SRF // #define WITH_MAVLINK @@ -45,12 +46,19 @@ // #define WITH_BMP180 // BMP180 pressure sensor // #define WITH_BMP280 // BMP280 pressure sensor -// #define WITH_BME280 // BMP280 with humidity (but still works with BMP280) +#define WITH_BME280 // BMP280 with humidity (but still works with BMP280) // #define WITH_MS5607 // MS5607 pressure sensor // #define WITH_MS5611 // MS5611 pressure sensor +// #define WITH_BMX055 // BMX055 magnetic and IMU sensor + +#define WITH_LORAWAN // LoRaWAN connectivity +#define WITH_FANET // FANET transmission and reception + #define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000 // #define WITH_POGNT +// #define WITH_GDL90 +// #define WITH_PGAV5 #define WITH_LOOKOUT #define WITH_CONFIG // interpret the console input: $POGNS to change parameters @@ -61,14 +69,15 @@ // #define WITH_KNOB // #define WITH_VARIO -// #define WITH_SD // use the SD card in SPI mode and FAT file system -#define WITH_SPIFFS_FAT +#define WITH_SD // use the SD card in SPI mode and FAT file system #define WITH_SPIFFS // use SPIFFS file system in Flash -#define WITH_LOG // log own positions and other received to SPIFFS and possibly to uSD +#define WITH_SPIFFS_FAT +#define WITH_LOG // log own positions and other received to SPIFFS +#define WITH_SDLOG // log own position and other data to uSD card +// #define WITH_STRATUX #define WITH_BT_SPP // Bluetooth serial port for smartphone/tablet link // #define WITH_WIFI // attempt to connect to the wifi router for uploading the log files -// #define WITH_SPIFFS_LOG // log transmitted and received packets to SPIFFS // #define WITH_ENCRYPT // Encrypt (optionally) the position diff --git a/main/disp.cpp b/main/disp.cpp index b881b48..16e946c 100644 --- a/main/disp.cpp +++ b/main/disp.cpp @@ -25,7 +25,7 @@ #include "disp_lcd.h" #ifdef WITH_U8G2_OLED -const uint8_t DISP_Pages = 10; +const uint8_t DISP_Pages = 11; static uint8_t DISP_Page = 0; #endif #if defined(WITH_ST7789) || defined(WITH_ILI9341) @@ -191,6 +191,7 @@ void vTaskDISP(void* pvParameters) case 7: OLED_DrawRelay (&U8G2_OLED, GPS); break; case 8: OLED_DrawLookout (&U8G2_OLED, GPS); break; case 9: OLED_DrawTrafWarn (&U8G2_OLED, GPS); break; + case 10: OLED_DrawLoRaWAN (&U8G2_OLED, GPS); break; } } //if ( DISP_Page != 6 ) diff --git a/main/disp_oled.cpp b/main/disp_oled.cpp index dab4d1b..8ab25c4 100644 --- a/main/disp_oled.cpp +++ b/main/disp_oled.cpp @@ -453,7 +453,7 @@ static uint8_t BattCapacity(uint16_t mVolt) return (mVolt-3600+2)/5; } void OLED_DrawBattery(u8g2_t *OLED, GPS_Position *GPS) // draw battery status page -{ +{ #ifdef WITH_MAVLINK uint8_t Cap=MAVLINK_BattCap; // [%] from the drone's telemetry #else @@ -725,7 +725,7 @@ void OLED_DrawID(u8g2_t *OLED, GPS_Position *GPS) #endif } u8g2_SetFont(OLED, u8g2_font_5x8_tr); - u8g2_DrawStr(OLED, 96, 62, "v0.1.1"); + u8g2_DrawStr(OLED, 96, 62, "v0.1.2"); } void OLED_DrawAltitudeAndSpeed(u8g2_t *OLED, GPS_Position *GPS) @@ -815,6 +815,23 @@ void OLED_DrawAltitudeAndSpeed(u8g2_t *OLED, GPS_Position *GPS) u8g2_DrawXBM(OLED, 118, 47, kmh_width, kmh_height, kmh_bits); } +void OLED_DrawLoRaWAN(u8g2_t *OLED, GPS_Position *GPS) // draw LoRaWAN status page +{ + u8g2_SetFont(OLED, u8g2_font_7x13_tf); +#ifndef WITH_LORAWAN + u8g2_DrawStr(OLED, 0, 28, "LoRaWAN: -OFF-"); +#endif +#ifdef WITH_LORAWAN + const char *StateName[3] = { "Idle", "Join-Req", "Joined" } ; + int Len=Format_String(Line, "LoRaWAN: "); + if(WANdev.State<=2) Len+=Format_String(Line+Len, StateName[WANdev.State]); + else Len+=Format_Hex(Line+Len, WANdev.State); + Line[Len]=0; + u8g2_DrawStr(OLED, 0, 24, Line); + +#endif // WITH_LORAWAN +} + #endif // ======================================================================================================================== diff --git a/main/disp_oled.h b/main/disp_oled.h index 2165afe..fec64fe 100644 --- a/main/disp_oled.h +++ b/main/disp_oled.h @@ -23,5 +23,5 @@ void OLED_DrawStatusBar(u8g2_t *OLED, GPS_Position *GPS=0); void OLED_DrawSystem (u8g2_t *OLED, GPS_Position *GPS=0); void OLED_DrawID (u8g2_t *OLED, GPS_Position *GPS=0); void OLED_DrawAltitudeAndSpeed(u8g2_t *OLED, GPS_Position *GPS=0); - +void OLED_DrawLoRaWAN (u8g2_t *OLED, GPS_Position *GPS=0); #endif diff --git a/main/hal.cpp b/main/hal.cpp index 8ecc597..f8db915 100644 --- a/main/hal.cpp +++ b/main/hal.cpp @@ -16,6 +16,10 @@ #include "esp_system.h" #include "esp_freertos_hooks.h" +#ifdef WITH_LORAWAN +#include "lorawan.h" +#endif + #ifdef WITH_SLEEP #include "esp_sleep.h" #endif @@ -539,6 +543,10 @@ uint8_t MAV_Seq=0; // sequence number for MAVlink message sen FlashParameters Parameters; +#ifdef WITH_LORAWAN +LoRaWANnode WANdev; +#endif + //-------------------------------------------------------------------------------------------------------- // Power control diff --git a/main/hal.h b/main/hal.h index ccb6e00..9e8f2aa 100644 --- a/main/hal.h +++ b/main/hal.h @@ -63,6 +63,11 @@ uint32_t getUniqueAddress(void); // get unique 24-bit address for the #include "parameters.h" extern FlashParameters Parameters; +#ifdef WITH_LORAWAN +#include "lorawan.h" +extern LoRaWANnode WANdev; +#endif + void CONS_UART_Init (void); int CONS_UART_Read (uint8_t &Byte); // non-blocking void CONS_UART_Write (char Byte); // blocking diff --git a/main/lorawan.h b/main/lorawan.h new file mode 100644 index 0000000..09f0f4f --- /dev/null +++ b/main/lorawan.h @@ -0,0 +1,240 @@ +#ifndef __LORAWAN_H__ +#define __LORAWAN_H__ + +#include + +#ifdef WITH_ESP32 +#include "nvs.h" +#endif + +#include "LoRaMacCrypto.h" + +#include "rfm.h" + +class LoRaWANnode +{ public: + static const uint8_t Chans=8; + uint64_t AppEUI; // from application registration: Application identification + uint8_t AppKey[16]; // from device registration: application encryption/decryption key + uint64_t DevEUI; // Device identification (MAC) + uint32_t DevNonce; // unique counter kept by the device for Join-Requests + uint8_t NetSesKey[16]; // from Join-Accept: Network Session Key + uint8_t AppSesKey[16]; // from Join-Accept: App Session Key + uint32_t JoinNonce; // from Join-Accept: unique must not be reused + uint32_t HomeNetID; // from Join-Accept: Home Network ID + uint32_t DevAddr; // from Join-Accept: Device Address + uint8_t DLsetting; // from Join-Accept: DownLink configuration: OptNeg | RX1DRoffset | RX2 data rate + uint8_t RxDelay; // from Join-Accept: + uint8_t State; // 0:disconencted, 1:join-request sent, 2:join-accept received + uint8_t Chan; // [0..7] Current channel being used + uint32_t UpCount; // [seq] Uplink frame counter: reset when joining the network + uint32_t DnCount; // [seq] Downlink frame counter: reset when joining the network + uint32_t TxCount; // [packets] transmitted to the network + uint32_t LastTx; // [sec] last transmission + uint32_t RxCount; // [packets] received from the network + uint32_t LastRx; // [sec] when last heard from the network + uint8_t RxSNR; // SNR on receive + int8_t RxRSSI; // [dBm] signal strength + union + { uint8_t Flags; + struct + { bool RxACK :1; // received ACK + bool TxACK :1; // ACK to be transmitted + bool RxPend:1; // more frames pending for reception + } ; + } ; + uint8_t Spare; + uint8_t Packet[40]; // generic packet for storage/processing + + public: + LoRaWANnode() { Reset(); } + + void Reset(void) + { State=0; DevNonce=0; JoinNonce=0; + LastTx=0; TxCount=0; LastRx=0; RxCount=0; Flags=0; } + + void Reset(uint64_t MAC, uint8_t *AppKey=0) + { AppEUI=0x70B3D57ED0035895; + DevEUI=MAC; + if(AppKey) memcpy(this->AppKey, AppKey, 16); + Reset(); } + + uint8_t incrChan(uint8_t Step=1) { Chan+=Step; if(Chan>=Chans) Chan-=Chans; return Chan; } + + int Save(FILE *File) { return fwrite(this, sizeof(LoRaWANnode), 1, File); } + int Save(const char *FileName) + { FILE *File=fopen(FileName, "wb"); if(File==0) return 0; + int Written=Save(File); fclose(File); return Written; } + + int Restore(FILE *File) { return fread(this, sizeof(LoRaWANnode), 1, File); } + int Restore(const char *FileName) + { FILE *File=fopen(FileName, "rb"); if(File==0) return 0; + int Read=Restore(File); fclose(File); return Read; } + + static int ReadHex(uint8_t *Data, int Len, const char *Inp) + { int Bytes=0; + for( ; Bytes + static Type readInt(const uint8_t *Inp, int Len=sizeof(Type)) + { int Idx=Len-1; + Type Value = Inp[Idx]; + for( ; Idx>0; ) + { Idx--; Value<<=8; Value|=Inp[Idx]; } + return Value; } + + template + static int writeInt(uint8_t *Out, Type Value, int Len=sizeof(Type), bool Rev=0) + { if(Rev) + { for( int Idx=Len; Idx>0; ) + { Out[--Idx] = Value; Value>>=8; } + } + else + { for(int Idx=0; Idx>=8; } + } + return Len; } + + int getJoinRequest(uint8_t *Req) + { Req[0] = 0x00; // MHDR, Join-Request: 000 000 00 + memcpy(Req+1, &AppEUI, 8); // + memcpy(Req+9, &DevEUI, 8); + Req[17] = DevNonce; + Req[18] = DevNonce>>8; + uint32_t MIC=0; + LoRaMacJoinComputeMic(Req, 19, AppKey, &MIC); // compute MIC + memcpy(Req+19, &MIC, 4); // append MIC + State=1; // State: Join-Request sent + return 23; } // 23 bytes packet length + + int getJoinRequest(uint8_t **Req) { int Len=getJoinRequest(Packet); *Req = Packet; return Len; } + + int procJoinAccept(const RFM_LoRa_RxPacket &RxPacket) + { int Ret=procJoinAccept(RxPacket.Byte, RxPacket.Len); if(Ret<0) return Ret; + RxSNR = RxPacket.SNR; + RxRSSI = RxPacket.RSSI; + return Ret; } + + int procJoinAccept(const uint8_t *PktData, int PktLen) + { if(PktLen<13) return -1; + uint8_t Type = PktData[0]>>5; if(Type!=1) return -1; + Packet[0] = PktData[0]; + LoRaMacJoinDecrypt(PktData+1, PktLen-1, AppKey, Packet+1); // decrypt the Join-Accept packet + uint32_t MIC=0; + LoRaMacJoinComputeMic(Packet, PktLen-4, AppKey, &MIC); // Compute MIC + if(memcmp( Packet+PktLen-4, &MIC, 4)) return -1; // Compare with the packet + LoRaMacJoinComputeSKeys(AppKey, Packet+1, DevNonce, NetSesKey, AppSesKey); // derive Network Session and Application Session keys + JoinNonce = readInt(Packet+1, 3); // this should be not smaller than the previous one + HomeNetID = readInt(Packet+4, 3); + DevAddr = readInt(Packet+7, 4); + DLsetting = Packet[11]; + RxDelay = Packet[12]; + State = 2; // State = accepted on network + UpCount = 0; + DnCount = 0; +#ifdef WITH_PRINTF + printf("Accept[%d] ", PktLen-4); + for(int Idx=0; Idx %02X%02X%02X%02X\n", MIC, MIC2[3], MIC2[2], MIC2[1], MIC2[0]); + memcpy(Packet+PktLen, &MIC, 4); PktLen+=4; // append MIC + UpCount++; return PktLen; } + + int getDataPacket(uint8_t **Pkt, const uint8_t *Data, int DataLen, uint8_t Port=1, bool Confirm=0) + { int Len=getDataPacket(Packet, Data, DataLen, Port, Confirm); *Pkt = Packet; return Len; } + + int procRxData(const RFM_LoRa_RxPacket &RxPacket) + { int Ret=procRxData(RxPacket.Byte, RxPacket.Len); if(Ret<0) return Ret; + RxSNR += (RxPacket.SNR-RxSNR+1)/2; + RxRSSI += (RxPacket.RSSI-RxRSSI+1)/2; + return Ret; } + + int procRxData(const uint8_t *PktData, int PktLen) + { if(PktLen<12) return -1; + uint8_t Type = PktData[0]>>5; if(Type!=3 && Type!=5) return -1; + uint32_t Addr=readInt(PktData+1, 4); + if(Addr!=DevAddr) return 0; + uint8_t Ctrl = PktData[5]; // Frame control: ADR | RFU | ACK | FPending | FOptLen[4] + uint32_t Count=readInt(PktData+6, 2); + // Count |= DnCount&0xFFFF0000; + int16_t CountDiff = Count-DnCount; // + if(CountDiff<=0) return -1; // attempt to reuse the counter: drop this packet + // if(Diff<=(-0x4000)) Count+=0x10000; + // else if(Diff>0x4000) Count-=0x10000; + // printf("RxData: %08X\n", Count); + uint32_t MIC=0; + LoRaMacComputeMic(PktData, PktLen-4, NetSesKey, Addr, 0x01, Count, &MIC); + // printf("RxData: %08X\n", MIC); + if(memcmp(PktData+PktLen-4, &MIC, 4)) return -1; // give up if MIC does not match + // if(Count==DnCount) return 0; + uint8_t DataOfs = 8 + (Ctrl&0x0F); // where the port byte should be + uint8_t DataLen = PktLen-DataOfs-4; // number of bytes of the user data field + if(DataLen) // if non-zero + { Packet[0] = PktData[DataOfs]; // copy port number + LoRaMacPayloadDecrypt(PktData+DataOfs+1, DataLen-1, AppSesKey, Addr, 0x01, Count, Packet+1); } // decrypt and copy the user data +#ifdef WITH_PRINTF + printf("RxData: [%d] ", DataLen); + for(int Idx=0; Idx>32)); Len+=Format_Hex(Line+Len, (uint32_t)ID); } + // uint32_t DefaultAddr=getUniqueAddress(); + // if(Address!=DefaultAddr) + // { Line[Len++]='/'; Len+=Format_Hex(Line+Len, DefaultAddr, 6); } #ifdef WITH_RFM69 Len+=Format_String(Line+Len, " RFM69"); if(isTxTypeHW()) Line[Len++]='H'; @@ -511,6 +516,15 @@ class FlashParameters if(strcmp(Name, "Verbose")==0) { int32_t Mode=0; if(Read_Int(Mode, Value)<=0) return 0; Verbose=Mode; return 1; } +#ifdef WITH_LORAWAN + if(strcmp(Name, "AppKey")==0) + { for( uint8_t Idx=0; Idx<16; Idx++) + { uint8_t Byte; + uint8_t Len=Read_Hex(Byte, Value); + if(Len!=2) break; + AppKey[Idx]=Byte; } + return 1; } +#endif #ifdef WITH_ENCRYPT if(strcmp(Name, "Encrypt")==0) { int32_t Encr=0; if(Read_Int(Encr, Value)<=0) return 0; diff --git a/main/rf.cpp b/main/rf.cpp index 302e004..b1a8d0c 100644 --- a/main/rf.cpp +++ b/main/rf.cpp @@ -4,6 +4,10 @@ #include "timesync.h" #include "lowpass2.h" +#ifdef WITH_LORAWAN +#include "lorawan.h" +#endif + // =============================================================================================== // OGNv1 SYNC: 0x0AF3656C encoded in Manchester diff --git a/main/rfm.h b/main/rfm.h index 33ebf69..a0a1a43 100644 --- a/main/rfm.h +++ b/main/rfm.h @@ -18,7 +18,7 @@ class RFM_LoRa_Config { uint8_t Spare :3; uint8_t LowRate :1; // 0..1 uint8_t TxInv :1; // 0..1, invert on TX - uint8_t RxInv :1; // 0..1, invert on RX + uint8_t RxInv :1; // 0..1, invert on RX <- probably inverted logic uint8_t IHDR :1; // 0..1, implicit header (no header on TX) uint8_t CRC :1; // 0..1, produce CRC on TX and check CRC on RX uint8_t CR :4; // 1..4, Coding Rate @@ -241,7 +241,7 @@ class RFM_TRX uint8_t (*TransferByte)(uint8_t); // exchange one byte through SPI #endif - void (*Delay_ms)(void); + void (*Delay_ms)(int ms); bool (*DIO0_isOn)(void); // read DIO0 = packet is ready // bool (*DIO4_isOn)(void); void (*RESET)(uint8_t On); // activate or desactivate the RF chip reset @@ -613,17 +613,20 @@ class RFM_TRX RFM_LoRa_Config CFG = RFM_FNTcfg; CFG.CR=CR; return LoRa_Configure(CFG, FANET_Packet::MaxBytes); } - void LoRa_setCRC(bool ON=1) // LoRaWAN: uplink with CRC, downlink without CRC + void LoRa_setIRQ(uint8_t Mode=0) // 0:on RX, 1:on TX, 2: on CAD + { WriteByte(Mode<<6, REG_DIOMAPPING1); } + + void LoRa_setCRC(bool ON=1) // LoRaWAN: uplink with CRC, downlink without CRC { uint8_t Reg=ReadByte(REG_LORA_MODEM_CONFIG2); if(ON) Reg|=0x04; else Reg&=0xFB; WriteByte(Reg, REG_LORA_MODEM_CONFIG2); } - void LoRa_InvertIQ(bool ON=0) // LoRaWAN + void LoRa_InvertIQ(bool ON=0) // LoRaWAN: uplink without inversion, downlink with inversion, but beacon without { WriteByte(ON?0x66:0x27, REG_LORA_INVERT_IQ); WriteByte(ON?0x19:0x1D, REG_LORA_INVERT_IQ2); } - int LoRa_SendPacket(const uint8_t *Data, uint8_t Len) + int LoRa_SendPacket(const uint8_t *Data, uint8_t Len, int Wait=0) { // WriteMode(RF_OPMODE_LORA_STANDBY); // check if FIFO empty, packets could be received ? WriteByte(0x00, REG_LORA_FIFO_ADDR); // tell write to FIFO at address 0x00 @@ -631,10 +634,16 @@ class RFM_TRX WriteByte(0x00, REG_LORA_TX_ADDR); // tell packet address in the FIFO WriteByte(Len, REG_LORA_PACKET_LEN); // tell packet length WriteMode(RF_OPMODE_LORA_TX); // enter transmission mode - return 0; } // afterwards just wait for TX mode to stop + if(Wait==0) return 0; + Delay_ms(10); int Check=10; + for(Check=0; Check int LoRa_ReceivePacket(RxPacket &Packet) @@ -658,7 +667,7 @@ class RFM_TRX // Packet.Len, Stat, 0.25*Packet.SNR, Packet.RSSI, FreqOfs, 0.5*0x1000000/32e9*FreqOfs, // Packet.Byte[0], Packet.Byte[1], Packet.Byte[2], Packet.Byte[3]); Packet.Len=Len; - WriteByte(LORA_FLAG_RX_DONE | LORA_FLAG_BAD_CRC, REG_LORA_IRQ_FLAGS); + WriteByte(LORA_FLAG_RX_DONE | LORA_FLAG_BAD_CRC | LORA_FLAG_RX_HEADER, REG_LORA_IRQ_FLAGS); return Len; } int LoRa_ReceivePacket(uint8_t *Data, uint8_t MaxLen)