#define _CRT_SECURE_NO_WARNINGS
#define __STDC_FORMAT_MACROS
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <inttypes.h>

static uint32_t rotate32r(uint32_t x, int n)
{
    return (x >> n) | (x << (32 - n));
}

static uint32_t rotate32l(uint32_t x, int n)
{
    return (x << n) | (x >> (32 - n));
}

static int parity(uint32_t x)
{
    x ^= x >> 16;
    x ^= x >> 8;
    x ^= x >> 4;
    x ^= x >> 2;
    x ^= x >> 1;
    return x & 1;
}

static uint32_t rround00(uint32_t y, uint32_t k, uint8_t flavor)
{
    static const unsigned char mtx[32][9] = {
        { 4,11,12,18,20,25,26,27,28},{ 5,12,13,19,21,26,27,28,29},
        { 6,13,14,20,22,27,28,29,30},{ 7,14,15,21,23,28,29,30,31},
        { 0, 8,15,16,22,24,29,30,31},{ 0, 1, 9,16,17,23,25,30,31},
        { 0, 1, 2,10,17,18,24,26,31},{ 0, 1, 2, 3,11,18,19,25,27},
        { 1, 2, 3, 4,12,19,20,26,28},{ 2, 3, 4, 5,13,20,21,27,29},
        { 3, 4, 5, 6,14,21,22,28,30},{ 4, 5, 6, 7,15,22,23,29,31},
        { 0, 5, 6, 7, 8,16,23,24,30},{ 1, 6, 7, 8, 9,17,24,25,31},
        { 0, 2, 7, 8, 9,10,18,25,26},{ 1, 3, 8, 9,10,11,19,26,27},
        { 2, 4, 9,10,11,12,20,27,28},{ 3, 5,10,11,12,13,21,28,29},
        { 4, 6,11,12,13,14,22,29,30},{ 5, 7,12,13,14,15,23,30,31},
        { 0, 6, 8,13,14,15,16,24,31},{ 0, 1, 7, 9,14,15,16,17,25},
        { 1, 2, 8,10,15,16,17,18,26},{ 2, 3, 9,11,16,17,18,19,27},
        { 3, 4,10,12,17,18,19,20,28},{ 4, 5,11,13,18,19,20,21,29},
        { 5, 6,12,14,19,20,21,22,30},{ 6, 7,13,15,20,21,22,23,31},
        { 0, 7, 8,14,16,21,22,23,24},{ 1, 8, 9,15,17,22,23,24,25},
        { 2, 9,10,16,18,23,24,25,26},{ 3,10,11,17,19,24,25,26,27}
    };
    const uint16_t salt = (flavor & 2) ? 0x5353 : 0;
    int i, j;
    uint32_t x = 0;
    uint32_t k_;
    for (i = 0; i < 32; i++) {
        int b = 0;
        for (j = 0; j < 9; j++) {
            b ^= (y >> mtx[i][j]);
        }
        x |= (b & 1) << i;
    }
    x = (x & 0x00ffff00) | (x >> 24) | (x << 24);
    x = (flavor & 1)
        ? (x & 0xaa55aa55) | ((x & 0x55005500) >> 7) | ((x & 0x00aa00aa) << 7)
        : (x & 0x55aa55aa) | ((x & 0xaa00aa00) >> 9) | ((x & 0x00550055) << 9);
    k_ = rotate32l(k, 1);
    x = (parity(x & k_)) ? x ^ ~k_ : x;
    x = ((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4);
    x = (0xffff & ((0xffff & x) - (k + salt))) | (((x >> 16) - ((k >> 16) + salt)) << 16);
    return x;
}

static uint32_t round00(uint32_t x, uint32_t k, uint8_t flavor)
{
    const uint16_t salt = (flavor & 2) ? 0x5353 : 0x0000;
    x = (0xffff & (x + k + salt)) | ((x >> 16) + (k >> 16) + salt) << 16;
    x = ((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4);
    k = rotate32l(k, 1);
    x = (parity(x & k)) ? x ^ ~k : x;
    x = (flavor & 1)
        ? (x & 0xaa55aa55) | ((x & 0x55005500) >> 7) | ((x & 0x00aa00aa) << 7)
        : (x & 0x55aa55aa) | ((x & 0xaa00aa00) >> 9) | ((x & 0x00550055) << 9);
    x = (x & 0x00ffff00) | (x >> 24) | (x << 24);
    x = x ^ rotate32r(x, 8)  ^ rotate32r(x, 7);
    return x;
}

static uint64_t rkeysched00(const uint32_t kext[2], uint8_t protocol)
{
    uint32_t k1 = rround00(kext[1], kext[0], 0x00);
    uint32_t k2 = round00(0x08090a0b, k1, 0x00);
    uint32_t k3 = round00(0x0c0d0e0f, k2, 0x00);
    uint32_t k4 = rround00(kext[0], k3, 0x00);
    uint32_t k5 = rround00(k4, ((protocol & 0x0c) == 0 ? 0x6aa32b6f : 0x84e5c4e7), 0x00);
    uint32_t k6 = rround00(k1, k4, 0x00);
    return ((uint64_t) k5 << 32) | k6;
}

static void keysched00(uint64_t key, uint32_t kext[4], uint8_t protocol)
{
    int i;
    uint32_t chain = (protocol & 0x0c) ? 0x84e5c4e7 : 0x6aa32b6f;
    kext[0] = (uint32_t) (key >> 32);
    kext[1] = (uint32_t) key;
    kext[2] = 0x08090a0b;
    kext[3] = 0x0c0d0e0f;
    for (i = 0; i < 8; i++) {
        kext[i & 3] = chain = round00(kext[i & 3], chain, 0);
    }
}

static uint32_t reverse32(uint32_t x)
{
    x = ((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8);
    return (x >> 16) | (x << 16);
}

int main(int argc, char **argv)
{
    int i;
    uint32_t kx[4], kx2[4];
    uint64_t k;

    if (argc < 5) {
        fprintf(stderr, "usage: %s SUBKEY1 SUBKEY2 SUBKEY3 SUBKEY4\n", argv[0]);
        exit(1);
    }

    for (i = 0; i < 4; i++) {
        kx[3 - i] = reverse32(strtoul(argv[i + 1], NULL, 16));
    }

    k = rkeysched00(kx, 0);
    keysched00(k, kx2, 0);

    if (memcmp(kx, kx2, sizeof(kx)) == 0)
        printf("Key: %"PRIx64"\n", k);
    else
        printf("invalid key\n");

    return 0;
}