/*
 * MBC1 optimised implementation.
 *
 * Algorithm developed by Michael W. Bombardieri <bombardierix@gmail.com>
 * 
 * Version 0.0.2 (2006.09.21)
 * 
 * The following code is released into the public domain by its author.
 */

/* rotate 32-bit integers */
#define rotr32(a,b) ((((a) >> (b)) | ((a) << (32 - (b)))) & 0xffffffff)
#define rotl32(a,b) ((((a) << (b)) | ((a) >> (32 - (b)))) & 0xffffffff)

/* add b to a */
#define add32(a,b) (((a) + (b)) & 0xffffffff)

/* subtract b from a */
#define sub32(a,b) (((a) - (b)) & 0xffffffff)

#define whiten(a,b,c,d,i)	      \
	blk[a] ^= key[(i + 16) % 32]; \
	blk[b] ^= key[(i + 19) % 32]; \
	blk[c] ^= key[(i + 21) % 32]; \
	blk[d] ^= key[(i + 23) % 32]  

#define GF_ENC(a,b,c,d,i)                                    \
	whiten(a, b, c, d, i);                               \
	blk[a] = add32(blk[a], key[i]);	                     \
	blk[b] = add32(blk[b], rotl32(blk[a], 1));           \
	blk[c] = add32(blk[c], rotl32(blk[b], 2));           \
	blk[d] = add32(blk[d], rotl32(blk[c], 3));           \
	fbk = add32(rotl32(blk[d], 5), key[i]) ^ key[i + 1]; \
	blk[a] ^= fbk;                                       \
	blk[b] ^= rotl32(fbk, 1);                            \
	blk[c] ^= rotl32(fbk, 2);                            \
	blk[d] ^= rotl32(key[i], 1)
	
#define GF_DEC(a,b,c,d,i)                                    \
	blk[d] ^= rotl32(key[i], 1);                         \
	fbk = add32(rotl32(blk[d], 5), key[i]) ^ key[i + 1]; \
	blk[a] ^= fbk;                                       \
	blk[b] ^= rotl32(fbk, 1);                            \
	blk[c] ^= rotl32(fbk, 2);                            \
	blk[d] = sub32(blk[d], rotl32(blk[c], 3));           \
	blk[c] = sub32(blk[c], rotl32(blk[b], 2));           \
	blk[b] = sub32(blk[b], rotl32(blk[a], 1));           \
	blk[a] = sub32(blk[a], key[i]);                      \
	whiten(a, b, c, d, i)

#define k1(a,b,c,d,i)                                   \
	blk[a] = add32(blk[a], iv_tbl[(i) % 64]);       \
	blk[b] = add32(blk[b], iv_tbl[((i) + 1) % 64]); \
	blk[c] = add32(blk[c], iv_tbl[((i) + 2) % 64]); \
	blk[d] = add32(blk[d], iv_tbl[((i) + 3) % 64])

#define key_blk(a,b,c,d,i)               \
	key[i] = rotl32(blk[a], 5);      \
	key[i + 1] = rotl32(blk[b],  9); \
	key[i + 2] = rotl32(blk[c], 13); \
	key[i + 3] = rotl32(blk[d], 17)

typedef unsigned char u1byte;
typedef unsigned long u4byte;	/* may be wider than 32-bit */

/* initialisation vector for the key schedule */
static u4byte iv_tbl[64] = 
{
	0xFA5D7CEC, 0x734533F8, 0x484EA4B2, 0x2BF0E46B,
	0xA853884B, 0xCBE7DEE0, 0x976DF92A, 0xE2B17575,
	0xF9BC1EFD, 0xF8ADC573, 0x2E5B6EA9, 0x1C5A4FFD,
	0x3DF27762, 0x2D7BF0BD, 0x039D26D6, 0x0A937D09,
	0x01052BC2, 0x8AEEEA1C, 0xFB7D3EBE, 0x2449A182,
	0x17B71BA3, 0xC6E68955, 0x74C7A0CD, 0xE1B88745,
	0x4D71958C, 0x149CEB88, 0x5344E0B9, 0xC3F0A21F,
	0xA348429B, 0xF672C8CD, 0xD01FA55C, 0x4C6FD3DF,
	0xC05B564E, 0xE0D81861, 0xA3B3EAAC, 0x17AE4A08,
	0x9D8DF3BF, 0x1353CBFE, 0x25C0F1C8, 0xD52818EC,
	0x1BA102B0, 0xC2FDC394, 0x9AB0505E, 0xF8D5F8CB,
	0x708749DA, 0x34C5D60B, 0xD1C046B0, 0x4C8E4672,
	0x0E236A84, 0x5A36C5AB, 0xDC9B9EB5, 0x0ADB5718,
	0x57D27CC5, 0xC4ECAEFD, 0xB57C0D9E, 0x764E92EA,
	0x4CA01ED6, 0xA7314405, 0xEE5E6B4B, 0xF9560189,
	0xC50CD4D5, 0xBFE0AFD8, 0x8322F984, 0x65601B60
};

static u4byte key[32];

