/*
 * The QUAFFLER library
 * Copyright (c) 2006 Clifford Wolf <clifford@clifford.at>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
 * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 *
 * Note: The bigint library used in this file is just a quick hack. A cleaner
 * and faster implementation is on my todo list.
 *
 */

#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#ifdef USE_GMP
#  include <gmp.h>
#endif

#ifdef RSA_TEST

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>

#else

#include "quaffler.h"

#define rsa_keygen qfl_rsa_keygen
#define rsa_encrypt qfl_rsa_encrypt
#define rsa_decrypt qfl_rsa_decrypt

#endif

/******************************  Bigint ******************************/

struct bigint {
#ifdef USE_GMP
	mpz_t mpz_value;
#else
	unsigned char *value;
	unsigned int value_size;
	unsigned int value_realsize;
#endif
};

struct bigint *bigint_create(unsigned int size)
{
	struct bigint *rc = calloc(sizeof(struct bigint), 1);
#ifdef USE_GMP
	if (size) { /* do not warn about unused size */ }
	mpz_init(rc->mpz_value);
#else
	rc->value = calloc(rc->value_size = rc->value_realsize = size, 1);
#endif
	return rc;
}

struct bigint *bigint_copy(struct bigint *x)
{
	struct bigint *rc = calloc(sizeof(struct bigint), 1);
#ifdef USE_GMP
	mpz_init(rc->mpz_value);
	mpz_set(rc->mpz_value, x->mpz_value);
#else
	rc->value = calloc(rc->value_size = rc->value_realsize = x->value_size, 1);
	memcpy(rc->value, x->value, x->value_size);
#endif
	return rc;
}

#ifndef USE_GMP
static struct bigint *bigint_shift_and_add(struct bigint *x, int lowbyte)
{
	struct bigint *rc = calloc(sizeof(struct bigint), 1);
	rc->value = calloc(rc->value_size = rc->value_realsize = x->value_size + 1, 1);
	memcpy(rc->value+1, x->value, x->value_size);
	rc->value[0] = lowbyte;
	return rc;
}

static void bigint_pack(struct bigint *x)
{
	while (x->value_size && !x->value[x->value_size-1])
		x->value_size--;
}

static void bigint_expand(struct bigint *x, unsigned int size)
{
	if (size <= x->value_realsize)
		return;

	x->value = realloc(x->value, size);

	while (x->value_realsize < size)
		x->value[x->value_realsize++] = 0;
}
#endif

static struct bigint *bigint_fromint(unsigned int value)
{
	struct bigint *rc = bigint_create(4);
#ifdef USE_GMP
	mpz_set_ui(rc->mpz_value, value);
#else
	rc->value[0] = (value >>  0) & 255;
	rc->value[1] = (value >>  8) & 255;
	rc->value[2] = (value >> 16) & 255;
	rc->value[3] = (value >> 24) & 255;

	bigint_pack(rc);
#endif
	return rc;
}

static void bigint_free(struct bigint *x)
{
#ifdef USE_GMP
	mpz_clear(x->mpz_value);
#else
	free(x->value);
#endif
	free(x);
}

#ifdef RSA_TEST
static char *bigint_hexdump(struct bigint *x)
{
	static char *rc = 0; 
	unsigned char *value_data;
	int value_size;
	int i, j;

	free(rc);

	if (!x)
		return rc = strdup("NULL");

#ifdef USE_GMP
	value_size = mpz_size(x->mpz_value) * sizeof(mp_size_t);
	unsigned char my_value_data[value_size];
	mpz_export(my_value_data, 0, -1, 1, -1, 0, x->mpz_value);
	value_data = my_value_data;
#else
	value_size = x->value_size;
	value_data = x->value;
#endif

	if (!value_size)
		return rc = strdup("0");

	rc = malloc(value_size*2 + 2);

	for (i = value_size - 1, j = 0; i >= 0; i--, j+=2)
		snprintf(rc + j, 3, "%02x", value_data[i]);

	return rc;
}
#endif

#ifndef USE_GMP
static struct bigint *bigint_add(struct bigint *a, struct bigint *b)
{
#ifdef USE_GMP
	struct bigint *rc = bigint_create(0);
	mpz_add(rc->mpz_value, a->mpz_value, b->mpz_value);
#else
	int size = a->value_size > b->value_size ? a->value_size+1 : b->value_size+1;
	struct bigint *rc = bigint_create(size);
	int carry = 0, i, t;

