#include "des.h"

/* DES bit numbering: bit 1 is the MSB of a 64-bit word. Tables match the Metal kernel. */

static const uint8_t IP_TBL[64] = {
    58,50,42,34,26,18,10, 2, 60,52,44,36,28,20,12, 4,
    62,54,46,38,30,22,14, 6, 64,56,48,40,32,24,16, 8,
    57,49,41,33,25,17, 9, 1, 59,51,43,35,27,19,11, 3,
    61,53,45,37,29,21,13, 5, 63,55,47,39,31,23,15, 7
};
static const uint8_t FP_TBL[64] = {
    40, 8,48,16,56,24,64,32, 39, 7,47,15,55,23,63,31,
    38, 6,46,14,54,22,62,30, 37, 5,45,13,53,21,61,29,
    36, 4,44,12,52,20,60,28, 35, 3,43,11,51,19,59,27,
    34, 2,42,10,50,18,58,26, 33, 1,41, 9,49,17,57,25
};
static const uint8_t E_TBL[48] = {
    32, 1, 2, 3, 4, 5,  4, 5, 6, 7, 8, 9,
     8, 9,10,11,12,13, 12,13,14,15,16,17,
    16,17,18,19,20,21, 20,21,22,23,24,25,
    24,25,26,27,28,29, 28,29,30,31,32, 1
};
static const uint8_t P_TBL[32] = {
    16, 7,20,21,29,12,28,17,  1,15,23,26, 5,18,31,10,
     2, 8,24,14,32,27, 3, 9, 19,13,30, 6,22,11, 4,25
};
static const uint8_t PC1_TBL[56] = {
    57,49,41,33,25,17, 9,  1,58,50,42,34,26,18,
    10, 2,59,51,43,35,27, 19,11, 3,60,52,44,36,
    63,55,47,39,31,23,15,  7,62,54,46,38,30,22,
    14, 6,61,53,45,37,29, 21,13, 5,28,20,12, 4
};
static const uint8_t PC2_TBL[48] = {
    14,17,11,24, 1, 5,  3,28,15, 6,21,10,
    23,19,12, 4,26, 8, 16, 7,27,20,13, 2,
    41,52,31,37,47,55, 30,40,51,45,33,48,
    44,49,39,56,34,53, 46,42,50,36,29,32
};
static const uint8_t SHIFTS[16] = {1,1,2,2,2,2,2,2,1,2,2,2,2,2,2,1};

static const uint8_t SBOX[8][64] = {
    {14,4,13,1,2,15,11,8,3,10,6,12,5,9,0,7, 0,15,7,4,14,2,13,1,10,6,12,11,9,5,3,8, 4,1,14,8,13,6,2,11,15,12,9,7,3,10,5,0, 15,12,8,2,4,9,1,7,5,11,3,14,10,0,6,13},
    {15,1,8,14,6,11,3,4,9,7,2,13,12,0,5,10, 3,13,4,7,15,2,8,14,12,0,1,10,6,9,11,5, 0,14,7,11,10,4,13,1,5,8,12,6,9,3,2,15, 13,8,10,1,3,15,4,2,11,6,7,12,0,5,14,9},
    {10,0,9,14,6,3,15,5,1,13,12,7,11,4,2,8, 13,7,0,9,3,4,6,10,2,8,5,14,12,11,15,1, 13,6,4,9,8,15,3,0,11,1,2,12,5,10,14,7, 1,10,13,0,6,9,8,7,4,15,14,3,11,5,2,12},
    {7,13,14,3,0,6,9,10,1,2,8,5,11,12,4,15, 13,8,11,5,6,15,0,3,4,7,2,12,1,10,14,9, 10,6,9,0,12,11,7,13,15,1,3,14,5,2,8,4, 3,15,0,6,10,1,13,8,9,4,5,11,12,7,2,14},
    {2,12,4,1,7,10,11,6,8,5,3,15,13,0,14,9, 14,11,2,12,4,7,13,1,5,0,15,10,3,9,8,6, 4,2,1,11,10,13,7,8,15,9,12,5,6,3,0,14, 11,8,12,7,1,14,2,13,6,15,0,9,10,4,5,3},
    {12,1,10,15,9,2,6,8,0,13,3,4,14,7,5,11, 10,15,4,2,7,12,9,5,6,1,13,14,0,11,3,8, 9,14,15,5,2,8,12,3,7,0,4,10,1,13,11,6, 4,3,2,12,9,5,15,10,11,14,1,7,6,0,8,13},
    {4,11,2,14,15,0,8,13,3,12,9,7,5,10,6,1, 13,0,11,7,4,9,1,10,14,3,5,12,2,15,8,6, 1,4,11,13,12,3,7,14,10,15,6,8,0,5,9,2, 6,11,13,8,1,4,10,7,9,5,0,15,14,2,3,12},
    {13,2,8,4,6,15,11,1,10,9,3,14,5,0,12,7, 1,15,13,8,10,3,7,4,12,5,6,11,0,14,9,2, 7,11,4,1,9,12,14,2,0,6,10,13,15,3,5,8, 2,1,14,7,4,10,8,13,15,12,9,0,3,5,6,11}
};

