/* Based on the public domain implementation in
 * crypto_hash/sha512/ref/ from http://bench.cr.yp.to/supercop.html
 * by D. J. Bernstein */

 #include <Library/UefiLib.h>
 #include <Library/BaseLib.h>
 #include <Library/UefiBootServicesTableLib.h>
 #include <Library/BaseMemoryLib.h>
 #include <Uefi.h>

#include "utils.h"
#include "sha2.h"

static UINT32 load_bigendian_32(const UINT8 *x) {
    return (UINT32)(x[3]) | (((UINT32)(x[2])) << 8) |
           (((UINT32)(x[1])) << 16) | (((UINT32)(x[0])) << 24);
}

static UINT64 load_bigendian_64(const UINT8 *x) {
    return (UINT64)(x[7]) | (((UINT64)(x[6])) << 8) |
           (((UINT64)(x[5])) << 16) | (((UINT64)(x[4])) << 24) |
           (((UINT64)(x[3])) << 32) | (((UINT64)(x[2])) << 40) |
           (((UINT64)(x[1])) << 48) | (((UINT64)(x[0])) << 56);
}

static VOID store_bigendian_32(UINT8 *x, UINT64 u) {
    x[3] = (UINT8) u;
    u >>= 8;
    x[2] = (UINT8) u;
    u >>= 8;
    x[1] = (UINT8) u;
    u >>= 8;
    x[0] = (UINT8) u;
}

static VOID store_bigendian_64(UINT8 *x, UINT64 u) {
    x[7] = (UINT8) u;
    u >>= 8;
    x[6] = (UINT8) u;
    u >>= 8;
    x[5] = (UINT8) u;
    u >>= 8;
    x[4] = (UINT8) u;
    u >>= 8;
    x[3] = (UINT8) u;
    u >>= 8;
    x[2] = (UINT8) u;
    u >>= 8;
    x[1] = (UINT8) u;
    u >>= 8;
    x[0] = (UINT8) u;
}

#define SHR(x, c) ((x) >> (c))
#define ROTR_32(x, c) (((x) >> (c)) | ((x) << (32 - (c))))
#define ROTR_64(x,c) (((x) >> (c)) | ((x) << (64 - (c))))

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

#define Sigma0_32(x) (ROTR_32(x, 2) ^ ROTR_32(x,13) ^ ROTR_32(x,22))
#define Sigma1_32(x) (ROTR_32(x, 6) ^ ROTR_32(x,11) ^ ROTR_32(x,25))
#define sigma0_32(x) (ROTR_32(x, 7) ^ ROTR_32(x,18) ^ SHR(x, 3))
#define sigma1_32(x) (ROTR_32(x,17) ^ ROTR_32(x,19) ^ SHR(x,10))

#define Sigma0_64(x) (ROTR_64(x,28) ^ ROTR_64(x,34) ^ ROTR_64(x,39))
#define Sigma1_64(x) (ROTR_64(x,14) ^ ROTR_64(x,18) ^ ROTR_64(x,41))
#define sigma0_64(x) (ROTR_64(x, 1) ^ ROTR_64(x, 8) ^ SHR(x,7))
#define sigma1_64(x) (ROTR_64(x,19) ^ ROTR_64(x,61) ^ SHR(x,6))

#define M_32(w0, w14, w9, w1) w0 = sigma1_32(w14) + (w9) + sigma0_32(w1) + (w0);
#define M_64(w0, w14, w9, w1) w0 = sigma1_64(w14) + (w9) + sigma0_64(w1) + (w0);

#define EXPAND_32           \
    M_32(w0, w14, w9, w1)   \
    M_32(w1, w15, w10, w2)  \
    M_32(w2, w0, w11, w3)   \
    M_32(w3, w1, w12, w4)   \
    M_32(w4, w2, w13, w5)   \
    M_32(w5, w3, w14, w6)   \
    M_32(w6, w4, w15, w7)   \
    M_32(w7, w5, w0, w8)    \
    M_32(w8, w6, w1, w9)    \
    M_32(w9, w7, w2, w10)   \
    M_32(w10, w8, w3, w11)  \
    M_32(w11, w9, w4, w12)  \
    M_32(w12, w10, w5, w13) \
    M_32(w13, w11, w6, w14) \
    M_32(w14, w12, w7, w15) \
    M_32(w15, w13, w8, w0)