	bigint_expand(a, size);
	bigint_expand(b, size);

	for (i=0; i<size; i++) {
		t = a->value[i] + (b->value[i] + carry);
		rc->value[i] = t & 255;
		carry = t >> 8;
	}

	assert(!carry);

	bigint_pack(rc);
#endif
	return rc;
}
#endif

#ifndef USE_GMP
static void bigint_subfrom(struct bigint *a, struct bigint *b)
{
	int carry = 0, i, t;

	for (i=0; i<b->value_size; i++) {
		t = b->value[i] + carry;
		carry = t > a->value[i];
		a->value[i] -= t;
	}

	for (; carry && i<a->value_size; i++) {
		if (a->value[i] == 0) {
			a->value[i] = -1;
		} else {
			a->value[i]--;
			carry = 0;
		}
	}

	assert(!carry);
	bigint_pack(a);
}
#endif

static struct bigint *bigint_sub(struct bigint *a, struct bigint *b)
{
#ifdef USE_GMP
	struct bigint *rc = bigint_create(0);
	mpz_sub(rc->mpz_value, a->mpz_value, b->mpz_value);
#else
	struct bigint *rc = bigint_copy(a);
	bigint_subfrom(rc, b);
#endif
	return rc;
}

static struct bigint *bigint_mul(struct bigint *a, struct bigint *b)
{
#ifdef USE_GMP
	struct bigint *rc = bigint_create(0);
	mpz_mul(rc->mpz_value, a->mpz_value, b->mpz_value);
#else
	int size = a->value_size > b->value_size ? a->value_size : b->value_size;
	struct bigint *rc = bigint_create(size * 2);
	int carry = 0, i, j, t;

	bigint_expand(a, size);
	bigint_expand(b, size+1);

	for (i=0; i<size; i++)
	{
		for (j=0; j<size+1; j++)
		{
			t = rc->value[i+j] + a->value[i] * b->value[j] + carry;
			rc->value[i+j] = t & 255;
			carry = t >> 8;
		}

		assert(!carry);
	}

	bigint_pack(rc);
#endif
	return rc;
}

#ifndef USE_GMP
static int bigint_ge(struct bigint *a, struct bigint *b)
{
	int size = a->value_size > b->value_size ? a->value_size : b->value_size;
	int i;

	bigint_expand(a, size);
	bigint_expand(b, size);

	for (i=size-1; i>=0; i--) {
		if (a->value[i] > b->value[i]) return 1;
		if (a->value[i] < b->value[i]) return 0;
	}

	return a->value[0] == b->value[0];
}

static int bigint_eq(struct bigint *a, struct bigint *b)
{
	int size = a->value_size > b->value_size ? a->value_size : b->value_size;
	int i;

	bigint_expand(a, size);
	bigint_expand(b, size);

	for (i=size-1; i>=0; i--) {
		if (a->value[i] != b->value[i]) return 0;
	}

	return 1;
}

static int bigint_smallmod(struct bigint *a, int b)
{
	int i, rem = 0;

	for (i=a->value_size-1; i>=0; i--)
		rem = (rem << 8 | a->value[i]) % b;

	return rem;
}

static void bigint_shr(struct bigint *x, int bits)
{
	int i, carry;

	if (bits <= 0)
		return;

	if (bits > 1) {
		for (i=0; i<bits; i++)
			bigint_shr(x, 1);
		return;
	}

	for (i=x->value_size-1, carry=0; i>=0; i--)
	{
		int oldval = x->value[i];
		x->value[i] = (oldval >> 1) | carry;
		carry = (oldval & 0x01) << 7;
	}
}

static void bigint_shl(struct bigint *x, int bits)
{
	int i, carry;

	if (bits <= 0)
		return;

	if (bits > 1) {
		for (i=0; i<bits; i++)
			bigint_shl(x, 1);
		return;
	}

	for (i=0, carry=0; i<x->value_size; i++)
	{
		int oldval = x->value[i];
		x->value[i] = (oldval << 1) | carry;
		carry = (oldval & 0x80) >> 7;
	}
}

