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

#include "NidNmk.h"


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 = word; 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;
}