/*
 * Copyright 2010 Trident Microsystems (Far East) Ltd. All rights reserved.
 * All prior copyright rights in this work and the accompanying software
 * products transferred to Trident Microsystems (Far East) Ltd. by written
 * agreement.
 */
/*****************************************************************************/
/* Copyright (c) 2010 NXP B.V.                                               */
/*                                                                           */
/* This program is free software; you can redistribute it and/or modify      */
/* it under the terms of the GNU General Public License as published by      */
/* the Free Software Foundation, using version 2 of the License.             */
/*                                                                           */
/* This program is distributed in the hope that it will be useful,           */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of            */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the              */
/* GNU General Public License for more details.                              */
/*                                                                           */
/* You should have received a copy of the GNU General Public License         */
/* along with this program; if not, write to the Free Software               */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307       */
/* USA.                                                                      */
/*                                                                           */
/*****************************************************************************/
/*

Rev Date        Author      Comments
--------------------------------------------------------------------------------
  1 20040917    amccurdy    Original created
  3 20080215    batelaan    Update file copyright/license text
  4 20080321    amccurdy    Resync with svn, minor fix to abn_print, etc
  5 20090225    amccurdy    Resync with svn, additional abn_print functions
--------------------------------------------------------------------------------
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <openssl/bn.h>

#include "aimage.h"
#include "abn.h"


void abn_init (unsigned int *bn, unsigned int initval, int words)
{
	*bn++ = initval;			/* lsword = initval */
	memset (bn, 0, ((words - 1) * 4));	/* remaining words = 0 */
}

void abn_init_64 (unsigned int *bn, unsigned long long initval, int words)
{
	*bn++ = (initval >>  0);		/* lsword = lsword of initval */
	*bn++ = (initval >> 32);
	memset (bn, 0, ((words - 2) * 4));	/* remaining words = 0 */
}

int abn_init_from_string_hex (unsigned int *bn, char *hexstring, int words)
{
	char *hs = hexstring;
	unsigned int ch;
	unsigned int digit;
	unsigned int digit_count = 0;
	unsigned int digit_count_limit = words * ABN_HEX_DIGITS_PER_WORD;
	unsigned int could_be_leading_zero = 1;

	if ((hs[0] == '0') && (hs[1] == 'x'))
		hs += 2;

	abn_init (bn, 0, words);

	while (1) {
		ch = *hs++;
		if (ch == 0)
			break;
		else if ((ch == ' ') || (ch == ',') || (ch == '\r') || (ch == '\n'))
			continue;
		else if ((ch == '0') && could_be_leading_zero)
			continue;
		else if ((ch >= '0') && (ch <= '9'))
			digit = ch - '0';
		else if ((ch >= 'a') && (ch <= 'f'))
			digit = ch - 'a' + 10;
		else if ((ch >= 'A') && (ch <= 'F'))
			digit = ch - 'A' + 10;
		else {
			fprintf (stderr, "Error: unable to parse hexstring '%s'\n", hexstring);
			exit (1);
		}

		could_be_leading_zero = 0;

		if (++digit_count > digit_count_limit) {
			fprintf (stderr, "Error: hexstring contains more than %d hex digits: '%s'\n",
				 digit_count_limit, hexstring);
			exit (1);
		}

		abn_shift_left (bn, 4, digit, words);
	}

//	abn_print_nlz (NULL, bn, "\n", words);

	return 0;
}

unsigned int abn_extract_lsbyte (unsigned int *bn, int words)
{
	unsigned int lsword = bn[0];

	return lsword & 0xFF;
}

unsigned int abn_extract_msbyte (unsigned int *bn, int words)
{
	unsigned int msword = bn[words - 1];

	return (msword >> 24);
}

/*
   Fixme: unify the various abn_print functions...
*/

void abn_print (char *prefix, unsigned int *bn, char *suffix, int words)
{
	int i, j;
	int count = -8;

	if (! prefix)
		prefix = "";

	printf ("%s", prefix);

	printf ("0x");
	for (i = (words - 1); i >= 0; i--) {
		printf ("%08x ", bn[i]);
		if ((++count % 8) == 0) {
			printf ("\n");
			for (j = 0; j < strlen (prefix) + 2; j++)
				printf (" ");
		}
	}

	if (suffix)
		printf ("%s", suffix);
}