static int bigint_divhelper(struct bigint *a, struct bigint **bmut, struct bigint**rem_p)
{
	int i, rc = 0;
	struct bigint *rem = bigint_copy(a);

	if (bigint_ge(rem, bmut[7]))
	{
		for (i=0; i<8; i++)
		{
			rc = rc << 1;
			if (bigint_ge(rem, bmut[i])) {
				bigint_subfrom(rem, bmut[i]);
				rc |= 1;
			}
		}
	}

	*rem_p = rem;
	return rc;
}

static struct bigint *bigint_div_backend(struct bigint *a, struct bigint *b, int ismod)
{
	int size = a->value_size;
	struct bigint *rc = bigint_create(size);
	struct bigint *div = bigint_fromint(0);
	struct bigint *olddiv, *bmut[8];
	int i;

	bigint_expand(a, size);
	bigint_expand(b, size);

	bmut[0] = bigint_shift_and_add(b, 0);
	bigint_shr(bmut[0], 1);

	for (i=1; i<8; i++) {
		bmut[i] = bigint_copy(bmut[i-1]);
		bigint_shr(bmut[i], 1);
	}

	for (i=size-1; i>=0; i--)
	{
		div = bigint_shift_and_add(olddiv=div, a->value[i]);
		bigint_free(olddiv);

		rc->value[i] = bigint_divhelper(olddiv=div, bmut, &div);
		bigint_free(olddiv);
	}

	for (i=0; i<8; i++)
		bigint_free(bmut[i]);

	if (ismod) {
		bigint_free(rc);
		rc = div;
	} else {
		bigint_free(div);
	}

	bigint_pack(rc);
	return rc;
}

static struct bigint *bigint_div(struct bigint *a, struct bigint *b)
{
#ifdef USE_GMP
	struct bigint *rc = bigint_create(0);
	mpz_tdiv_q(rc->mpz_value, a->mpz_value, b->mpz_value);
#else
	return bigint_div_backend(a, b, 0);
#endif
}
#endif

static struct bigint *bigint_mod(struct bigint *a, struct bigint *b)
{
#ifdef USE_GMP
	struct bigint *rc = bigint_create(0);
	mpz_tdiv_r(rc->mpz_value, a->mpz_value, b->mpz_value);
	return rc;
#else
	return bigint_div_backend(a, b, 1);
#endif
}

static struct bigint *bigint_expmod(struct bigint *a, struct bigint *b, struct bigint *c)
{
#ifdef USE_GMP
	struct bigint *rc = bigint_create(0);
	mpz_powm(rc->mpz_value, a->mpz_value, b->mpz_value, c->mpz_value);
#else
	struct bigint *subrc = bigint_copy(a);
	struct bigint *rc = bigint_fromint(1);
	struct bigint *oldrc, *oldsubrc;
	int i, subrc_level = 0;

	for (i=0; i<b->value_size*8; i++)
	{
		if ((b->value[i/8] & (1 << i%8)) == 0)
			continue;

		while (subrc_level < i) {
			subrc = bigint_mul(oldsubrc = subrc, subrc);
			bigint_free(oldsubrc);
			if (c) {
				subrc = bigint_mod(oldsubrc = subrc, c);
				bigint_free(oldsubrc);
			}
			subrc_level++;
		}

		rc = bigint_mul(oldrc = rc, subrc);
		bigint_free(oldrc);

		if (c) {
			rc = bigint_mod(oldrc = rc, c);
			bigint_free(oldrc);
		}
	}

	bigint_free(subrc);
#endif
	return rc;
}

#ifndef USE_GMP
static struct bigint *bigint_exp(struct bigint *a, struct bigint *b)
{
	return bigint_expmod(a, b, 0);
}

static inline void bigint_swap(struct bigint **a, struct bigint **b)
{
	struct bigint *t1 = *a;
	*a = *b; *b = t1;
}

static inline int bigint_iseven(struct bigint *x)
{
	if (x->value_size == 0)
		return 1;

	if ((x->value[0] & 1) == 0)
		return 1;

	return 0;
}

static inline int bigint_isodd(struct bigint *x)
{
	return !bigint_iseven(x);
}
#endif