#define EXPAND_64 \
  M_64(w0 ,w14,w9 ,w1 ) \
  M_64(w1 ,w15,w10,w2 ) \
  M_64(w2 ,w0 ,w11,w3 ) \
  M_64(w3 ,w1 ,w12,w4 ) \
  M_64(w4 ,w2 ,w13,w5 ) \
  M_64(w5 ,w3 ,w14,w6 ) \
  M_64(w6 ,w4 ,w15,w7 ) \
  M_64(w7 ,w5 ,w0 ,w8 ) \
  M_64(w8 ,w6 ,w1 ,w9 ) \
  M_64(w9 ,w7 ,w2 ,w10) \
  M_64(w10,w8 ,w3 ,w11) \
  M_64(w11,w9 ,w4 ,w12) \
  M_64(w12,w10,w5 ,w13) \
  M_64(w13,w11,w6 ,w14) \
  M_64(w14,w12,w7 ,w15) \
  M_64(w15,w13,w8 ,w0 )

#define F_32(w, k)                                   \
    T1 = h + Sigma1_32(e) + Ch(e, f, g) + (k) + (w); \
    T2 = Sigma0_32(a) + Maj(a, b, c);                \
    h = g;                                           \
    g = f;                                           \
    f = e;                                           \
    e = d + T1;                                      \
    d = c;                                           \
    c = b;                                           \
    b = a;                                           \
    a = T1 + T2;

#define F_64(w,k) \
    T1 = h + Sigma1_64(e) + Ch(e,f,g) + k + w; \
    T2 = Sigma0_64(a) + Maj(a,b,c); \
    h = g; \
    g = f; \
    f = e; \
    e = d + T1; \
    d = c; \
    c = b; \
    b = a; \
    a = T1 + T2;

