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

Last change on this file since 2 was 2, checked in by jglee, 11 years ago

first commit
  • Property svn:executable set to *
File size: 10.5 KB
Line 
1/***************************************************************************
2 *     Copyright (c) 2002-2009, Broadcom Corporation
3 *     All Rights Reserved
4 *     Confidential Property of Broadcom Corporation
5 *
6 *  THIS SOFTWARE MAY ONLY BE USED SUBJECT TO AN EXECUTED SOFTWARE LICENSE
7 *  AGREEMENT  BETWEEN THE USER AND BROADCOM.  YOU HAVE NO RIGHT TO USE OR
8 *  EXPLOIT THIS MATERIAL EXCEPT SUBJECT TO THE TERMS OF SUCH AN AGREEMENT.
9 *
10 * Filename: mp2.c
11 *
12 ***************************************************************************/
13/*
14 * Multiple-Precision Modular Arithmetic Routines
15 *
16 *
17 * Change log:  07/17/02 -      Created.
18 *              07/29/02 -      Split functions mpcopy(), mplong2mp(),
19 *                              mpzero(), newabs() into separate files.
20 *              07/30/02 - newabs & sign support removed definitely.
21 */
22
23
24#include "mp2.h"
25
26/* global vars */
27
28/* optimization: do not swap bytes in word, assume input is given
29 * as little-endian 32-bit words. */ 
30#undef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY
31
32void mpadd (mp a, mp b, mp c)
33{
34        register u32 *aa = a;
35        register u32 *bb = b;
36        register u32 *cc = c;
37        register u32 i = 0;
38        register u32 carry=0;
39        register u32 sa;
40        register u32 sb;
41        register u32 bdigit;
42
43        /* ensure length(a) >= length(b) */
44        if (mpcompare(aa, bb)<0)
45                mpswap(aa,bb);
46
47        sa = *aa;
48        sb = *bb;
49
50        /* Addition loop */     
51        aa++;
52        bb++;
53        cc++;
54        for (; i<sa; i++, aa++, bb++, cc++)
55        {
56                bdigit = (i>=sb)? 0L : *bb;
57
58                *cc = (u32) (*aa + bdigit + carry);
59                carry = 0;
60                if (*cc >= RADIX) /* overflow */
61                {
62                        *cc -= RADIX;
63                        carry = 1;
64                }
65        }
66
67        if (carry) {
68                *cc = carry;
69                sa++;
70        }
71
72        /* Set length of sum c */
73        *c = sa; 
74
75        return;
76}
77
78/* Substraction */
79
80void mpsub (mp a, mp b, mp c)
81{
82        register u32 *aa = a;
83        register u32 *bb = b;
84        register u32 *cc = c;
85        register u32 sa = *aa;
86        register u32 sb = *bb;
87        register u32 ina;
88
89        register u32 bdigit;
90        register u32 i = 1;
91        register u32 carry=0;
92
93        u32 lastnz=1;
94        u32 v, w;
95
96        if (aa==bb) 
97        {
98                mpzero(cc);
99                return;
100        }
101       
102        /* Substraction loop */ 
103        aa++;
104        bb++;
105        cc++;
106        for (; i<=sa; i++, aa++, bb++, cc++)
107        {
108                bdigit = (i>sb)? 0L : *bb;
109                w = bdigit+carry;
110
111                /* check for underflow */
112                carry = 0;
113                ina = *aa;
114                if (ina < w)
115                {
116                        v = (u32)(ina+(RADIX-w));
117                        carry = 1;
118                }else
119                        v = (u32)(ina - w);
120
121                if (v) lastnz=i;
122                *cc = v;
123        }
124
125        /* Set length of c */
126        *c = lastnz;
127
128        return;
129}
130
131
132/* Comparison between a and b: return the sign of a-b */
133s32 mpcompare (mp a, mp b)
134{
135        register u32 *aa = a;
136        register u32 *bb = b;
137        register u32 ina;
138        register u32 inb;
139
140        u32 sa = *aa;
141        u32 sb = *bb;
142        u32 isazero = iszero(aa);
143        u32 isbzero = iszero(bb);
144
145        /* Any zero? */
146        if (isazero || isbzero)
147                return (isazero-isbzero);
148
149        /* different length? */
150        if ( sa!= sb)
151                return (sa < sb)? (-1): (1);
152       
153        aa += sa;
154        bb += sa;
155        for (; sa>0; sa--, aa--, bb--)
156        {
157                ina = *aa;
158                inb = *bb;
159                if (ina != inb) 
160                        return (ina > inb)? (1): (-1);
161        }
162        return (0);
163}
164
165void mpmul (mp x, mp y, mp w)
166{
167        register u32 n = (*x)-1;
168        register u32 t = (*y)-1;
169        register u32 i,j;
170        register u32 *ww = w;
171        u32 c, tt=t+1;
172
173        u32 sw = n+t+2;
174
175        for (i=sw; i>0; i--)
176                *(ww++) = 0L;
177        for (i=1; i<=tt; i++)
178        {
179                c = 0L;
180                for (j=0; j<=n; j++)
181                        addmul (&c, &(w[i+j]), x[j+1], y[i]);
182                w[i+j] = c;
183        }
184
185        for (i=sw; i>1 && !w[i]; i--) 
186                ; 
187        *w = i;
188        return;
189} 
190
191/* Left shift 0<= k <30 bits */
192void mplshift (mp a, u32 k, mp b)
193{
194        register u32 sa = *a;
195        register u32 *aa = a+sa;
196        register u32 *bb = b+sa;
197        register u32 kk = k;
198       
199        u32 w;
200
201        *b = sa;
202       
203        /* shifting last chunk may yield an additional chunk */
204        if ((w = (((u32)*aa) >> (NBITS-kk))) != 0L)
205        {
206                *(bb+1) = w; 
207                *b = sa+1;
208        }
209
210        /* shift the remaining chunks */
211        for (; sa>1; sa--, aa--,bb--)
212                *bb = ( ((((u32)(*aa)) << kk) & MASK) | 
213                                ((((u32)(*(aa-1))) >> (NBITS-kk))) );
214
215        *bb = (((u32)*aa) << kk) & MASK; /* last iteration, when sa == 1 */
216        return;
217}
218
219/* Right shift 0<= k <30 bits */
220void mprshift (mp a, u32 k, mp b)
221{
222        register u32 *aa = a+1;
223        register u32 *bb = b+1;
224        register u32 kk = k;
225        register u32 sa = *a;
226        register u32 i=1;
227
228        /* shift the remaining chunks */
229        for (; i<sa; i++, aa++, bb++)
230                *bb = ( (((u32)(*aa)) >> kk) | ((((u32)(*(aa+1))) << (NBITS-kk)) & MASK) );
231
232        /* shifting last chunk may clear it */
233        if (((*bb = (((u32)(*aa)) >> kk)) == 0L) && (sa>1))
234                sa--;
235        *b = sa;
236        return;
237}
238
239/*      Division of mp numbers:                 
240 *
241 *  q = x / y,
242 *  r = x % y
243 *  - Assume y!=0
244 *  - always y*q + r == x,
245 *  - unless y | x, sign(r) == sign(y)
246 *
247 *  Result undefined if y = 0.
248 */
249int mpdiv (mp x1, mp y1, mp q, mp r)
250{
251        register u32 i;
252        register u32 ii;
253        register u32 tt;
254        register u32 imt;
255
256        u32 n,t,nt1,was_corrected;
257        u32 sx, sy;
258        u32  val, rem, leadingZeros;
259        u32 qq[2] = {1L, 0L};
260
261        u32 xx[LONGEXTSIZE];
262        u32 yy[EXTSIZE];
263        u32 ybnt[LONGEXTSIZE];
264        u32 aux[LONGEXTSIZE];
265        // used : 4*EXTSIZE+4*LONGEXTSIZE 32-bit words
266
267        CLZ (leadingZeros, y1[*y1]);
268        leadingZeros -= UNUSEDBITS;
269
270        /* normalization */
271        mplshift(x1, leadingZeros, xx);
272        mplshift(y1, leadingZeros, yy);
273
274        sx = *xx;
275        sy = *yy;
276        n = sx-1;
277        t = sy-1;
278        tt = t+1;
279        if (n<t)
280        {
281                mpcopy(x1,r);
282                mpzero(q);
283                return 1;
284        }
285        nt1 = n-t+1;
286
287        for (i=0; i<=nt1; i++)
288                q[i] = 0L;
289
290        mpclshift(yy,nt1-1,ybnt);
291
292        while (mpcompare(xx,ybnt)>=0)
293        {
294                q[nt1]++;
295                mpsub (xx, ybnt, xx);
296        }
297
298        for (i=n; i>=tt; i--)
299        {
300                ii = i+1;
301                imt = i-t;
302
303                if (xx[ii] == yy[tt]) 
304                {
305                        q[imt] = RADIX -1;
306                }
307                else
308                {
309                        if (!smalldiv (xx[ii], xx[i], yy[tt], &(q[imt]),&rem))
310                                return 0; 
311                }
312                if (t>0) {
313                do
314                {
315                        /* create xx bar */
316                        val = yy[t-1];
317                        yy[t-1]= (yy[tt]==0)? 1: 2;
318                        /* create qq */
319                        qq[1] = q[imt];
320                        mpmul (yy+t-1,qq,aux);
321
322                        yy[t-1] = val;
323
324                        /* create xxp */
325                        val = xx[i-2];
326                        xx[i-2] = ((xx[ii]==0)? ((xx[i]==0)? 1: 2): 3);
327
328                        if ( mpcompare (aux,xx+i-2) > 0 )
329                        {
330                                was_corrected = 1;
331                        }
332                        else
333                        {
334                                was_corrected = 0;
335                        }
336                       
337                        if (was_corrected == 1)
338                        {
339                                q[imt]--; 
340                        }
341                       
342                        xx[i-2] = val; /* restore xx[i-2] value */
343                } 
344                while (was_corrected);
345                }
346               
347                mpclshift(yy,imt-1,aux);
348                qq[1] = q[imt];
349
350                mpmul (aux, qq, ybnt);
351                if (mpcompare(ybnt,xx)>0)
352                {
353                        mpadd (xx, aux, xx);
354                        q[imt]--;
355                }
356                mpsub (xx, ybnt, xx);
357        }
358        mprshift (xx, leadingZeros, xx); /* because of normalization */
359
360        /* adjust lengths */
361        for (i=nt1; i>1 && !q[i]; i--)
362                ;
363        *q = i;
364
365        for (i=*xx; i>1 && !xx[i]; i--)
366                ;
367        *xx = i;
368
369        /* copy the remainder to r */
370        mpcopy (xx, r);
371        return 1;
372} 
373
374/* Unpack a sequence of 30-bit chunks into 32-bit words, reversing
375 * the order in the process (so it follows the ordering specified in
376 * PKCS#1 v2.1 standard. In fact, this implements the IP2OS primitive).
377 *
378 * The resulting byte sequence is a valid octet string encoded in the
379 * given mp number.
380 *
381 * n is the desired 32-bit-word size of the sequence (it returns the
382 * actual size).
383 * If bit sizes of mp numbers are not multiples of 32 zero padding
384 * is used.
385 *
386 * Assumptions:
387 *  (1) n>= ceil((NBITS*size)/BITSOFLONG)
388 *  (2) size (=length in chunks of src30)  > 1
389 */
390
391void unpack (mp src30, u32 *dest32, u32 n)
392{
393        register u32 i;
394        register u32 shift;
395        register u32 *lsrc30  = src30;
396        register u32 *ldest32 = dest32;
397        register u32 mask,w;
398        register u32 size = *lsrc30;
399        register u32 dlen = n;
400
401        ldest32 += (n-1); 
402        lsrc30++;
403
404        shift = 0;
405
406        for (i=size; 0<i; i--, lsrc30++, ldest32--)
407        {
408                mask = (~0x0UL) << shift;
409                w = ((*(lsrc30) &   mask) << UNUSEDBITS);
410                if(i != 1) /* all except last iteration */
411                        w |= (*(lsrc30+1) & ~(mask  << UNUSEDBITS));
412#ifdef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY
413                v = ROTR32(w,shift+UNUSEDBITS);
414                *ldest32 = SWAP(v);
415#else
416                *(ldest32) = ROTR32(w,shift+UNUSEDBITS);               
417#endif
418
419                if (shift == NBITS-UNUSEDBITS) 
420                {
421                        lsrc30++;
422                        i--;
423                } 
424                shift = (shift+UNUSEDBITS) % 30;
425                dlen--;
426        }
427
428        /* pad with zero bits if desired size is larger than actual size */
429        /* dlen could wrap to ~0, check it */
430        if(dlen < n)
431        {
432                ldest32 = dest32;
433                for (i=0; i<dlen; i++)
434                        *(ldest32++) = 0L;
435        }
436        return;
437}
438
439
440
441/* Pack a sequence of 32-bit words into 30-bit chunks, reversing
442 * the order in the process (so it follows the ordering specified in
443 * PKCS#1 v2.1 standard. See the OS2IP primitive).
444 *
445 * The resulting sequence is a valid
446 * representation of the given sequence as a mp number.
447 *
448 * n is the size of the input in 32-bit words.
449 * The implementation assumes src32[n] is a "readable" address and n>=1.
450 */
451
452void pack (u32 *src32, mp dest30, u32 n)
453{
454        register u32 *lsrc32  = src32;
455        register u32 *ldest30 = dest30;
456        register u32 i;
457        register u32 shift;
458        register u32 mask, u, w=0L;
459       
460        lsrc32 += (n-1);
461        ldest30++;
462
463        /* First piece is always rightmost */
464#ifdef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY
465        u = SWAP(lsrc32);
466#else
467        u = *lsrc32;
468#endif
469        w = u & MASK;   
470        shift = 0;
471        for (i=1; i<=n; i++, lsrc32--, ldest30++)
472        {
473                *ldest30 = ROTL32(w, shift);
474       
475                /* Compute shifting offset */
476                shift = ((shift+UNUSEDBITS) % 32);
477
478                /* Leftmost piece */
479                mask  = (0xFFFFFFFFUL >> shift);               
480                w = (u & ~mask); /* If shift==0 then ~mask==0 so w==0 */
481                if (i<n)
482                {       
483                        if ( shift==0 )
484                        {
485                                lsrc32++;
486                                n++; 
487                        } 
488                        /* Add rightmost piece */
489                        /* Note: If shift==30 then value in *(lsrc32-1) is not used, u is always 0 */
490#ifdef INPUT_IS_NOT_LITTLE_ENDIAN_WORD32_ARRAY
491                        u = SWAP(lsrc32-1);
492#else
493                        u = *(lsrc32-1);
494#endif
495                        w |= ( u & (mask >> UNUSEDBITS));
496                }
497        }
498        /* store last leftmost piece */
499        *ldest30 = ROTL32(w, shift);
500
501        /* adjust length, just in case most significant bits are zero */
502        for (i=n+1; i>1 && !(*ldest30); i--, ldest30--)
503                ;
504        *dest30 = i;
505       
506        return;
507}
508
509
510#define PAD_F (RSA_BITS / 32 - 10)
511/*     
512 *      RSA Signature Verification (RSA Encryption)
513 */
514int rsa_decrypt (unsigned long * in, unsigned long * pubkey, unsigned long * decrypted_digest)
515{
516        u32 i;
517        u32 pad[] = { 0x00302130L, 0x0906052BL, 0x0E03021AL, 0x05000414L };
518        u32 *lout;
519               
520        lout = decrypted_digest;
521
522        i = rsa_decrypt_raw(in, pubkey, lout);
523        if(OK != i)
524                return i;
525        /* Check Padding */
526        if  (*lout != 0x0001FFFFL) 
527        {
528                return 0;
529        }
530        for (i=0, lout++; (i<PAD_F); i++, lout++)
531        {
532                if (*lout != 0xFFFFFFFFL) 
533                {
534                        return 0;
535                }
536        }
537        for (i=0; (i<4); i++, lout++)
538        {
539                if (*lout != pad[i]) 
540                {
541                        return 0;
542                }
543        }
544
545        return OK;
546}
547
548/*     
549 *      RAW RSA Encryption without padding check;
550 */
551
552int rsa_decrypt_raw (unsigned long * in, unsigned long * pubkey, unsigned long * out)
553{
554        u32 i;
555        u32 tmp[LONGEXTSIZE];
556        u32 input[EXTSIZE];
557        u32 output[EXTSIZE];
558        u32 modulus[EXTSIZE];
559        u32 *lout;
560               
561        lout = out;
562       
563        /* Packing modulus */
564        pack ((u32 *)pubkey, modulus, MPSIZE/BITSOFLONG);
565       
566        /* Packing the input */
567        pack ((u32 *)in, input, MPSIZE/BITSOFLONG);
568
569        mpcopy (input, output);
570        for (i=0; i<17; i++)
571        {
572                mpmul(output, (i==16)? input: output, tmp);
573                if (!mpdiv (tmp, modulus, tmp, output))
574                {       
575                        return 0;
576                }
577        }
578
579        /* Unpack RSA result */
580        unpack (output, lout, MPSIZE/BITSOFLONG);
581
582        return OK;
583}
Note: See TracBrowser for help on using the repository browser.