static uint64_t perm(uint64_t src, const uint8_t *tbl, uint32_t nout, uint32_t srcbits) {
    uint64_t outv = 0;
    uint32_t i;
    for (i = 0; i < nout; i++) {
        uint64_t bit = (src >> (srcbits - tbl[i])) & 1ULL;
        outv = (outv << 1) | bit;
    }
    return outv;
}

static uint32_t rotl28(uint32_t v, uint32_t s) {
    return ((v << s) | (v >> (28 - s))) & 0x0FFFFFFFu;
}

static uint32_t feistel(uint32_t r, uint64_t subkey) {
    uint64_t er = perm((uint64_t)r, E_TBL, 48, 32) ^ subkey;
    uint32_t s = 0;
    uint32_t i;
    for (i = 0; i < 8; i++) {
        uint32_t chunk = (uint32_t)((er >> (42 - 6 * i)) & 0x3F);
        uint32_t row = ((chunk & 0x20) >> 4) | (chunk & 1);
        uint32_t col = (chunk >> 1) & 0xF;
        s = (s << 4) | (uint32_t)SBOX[i][row * 16 + col];
    }
    return (uint32_t)perm((uint64_t)s, P_TBL, 32, 32);
}

void des_key_schedule(uint64_t key, uint64_t sk[16]) {
    uint64_t cd = perm(key, PC1_TBL, 56, 64);
    uint32_t c = (uint32_t)(cd >> 28);
    uint32_t d = (uint32_t)(cd & 0x0FFFFFFFULL);
    uint32_t r;
    for (r = 0; r < 16; r++) {
        c = rotl28(c, SHIFTS[r]);
        d = rotl28(d, SHIFTS[r]);
        uint64_t cd2 = ((uint64_t)c << 28) | (uint64_t)d;
        sk[r] = perm(cd2, PC2_TBL, 48, 56);
    }
}

uint64_t des_crypt(uint64_t block, const uint64_t sk[16], int decrypt) {
    uint64_t ip = perm(block, IP_TBL, 64, 64);
    uint32_t l = (uint32_t)(ip >> 32);
    uint32_t r = (uint32_t)(ip & 0xFFFFFFFFULL);
    uint32_t i;
    for (i = 0; i < 16; i++) {
        uint32_t rnd = decrypt ? (15 - i) : i;
        uint32_t n = l ^ feistel(r, sk[rnd]);
        l = r;
        r = n;
    }
    uint64_t pre = ((uint64_t)r << 32) | (uint64_t)l;
    return perm(pre, FP_TBL, 64, 64);
}

uint64_t make_key(uint64_t index, uint32_t key_len, uint32_t charset_len, uint32_t pad_byte,
                  const uint8_t *charset) {
    uint8_t bytes[8];
    uint32_t i;
    int pos;
    uint64_t n = index;
    uint64_t key = 0;
    for (i = 0; i < 8; i++) {
        bytes[i] = (uint8_t)pad_byte;
    }
    for (pos = (int)key_len - 1; pos >= 0; pos--) {
        bytes[pos] = charset[n % charset_len];
        n /= charset_len;
    }
    for (i = 0; i < 8; i++) {
        key = (key << 8) | (uint64_t)bytes[i];
    }
    return key;
}

int is_fill(uint64_t pt, const uint64_t *fills, uint32_t fill_count) {
    uint32_t i;
    for (i = 0; i < fill_count; i++) {
        if (pt == fills[i]) {
            return 1;
        }
    }
    return 0;
}

int des_selftest(void) {
    uint64_t key = 0x133457799BBCDFF1ULL;
    uint64_t pt = 0x0123456789ABCDEFULL;
    uint64_t expected = 0x85E813540F0AB405ULL;
    uint64_t sk[16];
    uint64_t ct;
    uint64_t roundtrip;
    des_key_schedule(key, sk);
    ct = des_crypt(pt, sk, 0);
    roundtrip = des_crypt(ct, sk, 1);
    return ct == expected && roundtrip == pt;
}