csu3_am335x/EVSE/Projects/AW-CCS/Apps/CCS/NidNmk.c

/*===========================================================================
                    Combined Charging System (CCS): SECC
                                NidNmk.c

                        initiated by Vern, Joseph
                           (since 2019/07/19)
=============================================================================*/
#include <time.h>
#include <sys/time.h>
#include "NidNmk.h"

/*===========================================================================
FUNCTION: StoreLogMsg
DESCRIPTION:
PRE-CONDITION:
INPUT:
OUTPUT:
GLOBAL VARIABLES:
=============================================================================*/
void HPAVKeyNMK(uint8_t NMK [], const char *string)

{
    struct sha256 sha256;
    uint8_t digest [SHA256_DIGEST_LENGTH];
    const uint8_t secret [] =
    {
        0x08,
        0x85,
        0x6D,
        0xAF,
        0x7C,
        0xF5,
        0x81,
        0x86
    };
    unsigned rehash = 999;

    SHA256Reset(&sha256);
    SHA256Write(&sha256, string, strlen(string));
    SHA256Write(&sha256, secret, sizeof(secret));
    SHA256Fetch(&sha256, digest);

    while(rehash--)
    {
        SHA256Reset(&sha256);
        SHA256Write(&sha256, digest, sizeof(digest));
        SHA256Fetch(&sha256, digest);
    }

    memcpy(NMK, digest, HPAVKEY_NMK_LEN);
    return;
}

void HPAVKeyNID(uint8_t NID[], const uint8_t NMK[], uint8_t level)
{
    struct sha256 sha256;
    uint8_t digest [SHA256_DIGEST_LENGTH];
    unsigned int rehash = 4;

    SHA256Reset(&sha256);
    SHA256Write(&sha256, NMK, HPAVKEY_NMK_LEN);
    SHA256Fetch(&sha256, digest);

    while(rehash--)
    {
        SHA256Reset(&sha256);
        SHA256Write(&sha256, digest, sizeof(digest));
        SHA256Fetch(&sha256, digest);
    }

    #if 1

        level <<= 4;
        digest [HPAVKEY_NID_LEN - 1] >>= 4;
        digest [HPAVKEY_NID_LEN - 1] |= level;

    #else

        digest [HPAVKEY_NID_LEN - 1] &= ~0xC0;
        digest [HPAVKEY_NID_LEN - 1] |= level << 6;

    #endif

    memcpy(NID, digest, HPAVKEY_NID_LEN);

    return;
}

void SHA256Reset(struct sha256 *sha256)

{
    memset(sha256, 0, sizeof(struct sha256));
    sha256->state [0] = 0x6A09E667;
    sha256->state [1] = 0xBB67AE85;
    sha256->state [2] = 0x3C6EF372;
    sha256->state [3] = 0xA54FF53A;
    sha256->state [4] = 0x510E527F;
    sha256->state [5] = 0x9B05688C;
    sha256->state [6] = 0x1F83D9AB;
    sha256->state [7] = 0x5BE0CD19;
    sha256->extra [0] = 0x80;
    return;
}

