#include "sha256.h"
#include <stdio.h>
#include <string.h>

#define ROTL(x,n) ((x << n) | (x >> (32 - n)))
#define ROTR(x,n) ((x >> n) | (x << (32 - n)))

#define SHL(x,n) (x << n)
#define SHR(x,n) (x >> n)

#define Ch(x,y,z) ((x & y) ^ (~x&z))
#define Maj(x,y,z) ((x & y) ^ (x & z) ^ (y & z))

#define MSIZE 64

static inline u32 Sigma0(u32 x) {
    return ROTR(x,2) ^ ROTR(x,13) ^ ROTR(x,22);
}

static inline u32 Sigma1(u32 x) {
    return ROTR(x,6) ^ ROTR(x,11) ^ ROTR(x,25);
}

static inline u32 sigma0(u32 x) {
    return ROTR(x,7) ^ ROTR(x,18) ^ SHR(x,3);
}

static inline u32 sigma1(u32 x) {
	return ROTR(x,17) ^ ROTR(x,19) ^ SHR(x,10);
}

const u32 K[64] = {
	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
};

static void initState(sha256State_t *state) {
	state->H[0]=0x6a09e667;
	state->H[1]=0xbb67ae85;
	state->H[2]=0x3c6ef372;
	state->H[3]=0xa54ff53a;
	state->H[4]=0x510e527f;
	state->H[5]=0x9b05688c;
	state->H[6]=0x1f83d9ab;
	state->H[7]=0x5be0cd19;
	
	state->length = 0;
	state->finalised = 0;
}

static void sha256Round(sha256State_t *state);

static void sha256Padding(sha256State_t *state) {
	state->message[state->message_length / 4] |= 0x80 << (3 - (state->message_length % 4)) * 8;

	if (state->message_length * 8 + 1 > 448) {
		state->message_length = 64;
		sha256Round(state);
		memset(state->message, 0x00, 16*sizeof(u32));
	}

	state->finalised = 1;
	state->length <<= 3;
	state->message[14] = state->length >> 32;
	state->message[15] = state->length & 0xffffffff;
}

static void sha256Round(sha256State_t *state) {
	u32 T1, T2;
	u32 a, b, c, d, e, f, g, h;

	if (state->message_length != 64) {
		sha256Padding(state);
	}	
	
	int t = 0;
	for (; t < 16; t++) 
		state->W[t] = state->message[t];

	for (; t < 64; t++) 
		state->W[t] = sigma1(state->W[t-2]) + state->W[t-7] + sigma0(state->W[t-15]) + state->W[t-16];

	a=state->H[0];
    b=state->H[1];
    c=state->H[2];
    d=state->H[3];
    e=state->H[4];
   	f=state->H[5];
   	g=state->H[6];
    h=state->H[7];
	
	for (t = 0; t < 64; t++) {
       	T1 = h + Sigma1(e) + Ch(e,f,g) + K[t] + state->W[t];
       	T2 = Sigma0(a) + Maj(a,b,c);
       	h = g;
       	g = f;
       	f = e;
       	e = d + T1;
       	d = c;
       	c = b;
       	b = a;
       	a = T1 + T2;
    }
	
	state->H[0] += a;
    state->H[1] += b;
    state->H[2] += c;
    state->H[3] += d;
    state->H[4] += e;
    state->H[5] += f;
    state->H[6] += g;
    state->H[7] += h;
}

static void sha256Hash(sha256State_t *state, u8 *buffer, size_t b_len) {
    if((buffer == NULL) || (state == NULL))
        return;
	
    state->length += b_len;

    size_t message_length = 0;
    while(b_len > 0) {
        message_length = (b_len < MSIZE) ? b_len : MSIZE;
        memset(state->message, 0x00, MSIZE);
        memcpy(state->message, buffer, message_length);

		for(int i = 0; i < 16; i++)
			state->message[i] = __builtin_bswap32(state->message[i]);

        state->message_length = message_length;
        sha256Round(state);

        buffer += message_length;;
        b_len -= message_length;;
    }
}