static void bigint_exteuclid(struct bigint *u, struct bigint *v, struct bigint **a, struct bigint **b, struct bigint **c)
{
#ifdef USE_GMP
	struct bigint *u1 = bigint_create(0);
	struct bigint *u2 = bigint_create(0);
	struct bigint *u3 = bigint_create(0);

	mpz_gcdext(u3->mpz_value, u1->mpz_value, u2->mpz_value, u->mpz_value, v->mpz_value);
	mpz_neg(u2->mpz_value, u2->mpz_value);
#else
	struct bigint *t1, *t2, *t3, *u1, *u2, *u3, *h1;
	struct bigint *const_one = bigint_fromint(1);
	int k;

	u = bigint_copy(u);
	v = bigint_copy(v);

	if (!bigint_ge(u, v))
		bigint_swap(&u, &v);

	for (k=0; bigint_iseven(u) && bigint_iseven(v); k++) {
		bigint_shr(u, 1);
		bigint_shr(v, 1);
	}

	u1 = bigint_fromint(1);
	u2 = bigint_fromint(0);
	u3 = bigint_copy(u);
	t1 = bigint_copy(v);
	t2 = bigint_sub(u, const_one);
	t3 = bigint_copy(v);

	do {
		do {
			if (bigint_iseven(u3)) {
				if (bigint_isodd(u1) || bigint_isodd(u2)) {
					u1 = bigint_add(h1 = u1, v);
					bigint_free(h1);
					u2 = bigint_add(h1 = u2, u);
					bigint_free(h1);
				}
				bigint_shr(u1, 1);
				bigint_shr(u2, 1);
				bigint_shr(u3, 1);
			}
			if (bigint_iseven(t3) || !bigint_ge(u3, t3)) {
				bigint_swap(&u1, &t1);
				bigint_swap(&u2, &t2);
				bigint_swap(&u3, &t3);
			}
		} while (bigint_iseven(u3));
		while (!bigint_ge(u1, t1) || !bigint_ge(u2, t2)) {
			u1 = bigint_add(h1 = u1, v);
			bigint_free(h1);
			u2 = bigint_add(h1 = u2, u);
			bigint_free(h1);
		}
		u1 = bigint_sub(h1 = u1, t1);
		bigint_free(h1);
		u2 = bigint_sub(h1 = u2, t2);
		bigint_free(h1);
		u3 = bigint_sub(h1 = u3, t3);
		bigint_free(h1);
	} while (bigint_ge(t3, const_one));
	while (bigint_ge(u1, v) && bigint_ge(u2, u)) {
		u1 = bigint_sub(h1 = u1, v);
		bigint_free(h1);
		u2 = bigint_sub(h1 = u2, u);
		bigint_free(h1);
	}
	bigint_shl(u1, k);
	bigint_shl(u2, k);
	bigint_shl(u3, k);
#endif

	if (a) *a = u1;
	else bigint_free(u1);

	if (b) *b = u2;
	else bigint_free(u2);

	if (c) *c = u3;
	else bigint_free(u3);

#ifndef USE_GMP
	bigint_free(u);
	bigint_free(v);
	bigint_free(t1);
	bigint_free(t2);
	bigint_free(t3);
	bigint_free(const_one);
#endif
}


/******************************  Randomness ******************************/

static void brand(void *s, size_t n)
{
#ifdef RSA_TEST
	static int rand_fd = -1;

	if (rand_fd < 0) {
		rand_fd = open("/dev/urandom", O_RDONLY);
		if (rand_fd < 0)
			assert(!"Can not open /dev/urandom!");
	}

	while (n > 0) {
		int rc = read(rand_fd, s, n);
		if (rc <= 0)
			assert(!"I/O error while reding from /dev/urandom!");
		n -= rc;
		s += rc;
	}
#else
	qfl_os_brand(s, n);
#endif
}


/******************************  Primes ******************************/

#ifndef USE_GMP
static int some_small_primes[] = {
#include "primes.h"
};

static int prime_checker_smallprimes(struct bigint *p)
{
	int i, may_be_prime = 1;

	for (i=0; may_be_prime && some_small_primes[i]; i++)
	{
		if (bigint_smallmod(p, some_small_primes[i]) == 0) {
			may_be_prime = 0;
			break;
		}
	}

	return may_be_prime;
}