static UINTN crypto_hashblocks_sha256(UINT8 *statebytes,
                                       const UINT8 *in, UINTN inlen) {
    UINT32 state[8];
    UINT32 a;
    UINT32 b;
    UINT32 c;
    UINT32 d;
    UINT32 e;
    UINT32 f;
    UINT32 g;
    UINT32 h;
    UINT32 T1;
    UINT32 T2;

    a = load_bigendian_32(statebytes + 0);
    state[0] = a;
    b = load_bigendian_32(statebytes + 4);
    state[1] = b;
    c = load_bigendian_32(statebytes + 8);
    state[2] = c;
    d = load_bigendian_32(statebytes + 12);
    state[3] = d;
    e = load_bigendian_32(statebytes + 16);
    state[4] = e;
    f = load_bigendian_32(statebytes + 20);
    state[5] = f;
    g = load_bigendian_32(statebytes + 24);
    state[6] = g;
    h = load_bigendian_32(statebytes + 28);
    state[7] = h;

    while (inlen >= 64) {
        UINT32 w0  = load_bigendian_32(in + 0);
        UINT32 w1  = load_bigendian_32(in + 4);
        UINT32 w2  = load_bigendian_32(in + 8);
        UINT32 w3  = load_bigendian_32(in + 12);
        UINT32 w4  = load_bigendian_32(in + 16);
        UINT32 w5  = load_bigendian_32(in + 20);
        UINT32 w6  = load_bigendian_32(in + 24);
        UINT32 w7  = load_bigendian_32(in + 28);
        UINT32 w8  = load_bigendian_32(in + 32);
        UINT32 w9  = load_bigendian_32(in + 36);
        UINT32 w10 = load_bigendian_32(in + 40);
        UINT32 w11 = load_bigendian_32(in + 44);
        UINT32 w12 = load_bigendian_32(in + 48);
        UINT32 w13 = load_bigendian_32(in + 52);
        UINT32 w14 = load_bigendian_32(in + 56);
        UINT32 w15 = load_bigendian_32(in + 60);

        F_32(w0, 0x428a2f98)
        F_32(w1, 0x71374491)
        F_32(w2, 0xb5c0fbcf)
        F_32(w3, 0xe9b5dba5)
        F_32(w4, 0x3956c25b)
        F_32(w5, 0x59f111f1)
        F_32(w6, 0x923f82a4)
        F_32(w7, 0xab1c5ed5)
        F_32(w8, 0xd807aa98)
        F_32(w9, 0x12835b01)
        F_32(w10, 0x243185be)
        F_32(w11, 0x550c7dc3)
        F_32(w12, 0x72be5d74)
        F_32(w13, 0x80deb1fe)
        F_32(w14, 0x9bdc06a7)
        F_32(w15, 0xc19bf174)

        EXPAND_32

        F_32(w0, 0xe49b69c1)
        F_32(w1, 0xefbe4786)
        F_32(w2, 0x0fc19dc6)
        F_32(w3, 0x240ca1cc)
        F_32(w4, 0x2de92c6f)
        F_32(w5, 0x4a7484aa)
        F_32(w6, 0x5cb0a9dc)
        F_32(w7, 0x76f988da)
        F_32(w8, 0x983e5152)
        F_32(w9, 0xa831c66d)
        F_32(w10, 0xb00327c8)
        F_32(w11, 0xbf597fc7)
        F_32(w12, 0xc6e00bf3)
        F_32(w13, 0xd5a79147)
        F_32(w14, 0x06ca6351)
        F_32(w15, 0x14292967)

        EXPAND_32

        F_32(w0, 0x27b70a85)
        F_32(w1, 0x2e1b2138)
        F_32(w2, 0x4d2c6dfc)
        F_32(w3, 0x53380d13)
        F_32(w4, 0x650a7354)
        F_32(w5, 0x766a0abb)
        F_32(w6, 0x81c2c92e)
        F_32(w7, 0x92722c85)
        F_32(w8, 0xa2bfe8a1)
        F_32(w9, 0xa81a664b)
        F_32(w10, 0xc24b8b70)
        F_32(w11, 0xc76c51a3)
        F_32(w12, 0xd192e819)
        F_32(w13, 0xd6990624)
        F_32(w14, 0xf40e3585)
        F_32(w15, 0x106aa070)

        EXPAND_32

        F_32(w0, 0x19a4c116)
        F_32(w1, 0x1e376c08)
        F_32(w2, 0x2748774c)
        F_32(w3, 0x34b0bcb5)
        F_32(w4, 0x391c0cb3)
        F_32(w5, 0x4ed8aa4a)
        F_32(w6, 0x5b9cca4f)
        F_32(w7, 0x682e6ff3)
        F_32(w8, 0x748f82ee)
        F_32(w9, 0x78a5636f)
        F_32(w10, 0x84c87814)
        F_32(w11, 0x8cc70208)
        F_32(w12, 0x90befffa)
        F_32(w13, 0xa4506ceb)
        F_32(w14, 0xbef9a3f7)
        F_32(w15, 0xc67178f2)

        a += state[0];
        b += state[1];
        c += state[2];
        d += state[3];
        e += state[4];
        f += state[5];
        g += state[6];
        h += state[7];

        state[0] = a;
        state[1] = b;
        state[2] = c;
        state[3] = d;
        state[4] = e;
        state[5] = f;
        state[6] = g;
        state[7] = h;

        in += 64;
        inlen -= 64;
    }

    store_bigendian_32(statebytes + 0, state[0]);
    store_bigendian_32(statebytes + 4, state[1]);
    store_bigendian_32(statebytes + 8, state[2]);
    store_bigendian_32(statebytes + 12, state[3]);
    store_bigendian_32(statebytes + 16, state[4]);
    store_bigendian_32(statebytes + 20, state[5]);
    store_bigendian_32(statebytes + 24, state[6]);
    store_bigendian_32(statebytes + 28, state[7]);

    return inlen;
}