static void sha256Finalise(u8 *hash, sha256State_t *state) {
	if(!state->finalised) {
		memset(state->message, 0x00, 16*sizeof(u32));
        state->length <<= 3;
		state->message[0] = 0x80000000;
		state->message[14] = state->length >> 32;
		state->message[15] = state->length & 0xffffffff;
		state->message_length = 64;
		state->finalised = 1;
		sha256Round(state);
	}

	if(hash == NULL)
		return;

	for(int i = 0; i < 8; i++)
		state->H[i] =  __builtin_bswap32(state->H[i]);
	memcpy(hash, state->H, SIZE);
}

void sha256(u8 *hash, u8 *buffer, size_t buffer_length) {
    sha256State_t state;

    initState(&state);
    sha256Hash(&state, buffer, buffer_length);
    sha256Finalise(hash, &state);
}

#ifdef TEST
int main(int argc, char **argv) {
    unsigned char *tests[12] = {
        "",
        "\xe3\xb0\xc4\x42\x98\xfc\x1c\x14\x9a\xfb\xf4\xc8\x99\x6f\xb9\x24\x27\xae\x41\xe4\x64\x9b\x93\x4c\xa4\x95\x99\x1b\x78\x52\xb8\x55",

        "abc",
        "\xba\x78\x16\xbf\x8f\x01\xcf\xea\x41\x41\x40\xde\x5d\xae\x22\x23\xb0\x03\x61\xa3\x96\x17\x7a\x9c\xb4\x10\xff\x61\xf2\x00\x15\xad",

        "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq",
        "\x24\x8d\x6a\x61\xd2\x06\x38\xb8\xe5\xc0\x26\x93\x0c\x3e\x60\x39\xa3\x3c\xe4\x59\x64\xff\x21\x67\xf6\xec\xed\xd4\x19\xdb\x06\xc1",

        "abcdefghbcdefghicdefghijdefghijkefghijklfghijklmghijklmnhijklmnoijklmnopjklmnopqklmnopqrlmnopqrsmnopqrstnopqrstu",
        "\xcf\x5b\x16\xa7\x78\xaf\x83\x80\x03\x6c\xe5\x9e\x7b\x04\x92\x37\x0b\x24\x9b\x11\xe8\xf0\x7a\x51\xaf\xac\x45\x03\x7a\xfe\xe9\xd1",
        
        "\xbd",
        "\x68\x32\x57\x20\xaa\xbd\x7c\x82\xf3\x0f\x55\x4b\x31\x3d\x05\x70\xc9\x5a\xcc\xbb\x7d\xc4\xb5\xaa\xe1\x12\x04\xc0\x8f\xfe\x73\x2b",

        "\xc9\x8c\x8e\x55",
        "\x7a\xbc\x22\xc0\xae\x5a\xf2\x6c\xe9\x3d\xbb\x94\x43\x3a\x0e\x0b\x2e\x11\x9d\x01\x4f\x8e\x7f\x65\xbd\x56\xc6\x1c\xcc\xcd\x95\x04"
    };

    for(int i = 0; i < 12; i += 2) {
        u8 tmp[32];
        sha256(tmp, tests[i], strlen(tests[i]));
        for(int j = 0; j < 32; j++)
            printf("%02x", tmp[j]);
        printf(" ");
        for(int j = 0; j < 32; j++)
            printf("%02x", tests[i+1][j]);
        printf("\n");
    }

    u8 tmp[32];
    sha256State_t state;
    initState(&state);
    unsigned char *tvec = "abcdefghbcdefghicdefghijdefghijkefghijklfghijklmghijklmnhijklmno";
    unsigned char *tres = "\x50\xe7\x2a\x0e\x26\x44\x2f\xe2\x55\x2d\xc3\x93\x8a\xc5\x86\x58\x22\x8c\x0c\xbf\xb1\xd2\xca\x87\x2a\xe4\x35\x26\x6f\xcd\x05\x5e";
    for (size_t i = 0; i < 16777216; i++) {
        sha256Hash(&state, tvec, 64);
    }
    sha256Finalise(tmp, &state);
    for(int j = 0; j < 32; j++)
        printf("%02x", tmp[j]);
    printf(" ");
    for(int j = 0; j < 32; j++)
        printf("%02x", tres[j]);
    printf("\n");

    return 0;
}
#endif