static int prime_checker_rabin_miller(struct bigint *p, int a_size)
{
	struct bigint *a, *m, *p_minor, *z;
	int b, j, rc = 0;

	// p_minor := p - 1;
	{
		struct bigint *t1 = bigint_fromint(1);
		p_minor = bigint_sub(p, t1);
		bigint_free(t1);
	}

	// b := max(x | (p-1) % 2^x == 0)
	b = 0;
	while (1) {
		struct bigint *t1 = bigint_fromint(b+1);
		struct bigint *t2 = bigint_fromint(2);
		struct bigint *t3 = bigint_exp(t2, t1);
		struct bigint *t4 = bigint_mod(p_minor, t3);
		int isdivider = t4->value_size == 0;
		bigint_free(t1);
		bigint_free(t2);
		bigint_free(t3);
		bigint_free(t4);
		if (!isdivider)
			break;
		b++;
	}

	// m := (p-1) / 2^b
	{
		struct bigint *t1 = bigint_fromint(b);
		struct bigint *t2 = bigint_fromint(2);
		struct bigint *t3 = bigint_exp(t2, t1);
		m = bigint_div(p_minor, t3);
		bigint_free(t1);
		bigint_free(t2);
		bigint_free(t3);
	}

	a = bigint_create(a_size);
	brand(a->value, a_size);

	j = 0;

	// z := a^m % p
	z = bigint_expmod(a, m, p);

	// z = 1  OR  z = p-1  =>  OK
	{
		struct bigint *t1 = bigint_fromint(1);
		if (bigint_eq(z, t1) || bigint_eq(z, p_minor)) {
			rc = 1;
			bigint_free(t1);
			goto finish;
		}
		bigint_free(t1);
	}

loop_start:;

	// j > 0  AND  z = 1  =>  NOT_OK
	if (j > 0) {
		struct bigint *t1 = bigint_fromint(1);
		if (bigint_eq(z, t1)) {
			bigint_free(t1);
			goto finish;
		}
		bigint_free(t1);
	}

	j++;

	// j < b  AND  z != p - 1  =>  LOOP
	if (j < b && !bigint_eq(z, p_minor)) {
		// z := z^2 % p
		{
			struct bigint *t1 = bigint_mul(z, z);
			bigint_free(z);
			z = bigint_mod(t1, p);
			bigint_free(t1);
		}
		goto loop_start;
	}

	if (bigint_eq(z, p_minor))
		rc = 1;

finish:
	bigint_free(a);
	bigint_free(m);
	bigint_free(p_minor);
	bigint_free(z);

	return rc;
}

static int prime_checker(struct bigint *p)
{
	int i;

	if (!prime_checker_smallprimes(p))
		return 0;

	for (i=0; i<5; i++) {
#ifdef RSA_TEST
		printf("x");
		fflush(stdout);
#endif
		if (!prime_checker_rabin_miller(p, 4))
			return 0;
	}

	return 1;
}
#endif

static struct bigint *prime_generator(int size)
{
	struct bigint *prime = bigint_create(size);

#ifdef RSA_TEST
	printf("GENERATING RANDOM PRIME NUMBER (%d BITS) ..", size*8);
	fflush(stdout);
#endif

#ifdef USE_GMP
	do {
		unsigned char random_stuff[size];
		brand(random_stuff, size);
		random_stuff[size-1] |= 0x80;
		random_stuff[0] |= 0x01;
		mpz_import(prime->mpz_value, size, -1, 1, -1, 0, random_stuff);
#ifdef RSA_TEST
		printf(".");
		fflush(stdout);
#endif
	} while (!mpz_probab_prime_p(prime->mpz_value, 5));
#else
	do {
		brand(prime->value, size);
		prime->value[size-1] |= 0x80;
		prime->value[0] |= 0x01;
#ifdef RSA_TEST
		printf(".");
		fflush(stdout);
#endif
	} while (!prime_checker(prime));
#endif

#ifdef RSA_TEST
	printf("\nFOUND PRIME: %s\n", bigint_hexdump(prime));
#endif
	return prime;
}


/******************************  RSA ******************************/

