/***************************************************************************
 *     Copyright (c) 2002-2009, Broadcom Corporation
 *     All Rights Reserved
 *     Confidential Property of Broadcom Corporation
 *
 *  THIS SOFTWARE MAY ONLY BE USED SUBJECT TO AN EXECUTED SOFTWARE LICENSE
 *  AGREEMENT  BETWEEN THE USER AND BROADCOM.  YOU HAVE NO RIGHT TO USE OR
 *  EXPLOIT THIS MATERIAL EXCEPT SUBJECT TO THE TERMS OF SUCH AN AGREEMENT.
 *
 * Filename: mp2.c
 *
 ***************************************************************************/
/* 
 * Multiple-Precision Modular Arithmetic Routines
 *
 *
 * Change log: 	07/17/02 - 	Created.
 * 		07/29/02 - 	Split functions mpcopy(), mplong2mp(),
 * 		 		mpzero(), newabs() into separate files.
 *		07/30/02 - newabs & sign support removed definitely.
 */


#include "mp2.h"

/* global vars */

/* optimization: do not swap bytes in word, assume input is given
 * as little-endian 32-bit words. */ 
#undef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY

void mpadd (mp a, mp b, mp c)
{
	register u32 *aa = a;
	register u32 *bb = b;
	register u32 *cc = c;
	register u32 i = 0;
	register u32 carry=0;
	register u32 sa;
	register u32 sb;
	register u32 bdigit;

	/* ensure length(a) >= length(b) */
	if (mpcompare(aa, bb)<0)
		mpswap(aa,bb);

	sa = *aa;
	sb = *bb;

	/* Addition loop */	
	aa++;
	bb++;
	cc++;
	for (; i<sa; i++, aa++, bb++, cc++)
	{
		bdigit = (i>=sb)? 0L : *bb;

		*cc = (u32) (*aa + bdigit + carry);
		carry = 0;
		if (*cc >= RADIX) /* overflow */
		{
			*cc -= RADIX;
			carry = 1;
		}
	}

	if (carry) {
		*cc = carry;
		sa++;
	}

	/* Set length of sum c */
	*c = sa; 

	return;
}

/* Substraction */

void mpsub (mp a, mp b, mp c)
{
	register u32 *aa = a;
	register u32 *bb = b;
	register u32 *cc = c;
	register u32 sa = *aa;
	register u32 sb = *bb;
	register u32 ina;

	register u32 bdigit;
	register u32 i = 1;
	register u32 carry=0;

	u32 lastnz=1;
	u32 v, w;

	if (aa==bb) 
	{
		mpzero(cc);
		return;
	}
	
	/* Substraction loop */	
	aa++;
	bb++;
	cc++;
	for (; i<=sa; i++, aa++, bb++, cc++)
	{
		bdigit = (i>sb)? 0L : *bb;
		w = bdigit+carry;

		/* check for underflow */
		carry = 0;
		ina = *aa;
		if (ina < w)
		{
			v = (u32)(ina+(RADIX-w));
			carry = 1;
		}else
			v = (u32)(ina - w);

		if (v) lastnz=i;
		*cc = v;
	}

	/* Set length of c */
	*c = lastnz;

	return;
}


/* Comparison between a and b: return the sign of a-b */
s32 mpcompare (mp a, mp b)
{
	register u32 *aa = a;
	register u32 *bb = b;
	register u32 ina;
	register u32 inb;

	u32 sa = *aa;
	u32 sb = *bb;
	u32 isazero = iszero(aa);
	u32 isbzero = iszero(bb);

	/* Any zero? */
	if (isazero || isbzero)
		return (isazero-isbzero);

	/* different length? */
	if ( sa!= sb)
		return (sa < sb)? (-1): (1);
	
	aa += sa;
	bb += sa;
	for (; sa>0; sa--, aa--, bb--)
	{
		ina = *aa;
		inb = *bb;
		if (ina != inb) 
			return (ina > inb)? (1): (-1);
	}
	return (0);
}