void
set_key(const u4byte in_key[4])		/* prepare key schedule */
{
	u4byte blk[4];
	u4byte fbk = 0x19860719;	/* scheduler constant */
	u4byte t;
	u1byte key_exp[64];
	int x;
	int i;

	blk[0] = add32(in_key[0], rotl32(fbk, 3));
	blk[1] = add32(in_key[1], rotl32(blk[0], 5));
	blk[2] = add32(in_key[2], rotl32(blk[1], 7));
	blk[3] = add32(in_key[3], rotl32(blk[2], 11));

	x = rotl32(blk[3], 1) % 64;
	fbk ^= add32(rotl32(blk[3], 2), iv_tbl[x]);

	blk[0] ^= rotl32(fbk, 1);
	blk[1] ^= rotl32(blk[0], 2);
	blk[2] ^= rotl32(blk[1], 3);
	blk[3] ^= rotl32(blk[2], 4);
	
	x = (x + (blk[0] & 0xff)) % 64;
	k1(2, 1, 0, 3, x);
	k1(1, 2, 0, 3, x);
	k1(2, 0, 1, 3, x);
	k1(0, 2, 1, 3, x);
	key_blk(0, 1, 2, 3, 0);

	k1(3, 0, 1, 2, x);      key_blk(3, 0, 1, 2,  4);
	k1(3, 0, 2, 1, x +  4); key_blk(3, 0, 2, 1,  8);
	k1(3, 1, 0, 2, x +  8); key_blk(3, 1, 0, 2, 12);
	k1(3, 1, 2, 0, x + 12); key_blk(3, 1, 2, 0, 16);
	k1(3, 2, 0, 1, x + 16); key_blk(3, 2, 0, 1, 20);
	k1(3, 2, 1, 0, x + 20); key_blk(3, 2, 1, 0, 24);
	k1(1, 0, 2, 3, x + 24); key_blk(1, 0, 2, 3, 28);

	for (i = 0; i < 32; i++) {
		key_exp[i] = key[i] & 0xff;
		key_exp[i + 32] = (key[i] & 0xff00) >> 8;
	}

	for (i = 0; i < 64; i++) {
		x = key_exp[i] % 32;
		t = key[i % 32];
		key[i % 32] = key[x];
		key[x] = t;
	}
}

void 
encrypt(u4byte blk[4])		/* encrypt a block of text */
{
	u4byte fbk;

	GF_ENC(3, 0, 1, 2,  0); GF_ENC(3, 0, 2, 1,  1);
	GF_ENC(3, 1, 0, 2,  2); GF_ENC(3, 1, 2, 0,  3);
	GF_ENC(3, 2, 0, 1,  4); GF_ENC(3, 2, 1, 0,  5);
	GF_ENC(1, 0, 2, 3,  6); GF_ENC(1, 0, 3, 2,  7);
	GF_ENC(1, 2, 0, 3,  8); GF_ENC(1, 2, 3, 0,  9);
	GF_ENC(0, 1, 2, 3, 10); GF_ENC(0, 1, 3, 2, 11);
	GF_ENC(0, 2, 1, 3, 12); GF_ENC(0, 2, 3, 1, 13);
	GF_ENC(0, 3, 1, 2, 14); GF_ENC(0, 3, 2, 1, 15);
}

void 
decrypt(u4byte blk[4])		/* decrypt a block of text */
{
	u4byte fbk;

	GF_DEC(0, 3, 2, 1, 15); GF_DEC(0, 3, 1, 2, 14); 
	GF_DEC(0, 2, 3, 1, 13); GF_DEC(0, 2, 1, 3, 12);
	GF_DEC(0, 1, 3, 2, 11); GF_DEC(0, 1, 2, 3, 10); 
	GF_DEC(1, 2, 3, 0,  9); GF_DEC(1, 2, 0, 3,  8); 
	GF_DEC(1, 0, 3, 2,  7); GF_DEC(1, 0, 2, 3,  6); 
	GF_DEC(3, 2, 1, 0,  5); GF_DEC(3, 2, 0, 1,  4); 
	GF_DEC(3, 1, 2, 0,  3); GF_DEC(3, 1, 0, 2,  2); 
	GF_DEC(3, 0, 2, 1,  1); GF_DEC(3, 0, 1, 2,  0); 
}

#if defined DEBUG

#include <stdio.h>

int 
main(void)	/* demonstrate algorithm usage */
{
	u4byte in_key[4] = {0x11111111, 0x22222222, 0x33333333, 0x44444444};
	u4byte blk[4] = {0x00000000, 0x11111111, 0x22222222, 0x33333333};

	printf("%08lx%08lx%08lx%08lx = %s\n", in_key[0], in_key[1], 
	       in_key[2], in_key[3], "User key");

	printf("%08lx%08lx%08lx%08lx = %s\n", blk[0], blk[1], blk[2], 
	       blk[3], "Plaintext");

	set_key(&in_key[0]);

	encrypt(&blk[0]);

	printf("%08lx%08lx%08lx%08lx = %s\n", blk[0], blk[1], blk[2], 
	       blk[3], "Ciphertext");

	decrypt(&blk[0]);

	printf("%08lx%08lx%08lx%08lx = %s\n", blk[0], blk[1], blk[2], 
	       blk[3], "Plaintext");

	return 0;
}
#endif