LoRaMacCrypto1v1.cpp

00001 #include "lorawan_board.h"
00002 #include "LoRaMacCrypto.h"
00003 #include "cmac.h"
00004 #include "aes.h"
00005 
00006 #define LORAMAC_MIC_BLOCK_B0_SIZE                   16
00007 mbedtls_cipher_context_t ctx;
00008 
00009 uint32_t LoRaMacComputeMic(
00010     const block_t* block,
00011     const uint8_t* pktPayload,
00012     const uint8_t* key)
00013 {
00014     uint8_t Mic[16];
00015 
00016     if (block->b.dir == DOWN_LINK) {
00017         DEBUG_MIC_DOWN("down ");
00018         DEBUG_MIC_BUF_DOWN(block->octets, LORAMAC_MIC_BLOCK_B0_SIZE, "b0", ROW_MIC+1);
00019         DEBUG_MIC_BUF_DOWN(key, 16, "b0-key", ROW_MIC+2);
00020     } else if (block->b.dir == UP_LINK) {
00021         DEBUG_MIC_UP("  up ");
00022         DEBUG_MIC_BUF_UP(block->octets, LORAMAC_MIC_BLOCK_B0_SIZE, "b0", ROW_MIC+1);
00023         DEBUG_MIC_BUF_UP(key, 16, "b0-key", ROW_MIC+2);
00024     }
00025 
00026     mbedtls_cipher_cmac_starts(&ctx, key, 128);
00027 
00028     mbedtls_cipher_cmac_update(&ctx, block->octets, LORAMAC_MIC_BLOCK_B0_SIZE);
00029 
00030     mbedtls_cipher_cmac_update(&ctx, pktPayload, block->b.lenMsg);
00031 
00032     mbedtls_cipher_cmac_finish(&ctx, Mic);
00033 
00034     return ( uint32_t )( ( uint32_t )Mic[3] << 24 | ( uint32_t )Mic[2] << 16 | ( uint32_t )Mic[1] << 8 | ( uint32_t )Mic[0] );
00035 }
00036 
00037 void LoRaMacEncrypt( uint8_t ctr, const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t address, uint8_t dir, uint32_t sequenceCounter, uint8_t *encBuffer )
00038 {
00039     uint8_t aBlock[16];
00040     uint8_t sBlock[16];
00041     uint16_t i;
00042     uint8_t bufferIndex = 0;
00043     mbedtls_aes_context actx;
00044 
00045     //memset( AesContext.ksch, '\0', 240 );
00046     mbedtls_aes_init(&actx);
00047     //aes_set_key( key, 16, &AesContext );
00048     mbedtls_aes_setkey_enc(&actx, key, 128);
00049 
00050     aBlock[0] = 0x01;
00051 
00052     aBlock[1] = 0x00;
00053     aBlock[2] = 0x00;
00054     aBlock[3] = 0x00;
00055     aBlock[4] = 0x00;
00056 
00057     aBlock[5] = dir;
00058 
00059     aBlock[6] = ( address ) & 0xFF;
00060     aBlock[7] = ( address >> 8 ) & 0xFF;
00061     aBlock[8] = ( address >> 16 ) & 0xFF;
00062     aBlock[9] = ( address >> 24 ) & 0xFF;
00063 
00064     aBlock[10] = ( sequenceCounter ) & 0xFF;
00065     aBlock[11] = ( sequenceCounter >> 8 ) & 0xFF;
00066     aBlock[12] = ( sequenceCounter >> 16 ) & 0xFF;
00067     aBlock[13] = ( sequenceCounter >> 24 ) & 0xFF;
00068 
00069     aBlock[14] = 0;
00070 
00071     while( size >= 16 )
00072     {
00073         aBlock[15] = ( ( ctr ) & 0xFF );
00074         ctr++;
00075         //aes_encrypt( aBlock, sBlock, &AesContext );
00076         mbedtls_aes_encrypt(&actx, aBlock, sBlock);
00077         for( i = 0; i < 16; i++ )
00078         {
00079             encBuffer[bufferIndex + i] = buffer[bufferIndex + i] ^ sBlock[i];
00080         }
00081         size -= 16;
00082         bufferIndex += 16;
00083     }
00084 
00085     if( size > 0 )
00086     {
00087         aBlock[15] = ( ( ctr ) & 0xFF );
00088         //aes_encrypt( aBlock, sBlock, &AesContext );
00089         mbedtls_aes_encrypt(&actx, aBlock, sBlock);
00090         for( i = 0; i < size; i++ )
00091         {
00092             encBuffer[bufferIndex + i] = buffer[bufferIndex + i] ^ sBlock[i];
00093         }
00094     }
00095 
00096     mbedtls_aes_free(&actx);
00097 }
00098 
00099 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 )
00100 {
00101     LoRaMacEncrypt(1, buffer, size, key, address, dir, sequenceCounter, decBuffer );
00102 }
00103 
00104 void
00105 LoRaMacCryptoInit()
00106 {
00107     //int ret;
00108     const mbedtls_cipher_info_t *cipher_info = mbedtls_cipher_info_from_type( MBEDTLS_CIPHER_AES_128_ECB );
00109     if (cipher_info == NULL) {
00110         MAC_PRINTF("mbedtls_cipher_info_from_type() failed\n");
00111         return;
00112     }
00113     mbedtls_cipher_init(&ctx);
00114     /*ret = */mbedtls_cipher_setup(&ctx, cipher_info);
00115     //MAC_PRINTF("mbedtls_cipher_setup returned %d, type %d\r\n", ret, ctx.cipher_info->type);
00116     /* always using same ctx --- mbedtls_cipher_free(&ctx) */
00117 }
00118 
00119 #ifdef LORAWAN_JOIN_EUI
00120 void LoRaMacJoinComputeSKeys_1v1( const uint8_t *nwk_root_key, const uint8_t *app_root_key, const uint8_t *joinNonce, const uint8_t *joinEUI, uint16_t devNonce, skey_t* keys)
00121 {
00122     uint8_t buff[16];
00123     uint8_t *pDevNonce = ( uint8_t * )&devNonce;
00124     uint8_t* bufPtr;
00125     mbedtls_aes_context actx;
00126 
00127     mbedtls_aes_init(&actx);
00128     //memset( AesContext.ksch, '\0', 240 );
00129     DEBUG_CRYPT_BUF(app_root_key, 16, "AppSKey-root", 0);
00130     //aes_set_key( app_root_key, 16, &AesContext );
00131     mbedtls_aes_setkey_enc(&actx, app_root_key, 128);
00132 
00133     memset(buff, 0, sizeof(buff));
00134     bufPtr = buff + 1;
00135     memcpy(bufPtr, joinNonce, 3);
00136     bufPtr += 3;
00137     memcpyr(bufPtr, joinEUI, 8);
00138     bufPtr += 8;
00139     memcpy(bufPtr, pDevNonce, 2);
00140     bufPtr += 2;
00141 
00142     /* generate AppSKey */
00143     buff[0] = 0x02;
00144     DEBUG_CRYPT_BUF(buff, 16, "AppSKey-in", 0);
00145     //aes_encrypt(buff, keys->AppSKey, &AesContext );
00146     mbedtls_aes_encrypt(&actx, buff, keys->AppSKey);
00147     DEBUG_CRYPT_BUF(keys->AppSKey, 16, "AppSKey", 0);
00148 
00149     //memset( AesContext.ksch, '\0', 240 );
00150     //aes_set_key( nwk_root_key, 16, &AesContext );
00151     mbedtls_aes_setkey_enc(&actx, nwk_root_key, 128);
00152 
00153     /* generate FNwkSIntKey */
00154     buff[0] = 0x01;
00155     DEBUG_CRYPT_BUF(buff, sizeof(buff), "in-FNwkSIntKey", 0);
00156     //aes_encrypt(buff, keys->FNwkSIntKey, &AesContext );
00157     mbedtls_aes_encrypt(&actx, buff, keys->FNwkSIntKey);
00158     DEBUG_CRYPT_BUF(keys->FNwkSIntKey, 16, "FNwkSIntKey", 0);
00159 
00160     /* generate SNwkSIntKey */
00161     buff[0] = 0x03;
00162     //aes_encrypt(buff, keys->SNwkSIntKey, &AesContext );
00163     mbedtls_aes_encrypt(&actx, buff, keys->SNwkSIntKey);
00164     DEBUG_CRYPT_BUF(keys->SNwkSIntKey, 16, "SNwkSIntKey", 0);
00165 
00166     /* generate NwkSEncKey */
00167     buff[0] = 0x04;
00168     //aes_encrypt(buff, keys->NwkSEncKey, &AesContext );
00169     mbedtls_aes_encrypt(&actx, buff, keys->NwkSEncKey);
00170     DEBUG_CRYPT_BUF(keys->NwkSEncKey, 16, "NwkSEncKey", 0);
00171 
00172     mbedtls_aes_free(&actx);
00173 }
00174 
00175 void LoRaMacJoinComputeSKeys_1v0(const uint8_t *nwk_root_key, const uint8_t *ja_rx, uint16_t devNonce, skey_t* keys)
00176 {
00177     /* ja_rx: joinNonce + NetID, 6bytes */
00178     uint8_t nonce[16];
00179     uint8_t *pDevNonce = ( uint8_t * )&devNonce;
00180     mbedtls_aes_context actx;
00181 
00182     mbedtls_aes_init(&actx);
00183     //memset( AesContext.ksch, '\0', 240 );
00184     //aes_set_key( nwk_root_key, 16, &AesContext );
00185     mbedtls_aes_setkey_enc(&actx, nwk_root_key, 128);
00186 
00187     memset( nonce, 0, sizeof( nonce ) );
00188     nonce[0] = 0x01;
00189     memcpy( nonce + 1, ja_rx, 6 );
00190     memcpy( nonce + 7, pDevNonce, 2 );
00191     //aes_encrypt( nonce, keys->FNwkSIntKey, &AesContext );
00192     mbedtls_aes_encrypt(&actx, nonce, keys->FNwkSIntKey);
00193 
00194     memcpy(keys->SNwkSIntKey, keys->FNwkSIntKey, 16);
00195     memcpy(keys->NwkSEncKey, keys->FNwkSIntKey, 16);
00196 
00197     memset( nonce, 0, sizeof( nonce ) );
00198     nonce[0] = 0x02;
00199     memcpy( nonce + 1, ja_rx, 6 );
00200     memcpy( nonce + 7, pDevNonce, 2 );
00201     //aes_encrypt( nonce, keys->AppSKey, &AesContext );
00202     mbedtls_aes_encrypt(&actx, nonce, keys->AppSKey);
00203 
00204     mbedtls_aes_free(&actx);
00205 }
00206 
00207 int LoRaMacJoinComputeMic(bool verbose, const uint8_t *buffer, uint16_t size, const uint8_t *key, uint32_t *mic )
00208 {
00209     int ret;
00210     uint8_t Mic[LORAMAC_MIC_BLOCK_B0_SIZE];
00211 
00212 #ifndef ENABLE_VT100
00213     if (verbose) {
00214         print_buf(key, 16, "mic-key");
00215         print_buf(buffer, size, "mic-in-buf");
00216     }
00217 #endif /* ENABLE_VT100 */
00218 
00219     ret = mbedtls_cipher_cmac_starts(&ctx, key, 128);
00220     if (ret != 0) {
00221         MAC_PRINTF("0x%x = mbedtls_cipher_cmac_starts()\r\n", ret);
00222         return ret;
00223     }
00224 
00225     ret = mbedtls_cipher_cmac_update(&ctx, buffer, size & 0xff);
00226     if (ret != 0) {
00227         MAC_PRINTF("%d = mbedtls_cipher_cmac_update()\r\n", ret);
00228         return ret;
00229     }
00230 
00231     ret = mbedtls_cipher_cmac_finish(&ctx, Mic);
00232     if (ret != 0) {
00233         MAC_PRINTF("%d = mbedtls_cipher_cmac_finish()\r\n", ret);
00234         return ret;
00235     }
00236 
00237     *mic = ( uint32_t )( ( uint32_t )Mic[3] << 24 | ( uint32_t )Mic[2] << 16 | ( uint32_t )Mic[1] << 8 | ( uint32_t )Mic[0] );
00238     return 0;
00239 }
00240 
00241 void LoRaMacJoinDecrypt( const uint8_t *buffer, uint16_t size, const uint8_t *key, uint8_t *decBuffer )
00242 {
00243     mbedtls_aes_context actx;
00244     mbedtls_aes_init(&actx);
00245     if (mbedtls_aes_setkey_enc(&actx, key, 128) < 0) {
00246         MAC_PRINTF("%d = mbedtls_aes_setkey_enc()\r\n");
00247     }
00248     mbedtls_aes_encrypt(&actx, buffer, decBuffer);
00249     // Check if optional CFList is included
00250     if (size >= 16)
00251     {
00252         mbedtls_aes_encrypt(&actx, buffer + 16, decBuffer + 16);
00253     }
00254 
00255     mbedtls_aes_free(&actx);
00256 }
00257 
00258 void LoRaMacGenerateJoinKey(uint8_t token, const uint8_t* root_key, const uint8_t* devEui, uint8_t* output)
00259 {
00260     int i;
00261     uint8_t input[16];
00262     uint8_t* ptr = input;
00263     mbedtls_aes_context actx;
00264 
00265     mbedtls_aes_init(&actx);
00266 
00267     memset(ptr, 0, sizeof(input));
00268 
00269     *ptr++ = token;
00270 
00271     /* EUI put into buffer in same order that it appears over-the-air */
00272     for (i = LORA_EUI_LENGTH - 1; i >= 0; i--)
00273         *ptr++ = devEui[i];
00274 
00275     DEBUG_CRYPT_BUF(root_key, 16, "generate-join-key-root_key", 0);
00276     DEBUG_CRYPT_BUF(input, 16, "generate-join-key-input", 0);
00277 
00278     mbedtls_aes_setkey_enc(&actx, root_key, 128);
00279     mbedtls_aes_encrypt(&actx, input, output);
00280 
00281     mbedtls_aes_free(&actx);
00282 }
00283 #endif /* LORAWAN_JOIN_EUI */