void mpmul (mp x, mp y, mp w)
{
	register u32 n = (*x)-1;
	register u32 t = (*y)-1;
	register u32 i,j;
	register u32 *ww = w;
	u32 c, tt=t+1;

	u32 sw = n+t+2;

	for (i=sw; i>0; i--)
		*(ww++) = 0L;
	for (i=1; i<=tt; i++)
	{
		c = 0L;
		for (j=0; j<=n; j++)
			addmul (&c, &(w[i+j]), x[j+1], y[i]);
		w[i+j] = c;
	}

	for (i=sw; i>1 && !w[i]; i--) 
		; 
	*w = i;
	return;
} 

/* Left shift 0<= k <30 bits */
void mplshift (mp a, u32 k, mp b)
{
	register u32 sa = *a;
	register u32 *aa = a+sa;
	register u32 *bb = b+sa;
	register u32 kk = k;
	
	u32 w;

	*b = sa;
	
	/* shifting last chunk may yield an additional chunk */
	if ((w = (((u32)*aa) >> (NBITS-kk))) != 0L)
	{
		*(bb+1) = w; 
		*b = sa+1;
	}

	/* shift the remaining chunks */
	for (; sa>1; sa--, aa--,bb--)
		*bb = ( ((((u32)(*aa)) << kk) & MASK) | 
				((((u32)(*(aa-1))) >> (NBITS-kk))) );

	*bb = (((u32)*aa) << kk) & MASK; /* last iteration, when sa == 1 */
	return;
}

/* Right shift 0<= k <30 bits */
void mprshift (mp a, u32 k, mp b)
{
	register u32 *aa = a+1;
	register u32 *bb = b+1;
	register u32 kk = k;
	register u32 sa = *a;
	register u32 i=1;

	/* shift the remaining chunks */
	for (; i<sa; i++, aa++, bb++)
		*bb = ( (((u32)(*aa)) >> kk) | ((((u32)(*(aa+1))) << (NBITS-kk)) & MASK) );

	/* shifting last chunk may clear it */
	if (((*bb = (((u32)(*aa)) >> kk)) == 0L) && (sa>1))
		sa--;
	*b = sa;
	return;
}

/*	Division of mp numbers:			
 *
 *  q = x / y, 
 *  r = x % y 
 *  - Assume y!=0
 *  - always y*q + r == x, 
 *  - unless y | x, sign(r) == sign(y)
 *
 *  Result undefined if y = 0.
 */
