#include "spn.h"

unsigned char do_perm(unsigned char w, unsigned char perm[8]) {
    unsigned char output = 0x00;

    for (int i = 0; i < 8; i++) {
        output |= (w & (1 << i)) >> i << perm[i];
    }

    return output;
}

unsigned char do_subst(unsigned char w, unsigned char subst[16]) {
    unsigned char output = 0x00;

    output |= subst[(w & 0xF)];
    output |= subst[(w & 0xF0) >> 4] << 4;

    return output;
}

unsigned char encrypt(
	 unsigned char w,
	 unsigned short key,
	 unsigned char perm[8],
	 unsigned char subst[16]
) {
    unsigned char k0 = key & 0xFF;
    unsigned char k1 = (key & 0xFF00) >> 8;

    w ^= k0;
    w = do_subst(w, subst);
    w = do_perm(w, perm);

    w ^= k1;
    w = do_subst(w, subst);
    w = do_perm(w, perm);

    return w;
}

unsigned char decrypt(
	 unsigned char w,
	 unsigned short key,
	 unsigned char perm[8],
	 unsigned char subst[16]
) {
    unsigned char k0 = key & 0xFF;
    unsigned char k1 = (key & 0xFF00) >> 8;

    w = do_perm(w, perm);
    w = do_subst(w, subst);
    w ^= k1;

    w = do_perm(w, perm);
    w = do_subst(w, subst);
    w ^= k0;

    return w;
}