/* computes a = B**n mod b without division or multiplication useful for
* normalizing numbers in a Montgomery system.
*/
void fp_montgomery_calc_normalization(fp_int *a, fp_int *b)
{
int x, bits;
/* how many bits of last digit does b use */
bits = fp_count_bits (b) % DIGIT_BIT;
if (!bits) bits = DIGIT_BIT;
/* compute A = B^(n-1) * 2^(bits-1) */
if (b->used > 1) {
fp_2expt (a, (b->used - 1) * DIGIT_BIT + bits - 1);
} else {
fp_set(a, 1);
bits = 1;
}
/* now compute C = A * B mod b */
for (x = bits - 1; x < (int)DIGIT_BIT; x++) {
fp_mul_2 (a, a);
if (fp_cmp_mag (a, b) != FP_LT) {
s_fp_sub (a, b, a);
}
}
}
int do_rsa_proc(char *pSRC, int nDataLen,char *pOUT,int *pOUTLen,fp_int *key, int procedure)
{
// fp_int *key--> key[0], key[1], key[2]
int in_size = nDataLen;
int nret=-1;
int nlen;
fp_int c,m;
fp_int* k,* t;
unsigned char *pSRCBuff = (unsigned char *)pSRC;
unsigned char *pOUTBuff = (unsigned char *)pOUT;
memset(pOUTBuff,0,*pOUTLen);
*pOUTLen = 0;
//get key length
//for RSA1024 : 1028bit -> 128 bytes
//dec-i_len : 128 dec-o_len:127
//enc-i_len : 127 dec-o_len:128
uint32_t bits=fp_count_bits(&key[0]);
uint32_t o_len,i_len;
//dec
i_len=(bits+7)>>3;
o_len=i_len-1;
//pub key
k=&key[1];
switch(procedure & RSA_ENC_DEC_MASK)
{
case ENC_WITH_PRIV:o_len=i_len;--i_len;;k=&key[2];break;
case DEC_WITH_PUB:break;
case ENC_WITH_PUB:o_len=i_len;--i_len;break;
case DEC_WITH_PRIV:k=&key[2];break;
default:
//printf("Internal error! Not support this enc-dec mode %d\n",procedure);
//dbg_print("Internal error! Not support this enc-dec mode ", procedure);
return nret; break;
}
//buffer process
if((procedure & RSA_BUF_IN_TYPE_MASK) != RSA_BUF_IN_TYPE_IGNORE)
aml_buffer_proc_before(pSRCBuff,nDataLen,i_len,procedure);
for(nlen=0;nlen<in_size;nlen+=i_len)
{
fp_init(&c);
fp_init(&m);
memcpy(c.dp,pSRCBuff,i_len);
pSRCBuff += i_len;
c.used=FP_SIZE;
fp_clamp(&c);
fp_exptmod(&c,k,&key[0],&m);
memcpy(pOUTBuff+*pOUTLen,m.dp,o_len);
*pOUTLen += o_len;
}
//buffer process
if((procedure & RSA_BUF_OUT_TYPE_MASK) != RSA_BUF_OUT_TYPE_IGNORE)
aml_buffer_proc_after(pOUTBuff,pOUTLen,o_len,procedure);
nret = 0;
return nret;
}
static int count_bits(void *a)
{
LTC_ARGCHK(a != NULL);
return fp_count_bits(a);
}
/* a/b => cb + d == a */
int fp_div(fp_int *a, fp_int *b, fp_int *c, fp_int *d)
{
fp_int q, x, y, t1, t2;
int n, t, i, norm, neg;
/* is divisor zero ? */
if (fp_iszero (b) == 1) {
return FP_VAL;
}
/* if a < b then q=0, r = a */
if (fp_cmp_mag (a, b) == FP_LT) {
if (d != NULL) {
fp_copy (a, d);
}
if (c != NULL) {
fp_zero (c);
}
return FP_OKAY;
}
fp_init(&q);
q.used = a->used + 2;
fp_init(&t1);
fp_init(&t2);
fp_init_copy(&x, a);
fp_init_copy(&y, b);
/* fix the sign */
neg = (a->sign == b->sign) ? FP_ZPOS : FP_NEG;
x.sign = y.sign = FP_ZPOS;
/* normalize both x and y, ensure that y >= b/2, [b == 2**DIGIT_BIT] */
norm = fp_count_bits(&y) % DIGIT_BIT;
if (norm < (int)(DIGIT_BIT-1)) {
norm = (DIGIT_BIT-1) - norm;
fp_mul_2d (&x, norm, &x);
fp_mul_2d (&y, norm, &y);
} else {
norm = 0;
}
/* note hac does 0 based, so if used==5 then its 0,1,2,3,4, e.g. use 4 */
n = x.used - 1;
t = y.used - 1;
/* while (x >= y*b**n-t) do { q[n-t] += 1; x -= y*b**{n-t} } */
fp_lshd (&y, n - t); /* y = y*b**{n-t} */
while (fp_cmp (&x, &y) != FP_LT) {
++(q.dp[n - t]);
fp_sub (&x, &y, &x);
}
/* reset y by shifting it back down */
fp_rshd (&y, n - t);
/* step 3. for i from n down to (t + 1) */
for (i = n; i >= (t + 1); i--) {
if (i > x.used) {
continue;
}
/* step 3.1 if xi == yt then set q{i-t-1} to b-1,
* otherwise set q{i-t-1} to (xi*b + x{i-1})/yt */
if (x.dp[i] == y.dp[t]) {
q.dp[i - t - 1] = ((((fp_word)1) << DIGIT_BIT) - 1);
} else {
fp_word tmp;
tmp = ((fp_word) x.dp[i]) << ((fp_word) DIGIT_BIT);
tmp |= ((fp_word) x.dp[i - 1]);
tmp /= ((fp_word) y.dp[t]);
q.dp[i - t - 1] = (fp_digit) (tmp);
}
/* while (q{i-t-1} * (yt * b + y{t-1})) >
xi * b**2 + xi-1 * b + xi-2
do q{i-t-1} -= 1;
*/
q.dp[i - t - 1] = (q.dp[i - t - 1] + 1);
do {
q.dp[i - t - 1] = (q.dp[i - t - 1] - 1);
/* find left hand */
fp_zero (&t1);
t1.dp[0] = (t - 1 < 0) ? 0 : y.dp[t - 1];
t1.dp[1] = y.dp[t];
t1.used = 2;
fp_mul_d (&t1, q.dp[i - t - 1], &t1);
/* find right hand */
t2.dp[0] = (i - 2 < 0) ? 0 : x.dp[i - 2];
t2.dp[1] = (i - 1 < 0) ? 0 : x.dp[i - 1];
t2.dp[2] = x.dp[i];
t2.used = 3;
} while (fp_cmp_mag(&t1, &t2) == FP_GT);
/* step 3.3 x = x - q{i-t-1} * y * b**{i-t-1} */
fp_mul_d (&y, q.dp[i - t - 1], &t1);
fp_lshd (&t1, i - t - 1);
fp_sub (&x, &t1, &x);
/* if x < 0 then { x = x + y*b**{i-t-1}; q{i-t-1} -= 1; } */
if (x.sign == FP_NEG) {
fp_copy (&y, &t1);
fp_lshd (&t1, i - t - 1);
fp_add (&x, &t1, &x);
q.dp[i - t - 1] = q.dp[i - t - 1] - 1;
}
}
/* now q is the quotient and x is the remainder
* [which we have to normalize]
*/
/* get sign before writing to c */
x.sign = x.used == 0 ? FP_ZPOS : a->sign;
if (c != NULL) {
fp_clamp (&q);
fp_copy (&q, c);
c->sign = neg;
}
if (d != NULL) {
fp_div_2d (&x, norm, &x, NULL);
/* the following is a kludge, essentially we were seeing the right remainder but
with excess digits that should have been zero
*/
for (i = b->used; i < x.used; i++) {
x.dp[i] = 0;
}
fp_clamp(&x);
fp_copy (&x, d);
}
return FP_OKAY;
}
int do_rsa_enc_dec(char *pSRC, int nDataLen,char *pOUT,int *pOUTLen,int procedure)
{
int in_size = nDataLen;
int nret = 1;
if ( NULL == pSRC || NULL == pOUT )
return nret;
//check nSRCLen for dec & enc
//...
unsigned char *pSRCBuff = (unsigned char *)pSRC;
unsigned char *pOUTBuff = (unsigned char *)pOUT;
//clear output buffer??
memset(pOUTBuff,0,*pOUTLen);
*pOUTLen = 0;
//prepare key
fp_int key[3];
memset(&key[0],0,sizeof(key));
//get key from efuse
if(get_rsa_key(&key[0],&key[1],&key[2]))
return nret;
int nlen;
fp_int c,m;
fp_int* k,* t;
//get key length
//for RSA1024 : 1028bit -> 128 bytes
//dec-i_len : 128 dec-o_len:127
//enc-i_len : 127 dec-o_len:128
uint32_t bits=fp_count_bits(&key[0]);
uint32_t o_len,i_len;
//dec
i_len=(bits+7)>>3;
o_len=i_len-1;
//pub key
k=&key[1];
switch(procedure & RSA_ENC_DEC_MASK)
{
case ENC_WITH_PRIV:o_len=i_len;--i_len;;k=&key[2];break;
case DEC_WITH_PUB:break;
case ENC_WITH_PUB:o_len=i_len;--i_len;break;
case DEC_WITH_PRIV:k=&key[2];break;
default: printf("Internal error! Not support this enc-dec mode %d\n",procedure);return nret; break;
}
//buffer process
// aml_buffer_proc_before(pSRCBuff,nDataLen,i_len,procedure);
for(nlen=0;nlen<in_size;nlen+=i_len)
{
fp_init(&c);
fp_init(&m);
memcpy(c.dp,pSRCBuff,i_len);
pSRCBuff += i_len;
c.used=FP_SIZE;
fp_clamp(&c);
fp_exptmod(&c,k,&key[0],&m);
memcpy(pOUTBuff+*pOUTLen,m.dp,o_len);
*pOUTLen += o_len;
}
//buffer process
// aml_buffer_proc_after(pOUTBuff,pOUTLen,o_len,procedure);
nret = 0;
return nret;
}