int mpdiv (mp x1, mp y1, mp q, mp r)
{
	register u32 i;
	register u32 ii;
	register u32 tt;
	register u32 imt;

	u32 n,t,nt1,was_corrected;
	u32 sx, sy;
	u32  val, rem, leadingZeros;
	u32 qq[2] = {1L, 0L};

	u32 xx[LONGEXTSIZE];
	u32 yy[EXTSIZE];
	u32 ybnt[LONGEXTSIZE];
	u32 aux[LONGEXTSIZE];
	// used : 4*EXTSIZE+4*LONGEXTSIZE 32-bit words

	CLZ (leadingZeros, y1[*y1]);
	leadingZeros -= UNUSEDBITS;

	/* normalization */
	mplshift(x1, leadingZeros, xx);
	mplshift(y1, leadingZeros, yy);

	sx = *xx;
	sy = *yy;
	n = sx-1;
	t = sy-1;
	tt = t+1;
	if (n<t)
	{
		mpcopy(x1,r);
		mpzero(q);
		return 1;
	}
	nt1 = n-t+1;

	for (i=0; i<=nt1; i++)
		q[i] = 0L;

	mpclshift(yy,nt1-1,ybnt);

	while (mpcompare(xx,ybnt)>=0)
	{
		q[nt1]++;
		mpsub (xx, ybnt, xx);
	}

	for (i=n; i>=tt; i--)
	{
		ii = i+1;
		imt = i-t;

		if (xx[ii] == yy[tt]) 
		{
			q[imt] = RADIX -1;
		}
		else
		{
			if (!smalldiv (xx[ii], xx[i], yy[tt], &(q[imt]),&rem))
				return 0; 
		}
		if (t>0) {
		do
		{
			/* create xx bar */
			val = yy[t-1];
			yy[t-1]= (yy[tt]==0)? 1: 2;
			/* create qq */
			qq[1] = q[imt];
			mpmul (yy+t-1,qq,aux);

			yy[t-1] = val;

			/* create xxp */
			val = xx[i-2];
			xx[i-2] = ((xx[ii]==0)? ((xx[i]==0)? 1: 2): 3);

			if ( mpcompare (aux,xx+i-2) > 0 )
			{
				was_corrected = 1;
			}
			else
			{
				was_corrected = 0;
			}
			
			if (was_corrected == 1)
			{
				q[imt]--; 
			}
			
			xx[i-2] = val; /* restore xx[i-2] value */
		} 
		while (was_corrected);
		}
		
		mpclshift(yy,imt-1,aux);
		qq[1] = q[imt];

		mpmul (aux, qq, ybnt);
		if (mpcompare(ybnt,xx)>0)
		{
			mpadd (xx, aux, xx);
			q[imt]--;
		}
		mpsub (xx, ybnt, xx);
	}
	mprshift (xx, leadingZeros, xx); /* because of normalization */

	/* adjust lengths */
	for (i=nt1; i>1 && !q[i]; i--)
		;
	*q = i;

	for (i=*xx; i>1 && !xx[i]; i--)
		;
	*xx = i;

	/* copy the remainder to r */
	mpcopy (xx, r);
	return 1;
} 

/* Unpack a sequence of 30-bit chunks into 32-bit words, reversing
 * the order in the process (so it follows the ordering specified in
 * PKCS#1 v2.1 standard. In fact, this implements the IP2OS primitive).
 *
 * The resulting byte sequence is a valid octet string encoded in the
 * given mp number.
 * 
 * n is the desired 32-bit-word size of the sequence (it returns the
 * actual size).
 * If bit sizes of mp numbers are not multiples of 32 zero padding 
 * is used.
 * 
 * Assumptions:
 *  (1) n>= ceil((NBITS*size)/BITSOFLONG) 
 *  (2) size (=length in chunks of src30)  > 1
 */

void unpack (mp src30, u32 *dest32, u32 n)
{
	register u32 i;
	register u32 shift;
	register u32 *lsrc30  = src30;
	register u32 *ldest32 = dest32;
	register u32 mask,w;
	register u32 size = *lsrc30;
	register u32 dlen = n;

	ldest32 += (n-1);  
	lsrc30++;

	shift = 0;

	for (i=size; 0<i; i--, lsrc30++, ldest32--)
	{
		mask = (~0x0UL) << shift;
		w = ((*(lsrc30) &   mask) << UNUSEDBITS);
		if(i != 1) /* all except last iteration */
			w |= (*(lsrc30+1) & ~(mask  << UNUSEDBITS));
#ifdef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY
		v = ROTR32(w,shift+UNUSEDBITS);
		*ldest32 = SWAP(v);
#else
		*(ldest32) = ROTR32(w,shift+UNUSEDBITS);		
#endif

		if (shift == NBITS-UNUSEDBITS) 
		{
			lsrc30++;
			i--;
		} 
		shift = (shift+UNUSEDBITS) % 30;
		dlen--;
	}

	/* pad with zero bits if desired size is larger than actual size */
	/* dlen could wrap to ~0, check it */
	if(dlen < n)
	{
		ldest32 = dest32;
		for (i=0; i<dlen; i++)
			*(ldest32++) = 0L;
	}
	return;
}



