Crypto.c

#include "Crypto.h"
#include "clock.h"
#include "global_config.h"
#include "log.h"
#include "string.h"

#include "PhysicalLayer.h"
#include "Zigbee.h"
#include "OSAL.h"


/*
 Experimental implementation of zigbee encryption, probably does not work for all features etc.
*/



#define aes128Raw LL_ENC_AES128_Encrypt0

void aes128( const uint8* key,
                             uint8* plaintext,
                             uint8* ciphertext )
{
    AP_PCR->SW_CLK    |= BIT(MOD_AES);
    aes128Raw((uint8_t*)key,plaintext,ciphertext);
    AP_PCR->SW_CLK    &= ~BIT(MOD_AES);
}

__attribute__((section(".xip")))
void ZigbeeKeyHash(const uint8_t *key,uint8_t arg, uint8_t *output)
{
    uint8_t hash_in[32];
    const uint8_t ipad = 0x36;
    const uint8_t opad = 0x5c;

    for (int i=0; i<16; i++)
        hash_in[i] = key[i] ^ opad;
    for (int i=0; i<16; i++)
        output[i] = key[i] ^ ipad;
    output[16] = arg;
    ZigBeeHash(output, 17, hash_in+16);
    /* Hash the contents of hash_in to get the final result. */
    ZigBeeHash(hash_in, 32, output);
}

 void ZigBeeHash(const uint8_t *input, uint8_t input_len, uint8_t *output)
{
    AP_PCR->SW_CLK    |= BIT(MOD_AES);
    uint8_t              cipher_in[16];
    uint8_t               i, j;

    memset(output, 0, 16);
    i = 0;
    j = 0;
    while (i<input_len) {
            cipher_in[j++] = input[i++];
            if (j >= 16) {
                aes128Raw(output,cipher_in,output);
                for (j=0;j<16;j++) output[j] ^= cipher_in[j];
                j = 0;
            }
        }
        cipher_in[j++] = 0x80;
        while (j!=(16-2)) {
            if (j >= 16) {
                aes128Raw(output,cipher_in,output);
                for (j=0;j<16;j++) output[j] ^= cipher_in[j];
                j = 0;
            }
            cipher_in[j++] = 0x00;
        }
        cipher_in[j++] = ((input_len * 8) >> 8) & 0xff;
        cipher_in[j] = ((input_len * 8) >> 0) & 0xff;
        aes128Raw(output,cipher_in,output);
        for (j=0;j<16;j++)
            output[j] ^= cipher_in[j];
    AP_PCR->SW_CLK    &= ~BIT(MOD_AES);
}




#define ZIGBEE_CCM_L 2
#define ZIGBEE_NONCE_LENGTH 13
#define ZIGBEE_CCM_M 4

 // Zigbee decryption is basically just AES128 in counter mode...
void ZigbeeSymmetricEncDec(const uint8_t* key,
                           uint8_t* message,
                           uint8_t lenMessage,
                           uint8_t* nonce,
                           uint8_t* authenticationTagIn,
                           uint8_t* authenticationTagOut,
                           uint8_t* out)
{
    AP_PCR->SW_CLK    |= BIT(MOD_AES);
    uint8_t S[16];
    uint16_t counter = 1;

    S[0] = ZIGBEE_CCM_L-1;
    osal_memcpy(&S[1], nonce, ZIGBEE_NONCE_LENGTH);

    while(lenMessage != 0)
    {
        S[14] =  (counter >> 8) & 0xFF;
        S[15] = counter & 0xFF;
        counter++;
        aes128Raw(key, S, out);
        if(lenMessage >= 16)
        {
            for(int i = 0; i < 16; i++)
            {
                out[i] = message[i] ^ out[i];
            }
            lenMessage -= 16;
            out += 16;
            message += 16;
        }else
        {
            for(int i = 0; i < lenMessage; i++)
            {
                out[i] = message[i] ^ out[i];
            }
            lenMessage = 0;
        }
    }

    S[14] = 0;
    S[15] = 0;
    aes128(key,S,S);
    for(int i = 0; i < 4; i++)
        authenticationTagOut[i] = authenticationTagIn[i] ^ S[i];

    AP_PCR->SW_CLK    &= ~BIT(MOD_AES);
}