void abn_print_nlz (char *prefix, unsigned int *bn, char *suffix, int words)
{
	int i, j;
	int count = -8;
	unsigned int could_be_leading_zero = 1;

	if (! prefix)
		prefix = "";

	printf ("%s", prefix);

	printf ("0x");
	for (i = (words - 1); i >= 0; i--) {
		if ((i > 0) && (bn[i] == 0) && could_be_leading_zero)
			continue;
		printf ("%08x ", bn[i]);
		if ((++count % 8) == 0) {
			printf ("\n");
			for (j = 0; j < strlen (prefix) + 2; j++)
				printf (" ");
		}
	}

	if ((count % 8) != 0)
		printf ("\n");

	if (suffix)
		printf ("%s", suffix);
}

void abn_print_nlz_nsp (char *prefix, unsigned int *bn, char *suffix, int words)
{
	int i;
	unsigned int could_be_leading_zero = 1;

	if (! prefix)
		prefix = "";

	printf ("%s", prefix);

	printf ("0x");
	for (i = (words - 1); i >= 0; i--) {
		if ((i > 0) && (bn[i] == 0) && could_be_leading_zero)
			continue;
		printf ("%08x", bn[i]);
	}

	printf ("\n");

	if (suffix)
		printf ("%s", suffix);
}

void abn_shift_left (unsigned int *bn, unsigned int bits, unsigned int carry, int words)
{
	int i;
	unsigned int temp;
	unsigned int shift;

	while (bits) {
		shift = (bits > 31) ? 31 : bits;
		bits -= shift;
		for (i = 0; i < words; i++) {
			temp = bn[i];
			bn[i] = (temp << shift) | carry;
			carry = (temp >> (32 - shift));
		}
		carry = 0;
	}
}

void abn_shift_right (unsigned int *bn, unsigned int bits, int words)
{
	int i;
	unsigned int temp;
	unsigned int shift;
	unsigned int carry = 0;

	while (bits) {
		shift = (bits > 31) ? 31 : bits;
		bits -= shift;
		for (i = words - 1; i >= 0; i--) {
			temp = bn[i];
			bn[i] = (temp >> shift) | carry;
			carry = (temp << (32 - shift));
		}
		carry = 0;
	}
}

void abn_shift_left_single (unsigned int *bn, unsigned int carry, int words)
{
	unsigned int temp;

	do {
		temp = *bn;
		*bn++ = (temp << 1) | carry;
		carry = temp >> 31;
	}
	while (--words != 0);
}

void abn_shift_right_single (unsigned int *bn, int words)
{
	int i;
	unsigned int temp;
	unsigned int carry = 0;

	for (i = words - 1; i >= 0; i--) {
		temp = bn[i];
		bn[i] = (temp >> 1) | (carry << 31);
		carry = temp;
	}
}

int abn_cmp (unsigned int *bn1, unsigned int *bn2, int words)
{
	int i;

	for (i = words - 1; i >= 0; i--) {
		if (bn1[i] > bn2[i]) return  1;
		if (bn1[i] < bn2[i]) return -1;
	}

	return 0;
}

int abn_add (unsigned int *bn, unsigned int *q, int words)
{
	unsigned int t1, t2, c1, c2 = 0;

	do {
		t1 = *bn;
		t2 = t1 + *q++;
		c1 = (t2 < t1) ? 1 : 0;
		t1 = t2 + c2;
		c2 = (t1 < t2) ? 1 : c1;
		*bn++ = t1;
	}
	while (--words != 0);

	return c2;
}

int abn_sub (unsigned int *bn, unsigned int *q, int words)
{
	unsigned int t1, t2, c1, c2 = 0;

	do {
		t1 = *bn;
		t2 = t1 - *q++;
		c1 = (t2 > t1) ? 1 : 0;
		t1 = t2 - c2;
		c2 = (t1 > t2) ? 1 : c1;
		*bn++ = t1;
	}
	while (--words != 0);

	return c2;
}

BIGNUM *abn_to_opensslbn (unsigned int *bn, int words)
{
	int i, j;
	unsigned int temp;
	BIGNUM *result;
	unsigned char *bin = malloc (words * 4);

	if (bin == NULL) {
		printf ("Fatal: abn_to_opensslbin malloc failed...\n");
		exit (1);
	}

	for (j = 0, i = words - 1; i >= 0; i--) {
		temp = bn[i];
		bin[j++] = (temp >> 24);
		bin[j++] = (temp >> 16);
		bin[j++] = (temp >>  8);
		bin[j++] = (temp >>  0);
	}

	result = BN_bin2bn ((const unsigned char *) bin, (words * 4), NULL);

	if (result == NULL) {
		printf ("Fatal: abn_to_opensslbin BN_bin2BN failed...\n");
		exit (1);
	}

	free (bin);

	return result;
}