static INTN crypto_hashblocks_sha512(UINT8 *statebytes,const UINT8 *in,UINT64 inlen)
{
  UINT64 state[8];
  UINT64 a;
  UINT64 b;
  UINT64 c;
  UINT64 d;
  UINT64 e;
  UINT64 f;
  UINT64 g;
  UINT64 h;
  UINT64 T1;
  UINT64 T2;

  a = load_bigendian_64(statebytes +  0); state[0] = a;
  b = load_bigendian_64(statebytes +  8); state[1] = b;
  c = load_bigendian_64(statebytes + 16); state[2] = c;
  d = load_bigendian_64(statebytes + 24); state[3] = d;
  e = load_bigendian_64(statebytes + 32); state[4] = e;
  f = load_bigendian_64(statebytes + 40); state[5] = f;
  g = load_bigendian_64(statebytes + 48); state[6] = g;
  h = load_bigendian_64(statebytes + 56); state[7] = h;

  while (inlen >= 128) {
    UINT64 w0  = load_bigendian_64(in +   0);
    UINT64 w1  = load_bigendian_64(in +   8);
    UINT64 w2  = load_bigendian_64(in +  16);
    UINT64 w3  = load_bigendian_64(in +  24);
    UINT64 w4  = load_bigendian_64(in +  32);
    UINT64 w5  = load_bigendian_64(in +  40);
    UINT64 w6  = load_bigendian_64(in +  48);
    UINT64 w7  = load_bigendian_64(in +  56);
    UINT64 w8  = load_bigendian_64(in +  64);
    UINT64 w9  = load_bigendian_64(in +  72);
    UINT64 w10 = load_bigendian_64(in +  80);
    UINT64 w11 = load_bigendian_64(in +  88);
    UINT64 w12 = load_bigendian_64(in +  96);
    UINT64 w13 = load_bigendian_64(in + 104);
    UINT64 w14 = load_bigendian_64(in + 112);
    UINT64 w15 = load_bigendian_64(in + 120);

    F_64(w0 ,0x428a2f98d728ae22ULL)
    F_64(w1 ,0x7137449123ef65cdULL)
    F_64(w2 ,0xb5c0fbcfec4d3b2fULL)
    F_64(w3 ,0xe9b5dba58189dbbcULL)
    F_64(w4 ,0x3956c25bf348b538ULL)
    F_64(w5 ,0x59f111f1b605d019ULL)
    F_64(w6 ,0x923f82a4af194f9bULL)
    F_64(w7 ,0xab1c5ed5da6d8118ULL)
    F_64(w8 ,0xd807aa98a3030242ULL)
    F_64(w9 ,0x12835b0145706fbeULL)
    F_64(w10,0x243185be4ee4b28cULL)
    F_64(w11,0x550c7dc3d5ffb4e2ULL)
    F_64(w12,0x72be5d74f27b896fULL)
    F_64(w13,0x80deb1fe3b1696b1ULL)
    F_64(w14,0x9bdc06a725c71235ULL)
    F_64(w15,0xc19bf174cf692694ULL)

    EXPAND_64

    F_64(w0 ,0xe49b69c19ef14ad2ULL)
    F_64(w1 ,0xefbe4786384f25e3ULL)
    F_64(w2 ,0x0fc19dc68b8cd5b5ULL)
    F_64(w3 ,0x240ca1cc77ac9c65ULL)
    F_64(w4 ,0x2de92c6f592b0275ULL)
    F_64(w5 ,0x4a7484aa6ea6e483ULL)
    F_64(w6 ,0x5cb0a9dcbd41fbd4ULL)
    F_64(w7 ,0x76f988da831153b5ULL)
    F_64(w8 ,0x983e5152ee66dfabULL)
    F_64(w9 ,0xa831c66d2db43210ULL)
    F_64(w10,0xb00327c898fb213fULL)
    F_64(w11,0xbf597fc7beef0ee4ULL)
    F_64(w12,0xc6e00bf33da88fc2ULL)
    F_64(w13,0xd5a79147930aa725ULL)
    F_64(w14,0x06ca6351e003826fULL)
    F_64(w15,0x142929670a0e6e70ULL)

    EXPAND_64

    F_64(w0 ,0x27b70a8546d22ffcULL)
    F_64(w1 ,0x2e1b21385c26c926ULL)
    F_64(w2 ,0x4d2c6dfc5ac42aedULL)
    F_64(w3 ,0x53380d139d95b3dfULL)
    F_64(w4 ,0x650a73548baf63deULL)
    F_64(w5 ,0x766a0abb3c77b2a8ULL)
    F_64(w6 ,0x81c2c92e47edaee6ULL)
    F_64(w7 ,0x92722c851482353bULL)
    F_64(w8 ,0xa2bfe8a14cf10364ULL)
    F_64(w9 ,0xa81a664bbc423001ULL)
    F_64(w10,0xc24b8b70d0f89791ULL)
    F_64(w11,0xc76c51a30654be30ULL)
    F_64(w12,0xd192e819d6ef5218ULL)
    F_64(w13,0xd69906245565a910ULL)
    F_64(w14,0xf40e35855771202aULL)
    F_64(w15,0x106aa07032bbd1b8ULL)

    EXPAND_64

    F_64(w0 ,0x19a4c116b8d2d0c8ULL)
    F_64(w1 ,0x1e376c085141ab53ULL)
    F_64(w2 ,0x2748774cdf8eeb99ULL)
    F_64(w3 ,0x34b0bcb5e19b48a8ULL)
    F_64(w4 ,0x391c0cb3c5c95a63ULL)
    F_64(w5 ,0x4ed8aa4ae3418acbULL)
    F_64(w6 ,0x5b9cca4f7763e373ULL)
    F_64(w7 ,0x682e6ff3d6b2b8a3ULL)
    F_64(w8 ,0x748f82ee5defb2fcULL)
    F_64(w9 ,0x78a5636f43172f60ULL)
    F_64(w10,0x84c87814a1f0ab72ULL)
    F_64(w11,0x8cc702081a6439ecULL)
    F_64(w12,0x90befffa23631e28ULL)
    F_64(w13,0xa4506cebde82bde9ULL)
    F_64(w14,0xbef9a3f7b2c67915ULL)
    F_64(w15,0xc67178f2e372532bULL)

    EXPAND_64

    F_64(w0 ,0xca273eceea26619cULL)
    F_64(w1 ,0xd186b8c721c0c207ULL)
    F_64(w2 ,0xeada7dd6cde0eb1eULL)
    F_64(w3 ,0xf57d4f7fee6ed178ULL)
    F_64(w4 ,0x06f067aa72176fbaULL)
    F_64(w5 ,0x0a637dc5a2c898a6ULL)
    F_64(w6 ,0x113f9804bef90daeULL)
    F_64(w7 ,0x1b710b35131c471bULL)
    F_64(w8 ,0x28db77f523047d84ULL)
    F_64(w9 ,0x32caab7b40c72493ULL)
    F_64(w10,0x3c9ebe0a15c9bebcULL)
    F_64(w11,0x431d67c49c100d4cULL)
    F_64(w12,0x4cc5d4becb3e42b6ULL)
    F_64(w13,0x597f299cfc657e2aULL)
    F_64(w14,0x5fcb6fab3ad6faecULL)
    F_64(w15,0x6c44198c4a475817ULL)

    a += state[0];
    b += state[1];
    c += state[2];
    d += state[3];
    e += state[4];
    f += state[5];
    g += state[6];
    h += state[7];

    state[0] = a;
    state[1] = b;
    state[2] = c;
    state[3] = d;
    state[4] = e;
    state[5] = f;
    state[6] = g;
    state[7] = h;

    in += 128;
    inlen -= 128;
  }

  store_bigendian_64(statebytes +  0,state[0]);
  store_bigendian_64(statebytes +  8,state[1]);
  store_bigendian_64(statebytes + 16,state[2]);
  store_bigendian_64(statebytes + 24,state[3]);
  store_bigendian_64(statebytes + 32,state[4]);
  store_bigendian_64(statebytes + 40,state[5]);
  store_bigendian_64(statebytes + 48,state[6]);
  store_bigendian_64(statebytes + 56,state[7]);

  return inlen;
}