void rsa_encrypt(int bits, unsigned char *msg, const unsigned char *pub)
{
	assert(bits % 16 == 0);

	struct bigint *e = bigint_fromint(65537);

	struct bigint *m = bigint_create(bits/8);
#ifdef USE_GMP
	mpz_import(m->mpz_value, bits/8, -1, 1, -1, 0, msg);
#else
	memcpy(m->value, msg, bits/8);
#endif

	struct bigint *pq = bigint_create(bits/8);
#ifdef USE_GMP
	mpz_import(pq->mpz_value, bits/8, -1, 1, -1, 0, pub);
#else
	memcpy(pq->value, pub, bits/8);
#endif

#ifndef USE_GMP
	assert(!bigint_ge(m, pq));
#else
	assert(mpz_cmp(m->mpz_value, pq->mpz_value) < 0);
#endif

	struct bigint *c = bigint_expmod(m, e, pq);

#ifdef USE_GMP
	memset(msg, 0, bits/8);
	mpz_export(msg, 0, -1, 1, -1, 0, c->mpz_value);
#else
	bigint_expand(c, bits/8);
	assert(m->value_size <= bits/8);
	memcpy(msg, c->value, bits/8);
#endif

	bigint_free(e);
	bigint_free(m);
	bigint_free(pq);
	bigint_free(c);
}

void rsa_decrypt(int bits, unsigned char *msg, const unsigned char *pub, const unsigned char *sec)
{
	assert(bits % 16 == 0);

	struct bigint *c = bigint_create(bits/8);
#ifdef USE_GMP
	mpz_import(c->mpz_value, bits/8, -1, 1, -1, 0, msg);
#else
	memcpy(c->value, msg, bits/8);
#endif

	struct bigint *pq = bigint_create(bits/8);
#ifdef USE_GMP
	mpz_import(pq->mpz_value, bits/8, -1, 1, -1, 0, pub);
#else
	memcpy(pq->value, pub, bits/8);
#endif

	struct bigint *d = bigint_create(bits/8);
#ifdef USE_GMP
	mpz_import(d->mpz_value, bits/8, -1, 1, -1, 0, sec);
#else
	memcpy(d->value, sec, bits/8);
#endif

	struct bigint *m = bigint_expmod(c, d, pq);

#ifdef USE_GMP
	memset(msg, 0, bits/8);
	mpz_export(msg, 0, -1, 1, -1, 0, m->mpz_value);
#else
	bigint_expand(m, bits/8);
	assert(m->value_size <= bits/8);
	memcpy(msg, m->value, bits/8);
#endif

	bigint_free(c);
	bigint_free(pq);
	bigint_free(d);
	bigint_free(m);
}