void SHA256Block(struct sha256 *sha256, void const *memory)
{
    static const uint32_t K [sizeof(sha256->block)] =
    {
        0x428A2F98,
        0x71374491,
        0xB5C0FBCF,
        0xE9B5DBA5,
        0x3956C25B,
        0x59F111F1,
        0x923F82A4,
        0xAB1C5ED5,
        0xD807AA98,
        0x12835B01,
        0x243185BE,
        0x550C7DC3,
        0x72BE5D74,
        0x80DEB1FE,
        0x9BDC06A7,
        0xC19BF174,
        0xE49B69C1,
        0xEFBE4786,
        0x0FC19DC6,
        0x240CA1CC,
        0x2DE92C6F,
        0x4A7484AA,
        0x5CB0A9DC,
        0x76F988DA,
        0x983E5152,
        0xA831C66D,
        0xB00327C8,
        0xBF597FC7,
        0xC6E00BF3,
        0xD5A79147,
        0x06CA6351,
        0x14292967,
        0x27B70A85,
        0x2E1B2138,
        0x4D2C6DFC,
        0x53380D13,
        0x650A7354,
        0x766A0ABB,
        0x81C2C92E,
        0x92722C85,
        0xA2BFE8A1,
        0xA81A664B,
        0xC24B8B70,
        0xC76C51A3,
        0xD192E819,
        0xD6990624,
        0xF40E3585,
        0x106AA070,
        0x19A4C116,
        0x1E376C08,
        0x2748774C,
        0x34B0BCB5,
        0x391C0CB3,
        0x4ED8AA4A,
        0x5B9CCA4F,
        0x682E6FF3,
        0x748F82EE,
        0x78A5636F,
        0x84C87814,
        0x8CC70208,
        0x90BEFFFA,
        0xA4506CEB,
        0xBEF9A3F7,
        0xC67178F2
    };
    unsigned int pass;
    unsigned int word;
    uint32_t H [sizeof(sha256->state) / sizeof(uint32_t)];
    uint32_t W [sizeof(sha256->block)];
    uint8_t *buffer = (uint8_t *)(memory);

    for(word = 0; word < 16; word++)
    {
        W [word] = 0;
        W [word] |= (uint32_t)(*buffer++) << 24;
        W [word] |= (uint32_t)(*buffer++) << 16;
        W [word] |= (uint32_t)(*buffer++) << 8;
        W [word] |= (uint32_t)(*buffer++) << 0;;
    }

    for(; word < sizeof(sha256->block); word++)
    {
        uint32_t s0 = ROTR(W [word - 15], 7) ^ ROTR(W [word - 15], 18) ^ SHR(W [word - 15], 3);
        uint32_t s1 = ROTR(W [word - 2], 17) ^ ROTR(W [word - 2], 19) ^ SHR(W [word - 2], 10);
        W [word] = W [word - 16] + s0 + W [word - 7] + s1;
    }

    for(word = 0; word < (sizeof(sha256->state) / sizeof(uint32_t)); word++)
    {
        H [word] = sha256->state [word];
    }

    for(pass = 0; pass < sizeof(sha256->block); pass++)
    {
        uint32_t s2 = ROTR(H [0], 2) ^ ROTR(H [0], 13) ^ ROTR(H [0], 22);
        uint32_t maj = (H [0] & H [1]) ^ (H [0] & H [2]) ^ (H [1] & H [2]);
        uint32_t t2 = s2 + maj;
        uint32_t s3 = ROTR(H [4], 6) ^ ROTR(H [4], 11) ^ ROTR(H [4], 25);
        uint32_t ch = (H [4] & H [5]) ^ ((~H [4]) & H [6]);
        uint32_t t1 = H [7] + s3 + ch + K [pass] + W [pass];

        for(word = (sizeof(sha256->state) / sizeof(uint32_t)) - 1; word > 0; word--)
        {
            H [word] = H [word - 1];
        }

        H [0] = t1 + t2;
        H [4] += t1;
    }

    for(word = 0; word < (sizeof(sha256->state) / sizeof(uint32_t)); word++)
    {
        sha256->state [word] += H [word];
    }

    return;
}

void SHA256Write(struct sha256 *sha256, void const *memory, uint16_t extent)

{
    if(extent)
    {
        uint8_t *buffer = (uint8_t *)(memory);
        unsigned int left = sha256->count [0] & 0x3F;
        unsigned int fill = sizeof(sha256->block) - left;
        sha256->count [0] += (uint32_t)(extent);
        sha256->count [0] &= 0xFFFFFFFF;

        if(sha256->count [0] < extent)
        {
            sha256->count [1]++;
        }

        if((left) && (extent >= fill))
        {
            memcpy(sha256->block + left, buffer, fill);
            SHA256Block(sha256, sha256->block);
            extent -= fill;
            buffer += fill;
            left = 0;
        }

        while(extent >= sizeof(sha256->block))
        {
            SHA256Block(sha256, buffer);
            extent -= sizeof(sha256->block);
            buffer += sizeof(sha256->block);
        }

        if(extent)
        {
            memcpy(sha256->block + left, buffer, extent);
        }
    }

    return;
}

void SHAEncode(uint8_t memory[], uint32_t number)
{
    *memory++ = (uint8_t)(number >> 24);
    *memory++ = (uint8_t)(number >> 16);
    *memory++ = (uint8_t)(number >> 8);
    *memory++ = (uint8_t)(number >> 0);
    return;
}

void SHA256Fetch(struct sha256 *sha256, uint8_t digest[])

{
    unsigned int word;
    uint8_t bits [8];
    uint32_t upper = (sha256->count [0] >> 29) | (sha256->count [1] << 3);
    uint32_t lower = (sha256->count [0] << 3);
    uint32_t final = (sha256->count [0] & 0x3F);
    uint32_t extra = (final < 56) ? (56 - final) : (120 - final);

    SHAEncode(&bits[0], upper);
    SHAEncode(&bits[4], lower);
    SHA256Write(sha256, sha256->extra, extra);
    SHA256Write(sha256, bits, sizeof(bits));

    for(word = 0; word < sizeof(sha256->state) / sizeof(uint32_t); word++)
    {
        SHAEncode(digest, sha256->state [word]);
        digest += sizeof(uint32_t);
    }

    memset(sha256, 0, sizeof(struct sha256));
    return;
}