static const UINT8 iv_256[32] = {
    0x6a, 0x09, 0xe6, 0x67, 0xbb, 0x67, 0xae, 0x85,
    0x3c, 0x6e, 0xf3, 0x72, 0xa5, 0x4f, 0xf5, 0x3a,
    0x51, 0x0e, 0x52, 0x7f, 0x9b, 0x05, 0x68, 0x8c,
    0x1f, 0x83, 0xd9, 0xab, 0x5b, 0xe0, 0xcd, 0x19
};

static const UINT8 iv_512[64] = {
    0x6a, 0x09, 0xe6, 0x67, 0xf3, 0xbc, 0xc9, 0x08, 0xbb, 0x67, 0xae,
    0x85, 0x84, 0xca, 0xa7, 0x3b, 0x3c, 0x6e, 0xf3, 0x72, 0xfe, 0x94,
    0xf8, 0x2b, 0xa5, 0x4f, 0xf5, 0x3a, 0x5f, 0x1d, 0x36, 0xf1, 0x51,
    0x0e, 0x52, 0x7f, 0xad, 0xe6, 0x82, 0xd1, 0x9b, 0x05, 0x68, 0x8c,
    0x2b, 0x3e, 0x6c, 0x1f, 0x1f, 0x83, 0xd9, 0xab, 0xfb, 0x41, 0xbd,
    0x6b, 0x5b, 0xe0, 0xcd, 0x19, 0x13, 0x7e, 0x21, 0x79
};

