1/******************************************************************************
2 *
3 *  Copyright (C) 2006-2015 Broadcom Corporation
4 *
5 *  Licensed under the Apache License, Version 2.0 (the "License");
6 *  you may not use this file except in compliance with the License.
7 *  You may obtain a copy of the License at:
8 *
9 *  http://www.apache.org/licenses/LICENSE-2.0
10 *
11 *  Unless required by applicable law or agreed to in writing, software
12 *  distributed under the License is distributed on an "AS IS" BASIS,
13 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 *  See the License for the specific language governing permissions and
15 *  limitations under the License.
16 *
17 ******************************************************************************/
18
19 /******************************************************************************
20  *
21  *  This file contains simple pairing algorithms
22  *
23  ******************************************************************************/
24
25#include <string.h>
26#include "bt_target.h"
27#include "p_256_ecc_pp.h"
28#include "p_256_multprecision.h"
29
30void multiprecision_init(DWORD *c, uint32_t keyLength)
31{
32    for (uint32_t i = 0; i < keyLength; i++)
33        c[i] = 0;
34}
35
36void multiprecision_copy(DWORD *c, DWORD *a, uint32_t keyLength)
37{
38    for (uint32_t i = 0; i < keyLength; i++)
39        c[i] = a[i];
40}
41
42int multiprecision_compare(DWORD *a, DWORD *b, uint32_t keyLength)
43{
44    for (int i = keyLength - 1; i >= 0; i--)
45    {
46        if (a[i] > b[i])
47            return 1;
48        if (a[i] < b[i])
49            return -1;
50    }
51    return 0;
52}
53
54int multiprecision_iszero(DWORD *a, uint32_t keyLength)
55{
56    for (uint32_t i = 0; i < keyLength; i++)
57        if (a[i])
58            return 0;
59
60    return 1;
61}
62
63UINT32 multiprecision_dword_bits(DWORD a)
64{
65    uint32_t i;
66    for (i = 0; i < DWORD_BITS; i++, a >>= 1)
67        if (a == 0)
68            break;
69
70    return i;
71}
72
73UINT32 multiprecision_most_signdwords(DWORD *a, uint32_t keyLength)
74{
75    int  i;
76    for (i = keyLength - 1; i >= 0; i--)
77        if (a[i])
78           break;
79    return (i + 1);
80}
81
82UINT32 multiprecision_most_signbits(DWORD *a, uint32_t keyLength)
83{
84    int aMostSignDWORDs;
85
86    aMostSignDWORDs = multiprecision_most_signdwords(a, keyLength);
87    if (aMostSignDWORDs == 0)
88        return 0;
89
90    return (((aMostSignDWORDs-1) << DWORD_BITS_SHIFT) +
91            multiprecision_dword_bits(a[aMostSignDWORDs-1]) );
92}
93
94DWORD multiprecision_add(DWORD *c, DWORD *a, DWORD *b, uint32_t keyLength)
95{
96    DWORD carrier;
97    DWORD temp;
98
99    carrier=0;
100    for (uint32_t i = 0; i < keyLength; i++)
101    {
102        temp = a[i] + carrier;
103        carrier = (temp < carrier);
104        temp += b[i];
105        carrier |= (temp < b[i]);
106        c[i]=temp;
107    }
108
109    return carrier;
110}
111
112//c=a-b
113DWORD multiprecision_sub(DWORD *c, DWORD *a, DWORD *b, uint32_t keyLength)
114{
115    DWORD borrow;
116    DWORD temp;
117
118    borrow=0;
119    for (uint32_t i=0; i < keyLength; i++)
120    {
121        temp = a[i] - borrow;
122        borrow = (temp > a[i]);
123        c[i] = temp - b[i];
124        borrow |= (c[i] > temp);
125    }
126
127    return borrow;
128}
129
130// c = a << 1
131void multiprecision_lshift_mod(DWORD * c, DWORD * a, uint32_t keyLength)
132{
133    DWORD carrier;
134    DWORD *modp;
135
136    if (keyLength == KEY_LENGTH_DWORDS_P192)
137    {
138        modp = curve.p;
139    }
140    else if (keyLength == KEY_LENGTH_DWORDS_P256)
141    {
142        modp = curve_p256.p;
143    }
144    else
145        return;
146
147    carrier = multiprecision_lshift(c, a, keyLength);
148    if (carrier)
149    {
150        multiprecision_sub(c, c, modp, keyLength);
151    }
152    else if (multiprecision_compare(c, modp, keyLength)>=0)
153    {
154        multiprecision_sub(c, c, modp, keyLength);
155    }
156}
157
158// c=a>>1
159void multiprecision_rshift(DWORD * c, DWORD * a, uint32_t keyLength)
160{
161    int j;
162    DWORD b = 1;
163
164    j = DWORD_BITS - b;
165
166    DWORD carrier = 0;
167    DWORD temp;
168    for (int i = keyLength-1; i >= 0; i--)
169    {
170        temp = a[i]; // in case of c==a
171        c[i] = (temp >> b) | carrier;
172        carrier = temp << j;
173    }
174}
175
176// Curve specific optimization when p is a pseudo-Mersenns prime, p=2^(KEY_LENGTH_BITS)-omega
177void multiprecision_mersenns_mult_mod(DWORD *c, DWORD *a, DWORD *b, uint32_t keyLength)
178{
179    DWORD cc[2*KEY_LENGTH_DWORDS_P256];
180
181    multiprecision_mult(cc, a, b, keyLength);
182    if (keyLength == 6)
183    {
184        multiprecision_fast_mod(c, cc);
185    }
186    else if (keyLength == 8)
187    {
188        multiprecision_fast_mod_P256(c, cc);
189    }
190}
191
192// Curve specific optimization when p is a pseudo-Mersenns prime
193void multiprecision_mersenns_squa_mod(DWORD *c, DWORD *a, uint32_t keyLength)
194{
195    multiprecision_mersenns_mult_mod(c, a, a, keyLength);
196}
197
198// c=(a+b) mod p, b<p, a<p
199void multiprecision_add_mod(DWORD *c, DWORD *a, DWORD *b, uint32_t keyLength)
200{
201    DWORD carrier;
202    DWORD *modp;
203
204    if (keyLength == KEY_LENGTH_DWORDS_P192)
205    {
206        modp = curve.p;
207    }
208    else if (keyLength == KEY_LENGTH_DWORDS_P256)
209    {
210        modp = curve_p256.p;
211    }
212    else
213        return;
214
215    carrier = multiprecision_add(c, a, b, keyLength);
216    if (carrier)
217    {
218        multiprecision_sub(c, c, modp, keyLength);
219    }
220    else if (multiprecision_compare(c, modp, keyLength) >= 0)
221    {
222        multiprecision_sub(c, c, modp, keyLength);
223    }
224}
225
226// c=(a-b) mod p, a<p, b<p
227void multiprecision_sub_mod(DWORD *c, DWORD *a, DWORD *b, uint32_t keyLength)
228{
229    DWORD borrow;
230    DWORD *modp;
231
232    if (keyLength == KEY_LENGTH_DWORDS_P192)
233    {
234        modp = curve.p;
235    }
236    else if(keyLength == KEY_LENGTH_DWORDS_P256)
237    {
238        modp = curve_p256.p;
239    }
240    else
241        return;
242
243    borrow = multiprecision_sub(c, a, b, keyLength);
244    if(borrow)
245        multiprecision_add(c, c, modp, keyLength);
246}
247
248// c=a<<b, b<DWORD_BITS, c has a buffer size of NumDWORDs+1
249DWORD multiprecision_lshift(DWORD * c, DWORD * a, uint32_t keyLength)
250{
251    int j;
252    uint32_t b = 1;
253    j = DWORD_BITS - b;
254
255    DWORD carrier = 0;
256    DWORD temp;
257
258    for (uint32_t i = 0; i < keyLength; i++)
259    {
260        temp = a[i];  // in case c==a
261        c[i] = (temp << b) | carrier;
262        carrier = temp >> j;
263    }
264
265    return carrier;
266}
267
268// c=a*b; c must have a buffer of 2*Key_LENGTH_DWORDS, c != a != b
269void multiprecision_mult(DWORD *c, DWORD *a, DWORD *b, uint32_t keyLength)
270{
271    DWORD W;
272    DWORD U;
273    DWORD V;
274
275    U = V = W = 0;
276    multiprecision_init(c, keyLength);
277
278    //assume little endian right now
279    for (uint32_t i = 0; i < keyLength; i++)
280    {
281        U = 0;
282        for (uint32_t j = 0; j < keyLength; j++)
283        {
284            uint64_t result;
285            result = ((UINT64)a[i]) * ((uint64_t) b[j]);
286            W = result >> 32;
287            V = a[i] * b[j];
288            V = V + U;
289            U = (V < U);
290            U += W;
291            V = V + c[i+j];
292            U += (V < c[i+j]);
293            c[i+j] = V;
294        }
295        c[i+keyLength] = U;
296    }
297}
298
299void multiprecision_fast_mod(DWORD *c, DWORD *a)
300{
301    DWORD U;
302    DWORD V;
303    DWORD *modp = curve.p;
304
305    c[0] = a[0] + a[6];
306    U=c[0] < a[0];
307    c[0] += a[10];
308    U += c[0] < a[10];
309
310    c[1] = a[1] + U;
311    U = c[1] < a[1];
312    c[1] += a[7];
313    U += c[1] < a[7];
314    c[1] += a[11];
315    U += c[1]< a[11];
316
317    c[2] = a[2] + U;
318    U = c[2] < a[2];
319    c[2] += a[6];
320    U += c[2] < a[6];
321    c[2] += a[8];
322    U += c[2] < a[8];
323    c[2] += a[10];
324    U += c[2] < a[10];
325
326    c[3] = a[3]+U;
327    U = c[3] < a[3];
328    c[3] += a[7];
329    U += c[3] < a[7];
330    c[3] += a[9];
331    U += c[3] < a[9];
332    c[3] += a[11];
333    U += c[3] < a[11];
334
335    c[4] = a[4]+U;
336    U = c[4] < a[4];
337    c[4] += a[8];
338    U += c[4] < a[8];
339    c[4] += a[10];
340    U += c[4] < a[10];
341
342    c[5] = a[5]+U;
343    U = c[5] < a[5];
344    c[5] += a[9];
345    U += c[5] < a[9];
346    c[5] += a[11];
347    U += c[5] < a[11];
348
349    c[0] += U;
350    V = c[0] < U;
351    c[1] += V;
352    V = c[1] < V;
353    c[2] += V;
354    V = c[2] < V;
355    c[2] += U;
356    V = c[2] < U;
357    c[3] += V;
358    V = c[3] < V;
359    c[4] += V;
360    V = c[4] < V;
361    c[5] += V;
362    V = c[5] < V;
363
364    if (V)
365    {
366        multiprecision_sub(c, c, modp, KEY_LENGTH_DWORDS_P192);
367    }
368    else if(multiprecision_compare(c, modp, KEY_LENGTH_DWORDS_P192) >= 0)
369    {
370        multiprecision_sub(c, c, modp, KEY_LENGTH_DWORDS_P192);
371    }
372}
373
374void multiprecision_fast_mod_P256(DWORD *c, DWORD *a)
375{
376    DWORD A;
377    DWORD B;
378    DWORD C;
379    DWORD D;
380    DWORD E;
381    DWORD F;
382    DWORD G;
383    uint8_t UA;
384    uint8_t UB;
385    uint8_t UC;
386    uint8_t UD;
387    uint8_t UE;
388    uint8_t UF;
389    uint8_t UG;
390    DWORD U;
391    DWORD *modp = curve_p256.p;
392
393    // C = a[13] + a[14] + a[15];
394    C = a[13];
395    C += a[14];
396    UC = (C < a[14]);
397    C += a[15];
398    UC += (C < a[15]);
399
400    // E = a[8] + a[9];
401    E = a[8];
402    E += a[9];
403    UE = (E < a[9]);
404
405    // F = a[9] + a[10];
406    F = a[9];
407    F += a[10];
408    UF = (F < a[10]);
409
410    // G = a[10] + a[11]
411    G = a[10];
412    G += a[11];
413    UG = (G < a[11]);
414
415    // B = a[12] + a[13] + a[14] + a[15] == C + a[12]
416    B = C;
417    UB = UC;
418    B += a[12];
419    UB += (B < a[12]);
420
421    // A = a[11] + a[12] + a[13] + a[14] == B + a[11] - a[15]
422    A = B;
423    UA = UB;
424    A += a[11];
425    UA += (A < a[11]);
426    UA -= (A < a[15]);
427    A -= a[15];
428
429    // D = a[10] + a[11] + a[12] + a[13] == A + a[10] - a[14]
430    D = A;
431    UD = UA;
432    D += a[10];
433    UD += (D < a[10]);
434    UD -= (D < a[14]);
435    D -= a[14];
436
437    c[0] = a[0];
438    c[0] += E;
439    U = (c[0] < E);
440    U += UE;
441    U -= (c[0] < A);
442    U -= UA;
443    c[0] -= A;
444
445    if (U & 0x80000000)
446    {
447        DWORD UU;
448        UU = 0 - U;
449        U = (a[1] < UU);
450        c[1] = a[1] - UU;
451    }
452    else
453    {
454        c[1] = a[1] + U;
455        U = (c[1] < a[1]);
456    }
457
458    c[1] += F;
459    U += (c[1] < F);
460    U += UF;
461    U -= (c[1] < B);
462    U -= UB;
463    c[1] -= B;
464
465    if (U & 0x80000000)
466    {
467        DWORD UU;
468        UU = 0 - U;
469        U = (a[2] < UU);
470        c[2] = a[2] - UU;
471    }
472    else
473    {
474        c[2] = a[2] + U;
475        U = (c[2] < a[2]);
476    }
477
478    c[2] += G;
479    U += (c[2] < G);
480    U += UG;
481    U -= (c[2] < C);
482    U -= UC;
483    c[2] -= C;
484
485    if (U & 0x80000000)
486    {
487        DWORD UU;
488        UU = 0 - U;
489        U = (a[3] < UU);
490        c[3] = a[3] - UU;
491    }
492    else
493    {
494        c[3] = a[3] + U;
495        U = (c[3] < a[3]);
496    }
497
498    c[3] += A;
499    U += (c[3] < A);
500    U += UA;
501    c[3] += a[11];
502    U += (c[3] < a[11]);
503    c[3] += a[12];
504    U += (c[3] < a[12]);
505    U -= (c[3] < a[14]);
506    c[3] -= a[14];
507    U -= (c[3] < a[15]);
508    c[3] -= a[15];
509    U -= (c[3] < E);
510    U -= UE;
511    c[3] -= E;
512
513    if (U & 0x80000000)
514    {
515        DWORD UU;
516        UU = 0 - U;
517        U = (a[4] < UU);
518        c[4] = a[4] - UU;
519    }
520    else
521    {
522        c[4] = a[4] + U;
523        U = (c[4] < a[4]);
524    }
525
526    c[4] += B;
527    U += (c[4] < B);
528    U += UB;
529    U -= (c[4] < a[15]);
530    c[4] -= a[15];
531    c[4] += a[12];
532    U += (c[4] < a[12]);
533    c[4] += a[13];
534    U += (c[4] < a[13]);
535    U -= (c[4] < F);
536    U -= UF;
537    c[4] -= F;
538
539    if (U & 0x80000000)
540    {
541        DWORD UU;
542        UU = 0 - U;
543        U = (a[5] < UU);
544        c[5] = a[5] - UU;
545    }
546    else
547    {
548        c[5] = a[5] + U;
549        U = (c[5] < a[5]);
550    }
551
552    c[5] += C;
553    U += (c[5] < C);
554    U += UC;
555    c[5] += a[13];
556    U += (c[5] < a[13]);
557    c[5] += a[14];
558    U += (c[5] < a[14]);
559    U -= (c[5] < G);
560    U -= UG;
561    c[5] -= G;
562
563    if (U & 0x80000000)
564    {
565        DWORD UU;
566        UU = 0 - U;
567        U = (a[6] < UU);
568        c[6] = a[6] - UU;
569    }
570    else
571    {
572        c[6] = a[6] + U;
573        U = (c[6] < a[6]);
574    }
575
576    c[6] += C;
577    U += (c[6] < C);
578    U += UC;
579    c[6] += a[14];
580    U += (c[6] < a[14]);
581    c[6] += a[14];
582    U += (c[6] < a[14]);
583    c[6] += a[15];
584    U += (c[6] < a[15]);
585    U -= (c[6] < E);
586    U -= UE;
587    c[6] -= E;
588
589    if (U & 0x80000000)
590    {
591        DWORD UU;
592        UU = 0 - U;
593        U = (a[7] < UU);
594        c[7] = a[7] - UU;
595    }
596    else
597    {
598        c[7] = a[7] + U;
599        U = (c[7] < a[7]);
600    }
601
602    c[7] += a[15];
603    U += (c[7] < a[15]);
604    c[7] += a[15];
605    U += (c[7] < a[15]);
606    c[7] += a[15];
607    U += (c[7] < a[15]);
608    c[7] += a[8];
609    U += (c[7] < a[8]);
610    U -= (c[7] < D);
611    U -= UD;
612    c[7] -= D;
613
614    if (U & 0x80000000)
615    {
616        while (U)
617        {
618            multiprecision_add(c, c, modp, KEY_LENGTH_DWORDS_P256);
619            U++;
620        }
621    }
622    else if (U)
623    {
624        while (U)
625        {
626            multiprecision_sub(c, c, modp, KEY_LENGTH_DWORDS_P256);
627            U--;
628        }
629    }
630
631    if (multiprecision_compare(c, modp, KEY_LENGTH_DWORDS_P256)>=0)
632        multiprecision_sub(c, c, modp, KEY_LENGTH_DWORDS_P256);
633
634}
635
636void multiprecision_inv_mod(DWORD *aminus, DWORD *u, uint32_t keyLength)
637{
638    DWORD v[KEY_LENGTH_DWORDS_P256];
639    DWORD A[KEY_LENGTH_DWORDS_P256+1];
640    DWORD C[KEY_LENGTH_DWORDS_P256+1];
641    DWORD *modp;
642
643    if(keyLength == KEY_LENGTH_DWORDS_P256)
644    {
645        modp = curve_p256.p;
646    }
647    else
648    {
649        modp = curve.p;
650    }
651
652    multiprecision_copy(v, modp, keyLength);
653    multiprecision_init(A, keyLength);
654    multiprecision_init(C, keyLength);
655    A[0] = 1;
656
657    while (!multiprecision_iszero(u, keyLength))
658    {
659        while (!(u[0] & 0x01))  // u is even
660        {
661            multiprecision_rshift(u, u, keyLength);
662            if(!(A[0] & 0x01))  // A is even
663                multiprecision_rshift(A, A, keyLength);
664            else
665            {
666                A[keyLength]=multiprecision_add(A, A, modp, keyLength); // A =A+p
667                multiprecision_rshift(A, A, keyLength);
668                A[keyLength-1] |= (A[keyLength]<<31);
669            }
670        }
671
672        while (!(v[0] & 0x01))  // v is even
673        {
674            multiprecision_rshift(v, v, keyLength);
675            if (!(C[0] & 0x01))  // C is even
676            {
677                multiprecision_rshift(C, C, keyLength);
678            }
679            else
680            {
681                C[keyLength] = multiprecision_add(C, C, modp, keyLength); // C =C+p
682                multiprecision_rshift(C, C, keyLength);
683                C[keyLength-1] |= (C[keyLength] << 31);
684            }
685        }
686
687        if (multiprecision_compare(u, v, keyLength) >= 0)
688        {
689            multiprecision_sub(u, u, v, keyLength);
690            multiprecision_sub_mod(A, A, C, keyLength);
691        }
692        else
693        {
694            multiprecision_sub(v, v, u, keyLength);
695            multiprecision_sub_mod(C, C, A, keyLength);
696        }
697    }
698
699    if (multiprecision_compare(C, modp, keyLength) >= 0)
700        multiprecision_sub(aminus, C, modp, keyLength);
701    else
702        multiprecision_copy(aminus, C, keyLength);
703}
704
705