source: svn/trunk/newcon3bcm2_21bu/dta/src/bootloader/rsa/mp2.c

/***************************************************************************
 *     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;
}