/* Pack a sequence of 32-bit words into 30-bit chunks, reversing
 * the order in the process (so it follows the ordering specified in
 * PKCS#1 v2.1 standard. See the OS2IP primitive).
 *
 * The resulting sequence is a valid 
 * representation of the given sequence as a mp number.
 *
 * n is the size of the input in 32-bit words.
 * The implementation assumes src32[n] is a "readable" address and n>=1.
 */

void pack (u32 *src32, mp dest30, u32 n)
{
	register u32 *lsrc32  = src32;
	register u32 *ldest30 = dest30;
	register u32 i;
	register u32 shift;
	register u32 mask, u, w=0L;
	
	lsrc32 += (n-1);
	ldest30++;

	/* First piece is always rightmost */
#ifdef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY
	u = SWAP(lsrc32);
#else
	u = *lsrc32;
#endif
	w = u & MASK;	
	shift = 0;
	for (i=1; i<=n; i++, lsrc32--, ldest30++)
	{
		*ldest30 = ROTL32(w, shift);
	
		/* Compute shifting offset */
		shift = ((shift+UNUSEDBITS) % 32);

		/* Leftmost piece */
		mask  = (0xFFFFFFFFUL >> shift);		
		w = (u & ~mask); /* If shift==0 then ~mask==0 so w==0 */
		if (i<n)
		{	
			if ( shift==0 )
			{
				lsrc32++;
				n++; 
			} 
			/* Add rightmost piece */
			/* Note: If shift==30 then value in *(lsrc32-1) is not used, u is always 0 */
#ifdef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY
			u = SWAP(lsrc32-1);
#else
			u = *(lsrc32-1);
#endif
			w |= ( u & (mask >> UNUSEDBITS));
		}
	}
	/* store last leftmost piece */
	*ldest30 = ROTL32(w, shift);

	/* adjust length, just in case most significant bits are zero */
	for (i=n+1; i>1 && !(*ldest30); i--, ldest30--)
		;
	*dest30 = i;
	
	return;
}


#define PAD_F (RSA_BITS / 32 - 10)
/*	
 *	RSA Signature Verification (RSA Encryption) 
 */
int rsa_decrypt (unsigned long * in, unsigned long * pubkey, unsigned long * decrypted_digest)
{
	u32 i;
	u32 pad[] = { 0x00302130L, 0x0906052BL, 0x0E03021AL, 0x05000414L };
	u32 *lout;
		
	lout = decrypted_digest;

	i = rsa_decrypt_raw(in, pubkey, lout);
	if(OK != i)
		return i;
	/* Check Padding */
	if  (*lout != 0x0001FFFFL) 
	{
		return 0;
	}
	for (i=0, lout++; (i<PAD_F); i++, lout++)
	{
		if (*lout != 0xFFFFFFFFL)  
		{
			return 0;
		}
	}
	for (i=0; (i<4); i++, lout++)
	{
		if (*lout != pad[i]) 
		{
			return 0;
		}
	}

	return OK;
}

/*	
 *	RAW RSA Encryption without padding check;
 */

int rsa_decrypt_raw (unsigned long * in, unsigned long * pubkey, unsigned long * out)
{
	u32 i;
	u32 tmp[LONGEXTSIZE];
	u32 input[EXTSIZE];
	u32 output[EXTSIZE];
	u32 modulus[EXTSIZE];
	u32 *lout;
		
	lout = out;
	
	/* Packing modulus */
	pack ((u32 *)pubkey, modulus, MPSIZE/BITSOFLONG);
	
	/* Packing the input */
	pack ((u32 *)in, input, MPSIZE/BITSOFLONG);

	mpcopy (input, output);
	for (i=0; i<17; i++)
	{
		mpmul(output, (i==16)? input: output, tmp);
		if (!mpdiv (tmp, modulus, tmp, output))
		{	
			return 0;
		}
	}

	/* Unpack RSA result */
	unpack (output, lout, MPSIZE/BITSOFLONG);

	return OK;
}