VOID sha256_inc_init(UINT8 *state) {
    for (UINTN i = 0; i < 32; ++i) {
        state[i] = iv_256[i];
    }
    for (UINTN i = 32; i < 40; ++i) {
        state[i] = 0;
    }
}

VOID sha512_inc_init(UINT8 *state) {
    for (UINTN i = 0; i < 64; ++i) {
        state[i] = iv_512[i];
    }
    for (UINTN i = 64; i < 72; ++i) {
        state[i] = 0;
    }
}

VOID sha256_inc_blocks(UINT8 *state, const UINT8 *in, UINTN inblocks) {
    UINT64 bytes = load_bigendian_64(state + 32);

    crypto_hashblocks_sha256(state, in, 64 * inblocks);
    bytes += 64 * inblocks;

    store_bigendian_64(state + 32, bytes);
}

VOID sha512_inc_blocks(UINT8 *state, const UINT8 *in, UINTN inblocks) {
    UINT64 bytes = load_bigendian_64(state + 64);

    crypto_hashblocks_sha512(state, in, 128 * inblocks);
    bytes += 128 * inblocks;

    store_bigendian_64(state + 64, bytes);
}

VOID sha256_inc_finalize(UINT8 *out, UINT8 *state, const UINT8 *in, UINTN inlen) {
    UINT8 padded[128];
    UINT64 bytes = load_bigendian_64(state + 32) + inlen;

    crypto_hashblocks_sha256(state, in, inlen);
    in += inlen;
    inlen &= 63;
    in -= inlen;

    for (UINTN i = 0; i < inlen; ++i) {
        padded[i] = in[i];
    }
    padded[inlen] = 0x80;

    if (inlen < 56) {
        for (UINTN i = inlen + 1; i < 56; ++i) {
            padded[i] = 0;
        }
        padded[56] = (UINT8) (bytes >> 53);
        padded[57] = (UINT8) (bytes >> 45);
        padded[58] = (UINT8) (bytes >> 37);
        padded[59] = (UINT8) (bytes >> 29);
        padded[60] = (UINT8) (bytes >> 21);
        padded[61] = (UINT8) (bytes >> 13);
        padded[62] = (UINT8) (bytes >> 5);
        padded[63] = (UINT8) (bytes << 3);
        crypto_hashblocks_sha256(state, padded, 64);
    } else {
        for (UINTN i = inlen + 1; i < 120; ++i) {
            padded[i] = 0;
        }
        padded[120] = (UINT8) (bytes >> 53);
        padded[121] = (UINT8) (bytes >> 45);
        padded[122] = (UINT8) (bytes >> 37);
        padded[123] = (UINT8) (bytes >> 29);
        padded[124] = (UINT8) (bytes >> 21);
        padded[125] = (UINT8) (bytes >> 13);
        padded[126] = (UINT8) (bytes >> 5);
        padded[127] = (UINT8) (bytes << 3);
        crypto_hashblocks_sha256(state, padded, 128);
    }

    for (UINTN i = 0; i < 32; ++i) {
        out[i] = state[i];
    }

}

VOID sha512_inc_finalize(UINT8 *out, UINT8 *state, const UINT8 *in, UINTN inlen) {
    UINT8 padded[256];
    UINT64 bytes = load_bigendian_64(state + 64) + inlen;

    crypto_hashblocks_sha512(state, in, inlen);
    in += inlen;
    inlen &= 127;
    in -= inlen;

    for (UINTN i = 0; i < inlen; ++i) {
        padded[i] = in[i];
    }
    padded[inlen] = 0x80;

    if (inlen < 112) {
        for (UINTN i = inlen + 1; i < 119; ++i) {
            padded[i] = 0;
        }
        padded[119] = (UINT8) (bytes >> 61);
        padded[120] = (UINT8) (bytes >> 53);
        padded[121] = (UINT8) (bytes >> 45);
        padded[122] = (UINT8) (bytes >> 37);
        padded[123] = (UINT8) (bytes >> 29);
        padded[124] = (UINT8) (bytes >> 21);
        padded[125] = (UINT8) (bytes >> 13);
        padded[126] = (UINT8) (bytes >> 5);
        padded[127] = (UINT8) (bytes << 3);
        crypto_hashblocks_sha512(state, padded, 128);
    } else {
        for (UINTN i = inlen + 1; i < 247; ++i) {
            padded[i] = 0;
        }
        padded[247] = (UINT8) (bytes >> 61);
        padded[248] = (UINT8) (bytes >> 53);
        padded[249] = (UINT8) (bytes >> 45);
        padded[250] = (UINT8) (bytes >> 37);
        padded[251] = (UINT8) (bytes >> 29);
        padded[252] = (UINT8) (bytes >> 21);
        padded[253] = (UINT8) (bytes >> 13);
        padded[254] = (UINT8) (bytes >> 5);
        padded[255] = (UINT8) (bytes << 3);
        crypto_hashblocks_sha512(state, padded, 256);
    }

    for (UINTN i = 0; i < 64; ++i) {
        out[i] = state[i];
    }
}