void opensslbn_to_abn (unsigned int *bn, BIGNUM *openssl_bn, int words)
{
	int i;
	int bytes = BN_num_bytes (openssl_bn);
	unsigned char *bin;

	if (bytes > (words * 4)) {
		printf ("Fatal: opensslbin_to_abn input exceeds range of output...\n");
		exit (1);
	}

	if ((bin = malloc (bytes)) == NULL) {
		printf ("Fatal: opensslbin_to_abn malloc failed...\n");
		exit (1);
	}

	BN_bn2bin (openssl_bn, bin);

	abn_init (bn, 0, words);
	for (i = 0; i < bytes; i++)
		abn_shift_left (bn, 8, bin[i], words);

	free (bin);
}

unsigned int opensslbn_inverse_mod_two_tothepower_thirtytwo (BIGNUM *openssl_bn)
{
	BIGNUM *r = BN_new();
	BIGNUM *a = BN_new();
	BN_CTX *ctx = BN_CTX_new();
	unsigned int result;

	BN_one (a);
	BN_lshift (a, a, 32);
	BN_mod_inverse (r, openssl_bn, a, ctx); 	/* r = (bn^-1) mod 2^32 */
	result = BN_get_word (r);

	BN_free (r);
	BN_free (a);
	BN_CTX_free (ctx);

	return result;
}

void abn_mont_mul (unsigned int *z, unsigned int *x, unsigned int *y, const aimage_keypub_t *key)
{
	int i, j;
	int words = RSA_MOD_WORDS_1024;
	unsigned int temp, u, slice_x, cb, cw1, cw2;
	unsigned int mod_dash = key->mod_data[0];
	unsigned int *modulus = (unsigned int *) &key->mod_data[1];
	unsigned long long product;

	for (i = 0; i < (words + 1); i++)
		z[i] = 0;

	for (i = 0; i < words; i++) {
	   slice_x = *x++;
	   product = (((unsigned long long) *y++) * slice_x) + *z;
	   cw1 = (product >> 32);
	   temp = (product >> 0);
	   u = (temp * mod_dash);
	   product = (((unsigned long long) modulus[0]) * u) + temp;
	   cw2 = (product >> 32);
	   for (j = 1; j < words; j++) {
		   product = (((unsigned long long) *y++) * slice_x) + z[1] + cw1;
		   cw1 = (product >> 32);
		   temp = (product >> 0);
		   product = (((unsigned long long) modulus[j]) * u) + temp + cw2;
		   cw2 = (product >> 32);
		   temp = (product >> 0);
		   *z++ = temp;
	   }
	   temp = cw1 + z[1];
	   cb = (temp < cw1) ? 1 : 0;
	   temp += cw2;
	   if (temp < cw2)
		   cb = 1;
	   *z++ = temp;
	   *z = cb;
	   z -= words;
	   y -= words;
	}

	if (abn_cmp (z, modulus, (words + 1)) >= 0)
		abn_sub (z, modulus, (words + 1));
}

int abn_mont_exp (unsigned int *res, unsigned int *tmp, unsigned int *input, const aimage_keypub_t *key)
{
	int i;
	int words = RSA_MOD_WORDS_1024;
	unsigned int exp;

#if defined (AIMAGE_HARDCODED_EXP)
	exp = AIMAGE_HARDCODED_EXP;
#else
	exp = aimage_pubkey_exp (key);
#endif

	if (exp == 3) {
		abn_mont_mul (tmp, input, input, key);		/* tmp = input ^ 2 */
		abn_mont_mul (res, tmp, input, key);		/* res = input ^ 3 */
		return 0;
	}

	if (exp == 17) {
		abn_mont_mul (res, input, input, key);		/* res = input ^ 2 */
		abn_mont_mul (tmp, res, res, key);		/* tmp = input ^ 4 */
		abn_mont_mul (res, tmp, tmp, key);		/* res = input ^ 8 */
		abn_mont_mul (tmp, res, res, key);		/* tmp = input ^ 16 */
		abn_mont_mul (res, tmp, input, key);		/* res = input ^ 17 */
		return 0;
	}

	if (exp == 65537) {
		abn_mont_mul (res, input, input, key);		/* res = input ^ 2 */
		for (i = 0; i < 7; i++) {
			abn_mont_mul (tmp, res, res, key);	/* tmp = res ^ 2 */
			abn_mont_mul (res, tmp, tmp, key);	/* res = tmp ^ 2 */
		}
		abn_mont_mul (tmp, res, res, key);		/* tmp = input ^ 65536 */
		abn_mont_mul (res, tmp, input, key);		/* res = input ^ 65537 */
		return 0;
	}

	for (i = 0; i < (words + 1); i++)
		res[i] = 0;

	return -1;
}