void rsa_keygen(int bits, unsigned char *pub, unsigned char *sec)
{
	assert(bits % 16 == 0);

	struct bigint *e = bigint_fromint(65537);
	struct bigint *p, *q, *pq, *pq_minor, *d;

redo_primegen:
	p = prime_generator(bits/16);
	q = prime_generator(bits/16);
	pq = bigint_mul(p, q);

	// pq_minor := (p-1) * (q-1)
	{
		struct bigint *t1 = bigint_fromint(1);
		struct bigint *t2 = bigint_sub(p, t1);
		struct bigint *t3 = bigint_sub(q, t1);
		pq_minor = bigint_mul(t2, t3);
		bigint_free(t1);
		bigint_free(t2);
		bigint_free(t3);
	}

	// d := e^-1 % pq_minor
	//
	// a*u - b*v = gcd(u, v)
	//	u := pq_minor
	//	v := e
	//	d := u - b
	{
		struct bigint *u = pq_minor, *v = e;
		struct bigint *a, *b, *gcd;

		bigint_exteuclid(u, v, &a, &b, &gcd);

		// PARANOIA CHECK:
		// a*u - b*v = gcd
		{
			struct bigint *t1 = bigint_mul(a, u);
			struct bigint *t2 = bigint_mul(b, v);
			struct bigint *t3 = bigint_sub(t1, t2);
#ifdef USE_GMP
			assert(mpz_cmp(t3->mpz_value, gcd->mpz_value) == 0);
#else
			assert(bigint_eq(t3, gcd));
#endif
			bigint_free(t1);
			bigint_free(t2);
			bigint_free(t3);
		}

		// pq_minor % e = 0  =>  GENERATE_NEW_PRIMES (very unlikely)
#ifdef USE_GMP
		if (mpz_cmp_ui(gcd->mpz_value, 0) == 0)
#else
		if (gcd->value_size > 1 || gcd->value[0] != 1)
#endif
		{
			bigint_free(p);
			bigint_free(q);
			bigint_free(pq);
			bigint_free(pq_minor);

			bigint_free(a);
			bigint_free(b);
			bigint_free(gcd);

			goto redo_primegen;
		}

		d = bigint_sub(u, b);

		bigint_free(a);
		bigint_free(b);
		bigint_free(gcd);
	}

	// PARANOIA CHECK:
	// pq_minor % e != 0
	{
		struct bigint *t1 = bigint_mod(pq_minor, e);
#ifndef USE_GMP
		assert(t1->value_size != 0);
#else
		assert(mpz_cmp_ui(t1->mpz_value, 0) != 0);
#endif
		bigint_free(t1);
	}

#ifdef USE_GMP
	memset(pub, 0, bits/8);
	mpz_export(pub, 0, -1, 1, -1, 0, pq->mpz_value);

	memset(sec, 0, bits/8);
	mpz_export(sec, 0, -1, 1, -1, 0, d->mpz_value);
#else
	bigint_expand(pq, bits/8);
	assert(pq->value_size <= bits/8);
	memcpy(pub, pq->value, bits/8);

	bigint_expand(d, bits/8);
	assert(d->value_size <= bits/8);
	memcpy(sec, d->value, bits/8);
#endif

#ifndef RSA_TEST
	// Final paranoia check
	unsigned char m_orig[bits/8], m[bits/8];
	brand(m_orig, bits/8-1);
	m_orig[bits/8-1] = 0;
	memcpy(m, m_orig, bits/8);

	rsa_encrypt(bits, m, pub);
	rsa_decrypt(bits, m, pub, sec);
	assert(!memcmp(m, m_orig, bits/8));
#endif

	bigint_free(d);
	bigint_free(pq_minor);
	bigint_free(pq);
	bigint_free(p);
	bigint_free(q);
	bigint_free(e);
}


/******************************  Testbench ******************************/

#ifdef RSA_TEST

int ok_counter=0, run_counter=0;

static int test_rsa(int bits)
{
	int i, rc;

	printf("----------------- RSA API TEST ----------------\n");

	unsigned char pub[bits/8], sec[bits/8];
	rsa_keygen(bits, pub, sec);

	printf("RSA API: PUB KEY: ");
	for (i=0; i<bits/8; i++)
		printf("%02x", pub[i]);
	printf("\n");

	printf("RSA API: SEC KEY: ");
	for (i=0; i<bits/8; i++)
		printf("%02x", sec[i]);
	printf("\n");

	unsigned char m_orig[bits/8], m[bits/8];
	brand(m_orig, bits/8-1);
	m_orig[bits/8-1] = 0;
	memcpy(m, m_orig, bits/8);

	printf("RSA API: MESSAGE: ");
	for (i=0; i<bits/8; i++)
		printf("%02x", m[i]);
	printf("\n");

	rsa_encrypt(bits, m, pub);

	printf("RSA API: CRYPTED: ");
	for (i=0; i<bits/8; i++)
		printf("%02x", m[i]);
	printf("\n");

	rsa_decrypt(bits, m, pub, sec);

	printf("RSA API: MESSAGE: ");
	for (i=0; i<bits/8; i++)
		printf("%02x", m[i]);
	printf("\n");

	rc = memcmp(m, m_orig, bits/8) == 0;

	printf("RSA API: FINAL CHECK: %s\n", rc ? "OK" : "ERROR");

	printf("-----------------------------------------------\n");

	run_counter++;
	ok_counter += rc;
	printf("\n");

	return rc;
}

int main()
{
	printf("\n");

	test_rsa(128);
	test_rsa(256);
	test_rsa(512);
	test_rsa(1024);
	test_rsa(2048);

	printf("Executed %d test cycles: %d ok and %d with errors.\n\n", run_counter, ok_counter, run_counter-ok_counter);

	bigint_hexdump(0);
	return run_counter == ok_counter ? 0 : 1;
}

#endif