//Length of message length field
//Length of frame integrity
// See https://lucidar.me/en/zigbee/zigbee-frame-encryption-with-aes-128-ccm/
static void ZigbeeAuthTransformation(const uint8_t* key,
                                             uint8_t* nonce,
                                             uint8_t* A,//Network header || Auxiliary Header
                                             uint16_t lengthA,
                                             uint8_t* M,//String payload
                                             uint16_t lengthM,
                                             uint8_t* outTag)
{
    AP_PCR->SW_CLK    |= BIT(MOD_AES);
    uint8_t tmp[16]; //<Initially this holds B0, then used as temporary buffer
    uint8_t X[16];
    tmp[0] = 0x49;
    osal_memcpy(&tmp[1],nonce, ZIGBEE_NONCE_LENGTH);
    tmp[1 + ZIGBEE_NONCE_LENGTH] = (lengthM >> 8)&0xFF;
    tmp[2 + ZIGBEE_NONCE_LENGTH] = lengthM & 0xFF;
    aes128Raw(key, tmp, X); //Compute B0

    int i;
    if(lengthA > 0)
    {
        tmp[0] = ((lengthA >> 8) & 0xFF) ^ X[0];
        tmp[1] = (lengthA & 0xFF) ^ X[1];

        uint8_t* Aend = A + lengthA;
        i = 2;
        while(true)
        {
        // Start copying A to the buffer `tmp`.
        for(; (i < 16) &&  (A != Aend); i++)
        {
            tmp[i] = X[i] ^ *A++;
        }
        for(; i < 16; i++)
            tmp[i] = 0 ^ X[i];

        //for(i = 0; i < 16; i++)
         //   tmp[i] = X[i] ^ tmp[i];

        aes128Raw(key,tmp, X);
        if(A == Aend)
            break;
        i = 0;

        }
    }

    uint8_t* Mend = M + lengthM;
    while(true)
    {
        i = 0;
        for(i = 0; (i < 16) && (M != Mend); i++)
            tmp[i] = *M++ ^ X[i];

        for(; i < 16; i++)
            tmp[i] = X[i];
        aes128Raw(key,tmp, X);

        if(M == Mend)
            break;

    }
    for(int i = 0; i < 4; i++)
    {
        outTag[i] = X[i];
    }
    AP_PCR->SW_CLK    &= ~BIT(MOD_AES);
}

uint8_t ZigbeeCCMDec(const uint8_t* key, uint8_t* zigbeeSecurityHeader, uint8_t* A0, uint16_t lenA0, uint8_t* message,
                  uint16_t lengthMessage, uint8_t* outMessage)
{
   uint8_t nonce[13];

    uint8_t* frameCounter = &zigbeeSecurityHeader[1];
    uint8_t* extendedSource = &frameCounter[4];
    osal_memcpy(nonce, extendedSource,8);
    osal_memcpy(&nonce[8], frameCounter, 4);

    uint8_t securityHeaderPrev = *zigbeeSecurityHeader;
   *zigbeeSecurityHeader &= ~0x07;
   *zigbeeSecurityHeader |= (0x07 & 0x05);

    nonce[12] = *zigbeeSecurityHeader;

    uint8_t decryptedTag[4];
    uint8_t tagComputed[4];
    ZigbeeSymmetricEncDec(key,message, lengthMessage, nonce, message + lengthMessage,decryptedTag, outMessage);

    ZigbeeAuthTransformation(key,nonce,A0,lenA0,outMessage,lengthMessage,tagComputed);
    *zigbeeSecurityHeader = securityHeaderPrev;

    uint8_t result =  !memcmp(decryptedTag, tagComputed, 4);
    if(!result)
    {
        //LOG(": got %02x %02x, %02x %02x computed %02x %02x %02x %02x", decryptedTag[0], decryptedTag[1], decryptedTag[2], decryptedTag[3], tagComputed[0],tagComputed[1],tagComputed[2],tagComputed[3] );
        LOG("BadDec");
    }

    return result;
}

void ZigbeeCCMEnc(const uint8_t* key, uint8_t* zigbeeSecurityHeader, uint8_t* A0, uint8_t* message,
                  uint16_t lengthMessage, uint8_t* outMessage)
{
    uint16_t lenA0 = outMessage - A0;
    uint8_t nonce[13];

    uint8_t* frameCounter = &zigbeeSecurityHeader[1];
    uint8_t* extendedSource = &frameCounter[4];
    osal_memcpy(nonce, extendedSource,8);
    osal_memcpy(&nonce[8], frameCounter, 4);

    uint8_t securityHeaderPrev = *zigbeeSecurityHeader;
   *zigbeeSecurityHeader &= ~0x07;
   *zigbeeSecurityHeader |= (0x07 & 0x05);

    nonce[12] = *zigbeeSecurityHeader;

    uint8_t tagComputed[4];
    uint8_t tagOut[4];
    ZigbeeAuthTransformation(key,nonce,A0,lenA0,message,lengthMessage,tagComputed);
    ZigbeeSymmetricEncDec(key,message, lengthMessage, nonce, tagComputed, outMessage + lengthMessage, outMessage);

    *zigbeeSecurityHeader = securityHeaderPrev;
}

Crypto.h

#pragma once

#include "types.h"

/*
 Experimental implementation of zigbee encryption, probably does not work for all features etc.
*/


void aes128( const uint8* key, uint8* plaintext, uint8* ciphertext );

void DecryptZigbeeAES128(uint8_t* message,uint8_t lMessage, uint8_t* nonce, uint8_t* out);
 void ZigBeeHash(const uint8_t *input, uint8_t input_len, uint8_t *output);

__attribute__((section(".xip")))
void ZigbeeKeyHash(const uint8_t *key,uint8_t arg, uint8_t *output);

uint8_t ZigbeeCCMDec(const uint8_t* key, uint8_t* zigbeeSecurityHeader, uint8_t* A0, uint16_t lenA0, uint8_t* message, uint16_t lengthMessage, uint8_t* outMessage);
void ZigbeeCCMEnc(const uint8_t* key, uint8_t* zigbeeSecurityHeader, uint8_t* A0,  uint8_t* message, uint16_t lengthMessage, uint8_t* outMessage);