VOID sha256(UINT8 *out, const UINT8 *in, UINTN inlen) {
    UINT8 state[40];

    sha256_inc_init(state);
    sha256_inc_finalize(out, state, in, inlen);
}

VOID sha512(UINT8 *out, const UINT8 *in, UINTN inlen) {
    UINT8 state[72];

    sha512_inc_init(state);
    sha512_inc_finalize(out, state, in, inlen);
}

/**
 * mgf1 function based on the SHA-256 hash function
 * Note that inlen should be sufficiently small that it still allows for
 * an array to be allocated on the stack. Typically 'in' is merely a seed.
 * Outputs outlen number of bytes
 */
VOID mgf1_256(UINT8 *out, UINTN outlen,
          const UINT8 *in, UINTN inlen)
{
    SPX_VLA(UINT8, inbuf, inlen+4);
    UINT8 outbuf[SPX_SHA256_OUTPUT_BYTES];
    UINTN i;

    CopyMem(inbuf, in, inlen);

    /* While we can fit in at least another full block of SHA256 output.. */
    for (i = 0; (i+1)*SPX_SHA256_OUTPUT_BYTES <= outlen; i++) {
        u32_to_bytes(inbuf + inlen, i);
        sha256(out, inbuf, inlen + 4);
        out += SPX_SHA256_OUTPUT_BYTES;
    }
    /* Until we cannot anymore, and we fill the remainder. */
    if (outlen > i*SPX_SHA256_OUTPUT_BYTES) {
        u32_to_bytes(inbuf + inlen, i);
        sha256(outbuf, inbuf, inlen + 4);
        CopyMem(out, outbuf, outlen - i*SPX_SHA256_OUTPUT_BYTES);
    }
}

/*
 * mgf1 function based on the SHA-512 hash function
 */
VOID mgf1_512(UINT8 *out, UINTN outlen,
          const UINT8 *in, UINTN inlen)
{
    SPX_VLA(UINT8, inbuf, inlen+4);
    UINT8 outbuf[SPX_SHA512_OUTPUT_BYTES];
    UINTN i;

    CopyMem(inbuf, in, inlen);

    /* While we can fit in at least another full block of SHA512 output.. */
    for (i = 0; (i+1)*SPX_SHA512_OUTPUT_BYTES <= outlen; i++) {
        u32_to_bytes(inbuf + inlen, i);
        sha512(out, inbuf, inlen + 4);
        out += SPX_SHA512_OUTPUT_BYTES;
    }
    /* Until we cannot anymore, and we fill the remainder. */
    if (outlen > i*SPX_SHA512_OUTPUT_BYTES) {
        u32_to_bytes(inbuf + inlen, i);
        sha512(outbuf, inbuf, inlen + 4);
        CopyMem(out, outbuf, outlen - i*SPX_SHA512_OUTPUT_BYTES);
    }
}


/**
 * Absorb the constant pub_seed using one round of the compression function
 * This initializes state_seeded and state_seeded_512, which can then be
 * reused in thash
 **/
VOID seed_state(spx_ctx *ctx) {
    UINT8 block[SPX_SHA512_BLOCK_BYTES];
    UINTN i;

    for (i = 0; i < SPX_N; ++i) {
        block[i] = ctx->pub_seed[i];
    }
    for (i = SPX_N; i < SPX_SHA512_BLOCK_BYTES; ++i) {
        block[i] = 0;
    }
    /* block has been properly initialized for both SHA-256 and SHA-512 */

    sha256_inc_init(ctx->state_seeded);
    sha256_inc_blocks(ctx->state_seeded, block, 1);
#if SPX_SHA512
    sha512_inc_init(ctx->state_seeded_512);
    sha512_inc_blocks(ctx->state_seeded_512, block, 1);
#endif
}
