Cipher POC #2

#include <stdint.h>
#include <stdio.h>
#include <inttypes.h>
#include <string.h>

#define ROUNDS 32 //32 plus 1/2 rounds
#define SUBKEYS (ROUNDS+2)

static const uint64_t pi[] = {
    0x243f6a8885a308d3, 0x13198a2e03707344, 0xa4093822299f31d0, 0x082efa98ec4e6c89
};

struct schedule
{
    uint64_t subkeys[SUBKEYS][4];
};

uint64_t rotl(uint64_t x, uint8_t r)
{
    return (x << r) | (x >> (64-r));
}

void F(const uint64_t x[4], uint64_t y[4])
{
    y[0] = x[1] + x[2] + x[3];
    y[1] = x[2] + x[0] + x[3];
    y[2] = x[1] - x[0] - x[3];
    y[3] = x[0] - x[1] - x[2];
}

void G(const uint64_t x[4], uint64_t y[4])
{
    y[0] = rotl(x[1],17) ^ rotl(x[2],30) ^ rotl(x[3],48);
    y[1] = rotl(x[0],47) ^ rotl(x[2],13) ^ rotl(x[3],31);
    y[2] = rotl(x[0],34) ^ rotl(x[1],51) ^ rotl(x[3],18);
    y[3] = rotl(x[0],16) ^ rotl(x[1],33) ^ rotl(x[2],46);
}

void P(const uint64_t x[4], uint64_t y[4])
{
    y[0] = x[2] - x[1] + x[3];
    y[1] = x[3] - x[0] - x[2];
    y[2] = x[0] + x[1] - x[3];
    y[3] = x[0] + x[1] + x[2];
}

void Q(const uint64_t x[4], uint64_t y[4])
{
    y[0] = rotl(x[1],13) ^ rotl(x[2],40) ^ rotl(x[3],56);
    y[1] = rotl(x[0],51) ^ rotl(x[2],27) ^ rotl(x[3],43);
    y[2] = rotl(x[0],24) ^ rotl(x[1],37) ^ rotl(x[3],16);
    y[3] = rotl(x[0], 8) ^ rotl(x[1],21) ^ rotl(x[2],48);
}

int crypt_init(const char *key, size_t key_len, struct schedule *s)
{
    uint64_t k[4], t1[4], t2[4], count = 0;
    int i, j;

    if (key_len != 32)
        return 0;

    memcpy(k, key, key_len);

    for (i=0; i<SUBKEYS/2; i++)
    {
        k[0] ^= pi[0];
        k[1] ^= pi[1];
        k[2] ^= pi[2];
        k[3] ^= pi[3];

        for (j=0; j<4; j++)
        {
            Q(k,t1);
            P(t1,k);
            t2[j] = k[j];
            k[j] ^= ++count;
        }

        Q(t1,t2);
        P(t2,s->subkeys[i]);

        if (i == 0)
        {
            memcpy(s->subkeys[SUBKEYS-1-i], s->subkeys[i], sizeof(s->subkeys[i]));
        }
        else
        {
            G(s->subkeys[i],s->subkeys[SUBKEYS-1-i]);
        }
    }
    return 1;
}

void crypt(const uint64_t in[4], const uint64_t tweak[4],
        uint64_t out[4], const struct schedule *s)
{
    uint64_t a[4], b[4];
    int i;

    a[0] = in[0];
    a[1] = in[1];
    a[2] = in[2];
    a[3] = in[3];

    for (i=0; i<ROUNDS; i++)
    {
        a[0] ^= s->subkeys[i][0];
        a[1] ^= s->subkeys[i][1];
        a[2] ^= s->subkeys[i][2];
        a[3] ^= s->subkeys[i][3];

        F(a,b);

        b[0] += tweak[0];
        b[1] += tweak[1];
        b[2] += tweak[2];
        b[3] += tweak[3];

        G(b,a);
    }
    a[0] ^= s->subkeys[SUBKEYS-2][0];
    a[1] ^= s->subkeys[SUBKEYS-2][1];
    a[2] ^= s->subkeys[SUBKEYS-2][2];
    a[3] ^= s->subkeys[SUBKEYS-2][3];

    F(a,b);

    b[0] += tweak[0];
    b[1] += tweak[1];
    b[2] += tweak[2];
    b[3] += tweak[3];

    out[0] = b[0] ^ s->subkeys[SUBKEYS-1][0];
    out[1] = b[1] ^ s->subkeys[SUBKEYS-1][1];
    out[2] = b[2] ^ s->subkeys[SUBKEYS-1][2];
    out[3] = b[3] ^ s->subkeys[SUBKEYS-1][3];
}


int main()
{
    struct schedule s;
    uint64_t x[4] = {0x0123456789ABCDEF,0x0000000000000000,0xFFFFFFFFFFFFFFFF,0xFEDCBA9876543210}, y[4], z[4];
    char k[32] = {0};
    uint64_t t1[4] = {0x8888888888888888,0x7777777777777777,0x0000000000000000,0x0000000000000001};
    uint64_t t2[4];

    if (crypt_init(k, sizeof(k), &s))
    {
        crypt(x, t1, y, &s);
        printf("%016"PRIx64"%016"PRIx64"%016"PRIx64"%016"PRIx64"\n", y[0], y[1], y[2], y[3]);

        t1[0] = 0-t1[0];
        t1[1] = 0-t1[1];
        t1[2] = 0-t1[2];
        t1[3] = 0-t1[3];

        F(t1,t2);

        crypt(y, t2, z, &s);
        printf("%016"PRIx64"%016"PRIx64"%016"PRIx64"%016"PRIx64"\n", z[0], z[1], z[2], z[3]);
    }
    return 0;
}