char filter(){
fp_t abs = fp_add(fp_add(fp_mul(acc[0], acc[0]),
fp_mul(acc[1], acc[1])),
fp_mul(acc[2], acc[2]));
////////////////////////////////////////////////////////////////////////////////
//idle state filter
if (!(fp_cmp(abs, d2fp(0.32))==1 ||
fp_cmp(abs, d2fp(0.27))==-1)) { // if between 0.01 - 0.09
return false;
}
////////////////////////////////////////////////////////////////////////////////
// def = directional equivalence filter
fp_t def_sensitivity = d2fp(0.08);
if (fp_cmp(acc[0], fp_sub(dir_filter_ref[0], def_sensitivity))==-1 ||
fp_cmp(acc[0], fp_add(dir_filter_ref[0], def_sensitivity))== 1 ||
fp_cmp(acc[1], fp_sub(dir_filter_ref[1], def_sensitivity))==-1 ||
fp_cmp(acc[1], fp_add(dir_filter_ref[1], def_sensitivity))== 1 ||
fp_cmp(acc[2], fp_sub(dir_filter_ref[2], def_sensitivity))==-1 ||
fp_cmp(acc[2], fp_add(dir_filter_ref[2], def_sensitivity))==1) {
dir_filter_ref[0] = acc[0];
dir_filter_ref[1] = acc[1];
dir_filter_ref[2] = acc[2];
return true;
}
return false;
}
char filter(){
fp_t abs = fp_add(fp_add(fp_mul(acc[0], acc[0]),
fp_mul(acc[1], acc[1])),
fp_mul(acc[2], acc[2]));
////////////////////////////////////////////////////////////////////////////////
//idle state filter
if (!(fp_cmp(abs, 10486)==1 /*0.32*/ ||
fp_cmp(abs, 8847)==2 /*0.27*/)) { // if between 0.01 - 0.09
return false;
}
////////////////////////////////////////////////////////////////////////////////
// def = directional equivalence filter
const fp_t def_sensitivity = 2621; //0.08
if (fp_cmp(acc[0], fp_sub(dir_filter_ref[0], def_sensitivity))==2 ||
fp_cmp(acc[0], fp_add(dir_filter_ref[0], def_sensitivity))== 1 ||
fp_cmp(acc[1], fp_sub(dir_filter_ref[1], def_sensitivity))==2 ||
fp_cmp(acc[1], fp_add(dir_filter_ref[1], def_sensitivity))== 1 ||
fp_cmp(acc[2], fp_sub(dir_filter_ref[2], def_sensitivity))==2 ||
fp_cmp(acc[2], fp_add(dir_filter_ref[2], def_sensitivity))==1) {
dir_filter_ref[0] = acc[0];
dir_filter_ref[1] = acc[1];
dir_filter_ref[2] = acc[2];
return true;
}
return false;
}
int ep_cmp(const ep_t p, const ep_t q) {
if (fp_cmp(p->x, q->x) != CMP_EQ) {
return CMP_NE;
}
if (fp_cmp(p->y, q->y) != CMP_EQ) {
return CMP_NE;
}
if (fp_cmp(p->z, q->z) != CMP_EQ) {
return CMP_NE;
}
return CMP_EQ;
}
void fp_rdcn_low(dig_t *c, dig_t *a) {
int i;
dig_t r, c0, c1, u, *tmp;
const dig_t *m;
u = *(fp_prime_get_rdc());
m = fp_prime_get();
tmp = a;
c1 = 0;
for (i = 0; i < FP_DIGS; i++, tmp++) {
r = (dig_t)(*tmp * u);
c0 = mpn_addmul_1(tmp, m, FP_DIGS, r);
c1 += mpn_add_1(tmp + FP_DIGS, tmp + FP_DIGS, FP_DIGS - i, c0);
}
for (i = 0; i < FP_DIGS; i++, tmp++) {
c[i] = *tmp;
}
for (i = 0; i < c1; i++) {
fp_subn_low(c, c, m);
}
if (fp_cmp(c, m) != CMP_LT) {
fp_subn_low(c, c, m);
}
}
//Called at the end of the gesture,
//Returns the id of the recognized gesture or -1 if none.
char input_end(){
fp_t prob;
char recognized = -1; // which gesture has been recognized
fp_t recogprob = -1; // probability of this gesture
fp_t tmpgesture;
char i, j;
started = false;
for (i = 0; i < 2; i++){
prob = 0;
// add probabilities
for (j = 0; j < 8; j++){
prob = fp_add(prob, s[j]);
}
if (fp_cmp(prob, recogprob)==1) {
recogprob = prob;
recognized = i;
}
}
//printf("m->prob = %.30f\n", fp2d(recogprob));
UARTSendArray("p=", 2);
UARTSendInt(recogprob);
//dir_filter_ref[0] = 0; //reset for next time
return recognized;
}
int fp_cmp_dig(const fp_t a, dig_t b) {
#if FP_RDC == MONTY
fp_t t;
int r = CMP_EQ;
fp_null(t);
TRY {
fp_new(t);
fp_prime_conv_dig(t, b);
r = fp_cmp(a, t);
} CATCH_ANY {
THROW(ERR_CAUGHT);
} FINALLY {
fp_free(t);
}
return r;
#else
for (int i = 1; i < FP_DIGS; i++) {
if (a[i] > 0) {
return CMP_GT;
}
}
return fp_cmp1_low(a[0], b);
#endif
}
bool operator< (const Entry& rhs) const
{
if (fp_cmp (weight, rhs.weight)) {
return height > rhs.height;
} else {
return weight < rhs.weight;
}
}
int ed_cmp(const ed_t p, const ed_t q) {
int ret = CMP_NE;
if (fp_cmp(p->x, q->x) != CMP_EQ) {
ret = CMP_NE;
} else if (fp_cmp(p->y, q->y) != CMP_EQ) {
ret = CMP_NE;
} else if (fp_cmp(p->z, q->z) != CMP_EQ) {
ret = CMP_NE;
#if ED_ADD == EXTND
} else if (fp_cmp(p->t, q->t) != CMP_EQ) {
ret = CMP_NE;
#endif
} else {
ret = CMP_EQ;
}
return ret;
}
void fp_rdcn_low(dig_t *c, dig_t *a) {
dig_t r1, *m, *tmpc;
m = fp_prime_get();
tmpc = c;
r1 = fp_rdci_low(c, a);
if (r1 || fp_cmp(c, m) != CMP_LT) {
fp_subn_low(c, c, m);
}
}
static int compare(void *a, void *b)
{
int ret;
LTC_ARGCHK(a != NULL);
LTC_ARGCHK(b != NULL);
ret = fp_cmp(a, b);
switch (ret) {
case FP_LT: return LTC_MP_LT;
case FP_EQ: return LTC_MP_EQ;
case FP_GT: return LTC_MP_GT;
}
return 0;
}
void fp_rdcn_low(dig_t *c, dig_t *a) {
int i, j;
dig_t r0, r1, r2, u, v;
dig_t *m, *tmp, *tmpm, *tmpc;
dig_t t[2 * FP_DIGS] = {0};
m = fp_prime_get();
tmpc = c;
r1 = fp_rdci_low(c, a);
if (r1 || fp_cmp(c, m) != CMP_LT) {
fp_subn_low(c, c, m);
}
}
int ed_is_valid(const ed_t p) {
ed_t t;
#if ED_ADD == EXTND
fp_t x_times_y;
#endif
int r = 0;
ed_null(t);
#if ED_ADD == EXTND
fp_null(x_times_y);
#endif
if (fp_is_zero(p->z)) {
r = 0;
} else {
TRY {
#if ED_ADD == EXTND
fp_new(x_times_y);
#endif
ed_new(t);
ed_norm(t, p);
// check t coordinate
#if ED_ADD == PROJC
r = ed_affine_is_valid(t->x, t->y);
#elif ED_ADD == EXTND
fp_mul(x_times_y, t->x, t->y);
if (fp_cmp(x_times_y, t->t) != CMP_EQ) {
r = 0;
} else {
r = ed_affine_is_valid(t->x, t->y);
}
#endif
// if (r == 0) {
// util_printf("\n\n(X, Y, T, Z) = \n");
// ed_print(p);
// }
} CATCH_ANY {
THROW(ERR_CAUGHT);
} FINALLY {
#if ED_ADD == EXTND
fp_free(x_times_y);
#endif
ed_free(t);
}
}
return r;
}
static int
tfm_dh_compute_key(unsigned char *shared, const BIGNUM * pub, DH *dh)
{
fp_int s, priv_key, p, peer_pub;
size_t size = 0;
int ret;
if (dh->pub_key == NULL || dh->g == NULL || dh->priv_key == NULL)
return -1;
fp_init(&p);
BN2mpz(&p, dh->p);
fp_init(&peer_pub);
BN2mpz(&peer_pub, pub);
/* check if peers pubkey is reasonable */
if (fp_isneg(&peer_pub)
|| fp_cmp(&peer_pub, &p) >= 0
|| fp_cmp_d(&peer_pub, 1) <= 0)
{
fp_zero(&p);
fp_zero(&peer_pub);
return -1;
}
fp_init(&priv_key);
BN2mpz(&priv_key, dh->priv_key);
fp_init(&s);
ret = fp_exptmod(&peer_pub, &priv_key, &p, &s);
fp_zero(&p);
fp_zero(&peer_pub);
fp_zero(&priv_key);
if (ret != 0)
return -1;
size = fp_unsigned_bin_size(&s);
fp_to_unsigned_bin(&s, shared);
fp_zero(&s);
return size;
}
int fp_cmp_dig(const fp_t a, dig_t b) {
fp_t t;
int r = RLC_EQ;
fp_null(t);
TRY {
fp_new(t);
fp_prime_conv_dig(t, b);
r = fp_cmp(a, t);
} CATCH_ANY {
THROW(ERR_CAUGHT);
} FINALLY {
fp_free(t);
}
return r;
}
int ep_is_valid(const ep_t p) {
ep_t t;
int r = 0;
ep_null(t);
TRY {
ep_new(t);
ep_norm(t, p);
ep_rhs(t->x, t);
fp_sqr(t->y, t->y);
r = (fp_cmp(t->x, t->y) == CMP_EQ) || ep_is_infty(p);
} CATCH_ANY {
THROW(ERR_CAUGHT);
} FINALLY {
ep_free(t);
}
return r;
}
/**
* Detects an optimization based on the curve coefficients.
*
* @param[out] opt - the resulting optimization.
* @param[in] a - the curve coefficient.
*/
static void detect_opt(int *opt, fp_t a) {
fp_t t;
fp_null(t);
TRY {
fp_new(t);
fp_prime_conv_dig(t, 3);
fp_neg(t, t);
if (fp_cmp(a, t) == CMP_EQ) {
*opt = OPT_MINUS3;
} else {
if (fp_is_zero(a)) {
*opt = OPT_ZERO;
} else {
fp_set_dig(t, 1);
if (fp_cmp_dig(a, 1) == CMP_EQ) {
*opt = OPT_ONE;
} else {
if (fp_cmp_dig(a, 2) == CMP_EQ) {
*opt = OPT_TWO;
} else {
if (fp_bits(a) <= FP_DIGIT) {
*opt = OPT_DIGIT;
} else {
*opt = RELIC_OPT_NONE;
}
}
}
}
}
}
CATCH_ANY {
THROW(ERR_CAUGHT);
}
FINALLY {
fp_free(t);
}
}
unsigned char derive_group2(){
fp_t a, b, c, d;
fp_t minDist = 0x7fff; //0x7fff;
char minGroup=0;
fp_t *ref;
char i;
for (i = 0; i < 14; i++){
ref = quantizerMap2[i];
a = fp_sub(ref[0], acc[0]);
b = fp_sub(ref[1], acc[1]);
c = fp_sub(ref[2], acc[2]);
d = fp_add(fp_add(fp_mul(a,a), fp_mul(b,b)), fp_mul(c,c));
if (fp_cmp(d, minDist) == 2){
minDist = d;
minGroup = i;
}
}
/* UARTSendArray("group=", 6); */
/* UARTSendInt(minGroup); */
return minGroup;
}
//Called at the end of the gesture,
//Returns the id of the recognized gesture or -1 if none.
char input_end(){
fp_t prob, prob2;
char recognized; // which gesture has been recognized
char j;
started = false;
prob = 0;
prob2 = 0;
// add probabilities
for (j = 0; j < 8; j++){
prob = fp_add(prob, s[j]);
prob2 = fp_add(prob2, s2[j]);
}
if (fp_cmp(prob, prob2)==1) {
recognized = 0;
} else {
recognized = 1;
}
//printf("\np = %d %d\n", prob, prob2);
return recognized;
}
static int tfm_ecc_projective_dbl_point(ecc_point *P, ecc_point *R, void *modulus, void *Mp)
{
fp_int t1, t2;
fp_digit mp;
LTC_ARGCHK(P != NULL);
LTC_ARGCHK(R != NULL);
LTC_ARGCHK(modulus != NULL);
LTC_ARGCHK(Mp != NULL);
mp = *((fp_digit*)Mp);
fp_init(&t1);
fp_init(&t2);
if (P != R) {
fp_copy(P->x, R->x);
fp_copy(P->y, R->y);
fp_copy(P->z, R->z);
}
/* t1 = Z * Z */
fp_sqr(R->z, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* Z = Y * Z */
fp_mul(R->z, R->y, R->z);
fp_montgomery_reduce(R->z, modulus, mp);
/* Z = 2Z */
fp_add(R->z, R->z, R->z);
if (fp_cmp(R->z, modulus) != FP_LT) {
fp_sub(R->z, modulus, R->z);
}
/* &t2 = X - T1 */
fp_sub(R->x, &t1, &t2);
if (fp_cmp_d(&t2, 0) == FP_LT) {
fp_add(&t2, modulus, &t2);
}
/* T1 = X + T1 */
fp_add(&t1, R->x, &t1);
if (fp_cmp(&t1, modulus) != FP_LT) {
fp_sub(&t1, modulus, &t1);
}
/* T2 = T1 * T2 */
fp_mul(&t1, &t2, &t2);
fp_montgomery_reduce(&t2, modulus, mp);
/* T1 = 2T2 */
fp_add(&t2, &t2, &t1);
if (fp_cmp(&t1, modulus) != FP_LT) {
fp_sub(&t1, modulus, &t1);
}
/* T1 = T1 + T2 */
fp_add(&t1, &t2, &t1);
if (fp_cmp(&t1, modulus) != FP_LT) {
fp_sub(&t1, modulus, &t1);
}
/* Y = 2Y */
fp_add(R->y, R->y, R->y);
if (fp_cmp(R->y, modulus) != FP_LT) {
fp_sub(R->y, modulus, R->y);
}
/* Y = Y * Y */
fp_sqr(R->y, R->y);
fp_montgomery_reduce(R->y, modulus, mp);
/* T2 = Y * Y */
fp_sqr(R->y, &t2);
fp_montgomery_reduce(&t2, modulus, mp);
/* T2 = T2/2 */
if (fp_isodd(&t2)) {
fp_add(&t2, modulus, &t2);
}
fp_div_2(&t2, &t2);
/* Y = Y * X */
fp_mul(R->y, R->x, R->y);
fp_montgomery_reduce(R->y, modulus, mp);
/* X = T1 * T1 */
fp_sqr(&t1, R->x);
fp_montgomery_reduce(R->x, modulus, mp);
/* X = X - Y */
fp_sub(R->x, R->y, R->x);
if (fp_cmp_d(R->x, 0) == FP_LT) {
fp_add(R->x, modulus, R->x);
}
/* X = X - Y */
fp_sub(R->x, R->y, R->x);
if (fp_cmp_d(R->x, 0) == FP_LT) {
fp_add(R->x, modulus, R->x);
}
/* Y = Y - X */
fp_sub(R->y, R->x, R->y);
if (fp_cmp_d(R->y, 0) == FP_LT) {
fp_add(R->y, modulus, R->y);
}
/* Y = Y * T1 */
fp_mul(R->y, &t1, R->y);
fp_montgomery_reduce(R->y, modulus, mp);
/* Y = Y - T2 */
fp_sub(R->y, &t2, R->y);
if (fp_cmp_d(R->y, 0) == FP_LT) {
fp_add(R->y, modulus, R->y);
}
return CRYPT_OK;
}
int fp_srt(fp_t c, const fp_t a) {
bn_t e;
fp_t t0;
fp_t t1;
int r = 0;
bn_null(e);
fp_null(t0);
fp_null(t1);
TRY {
bn_new(e);
fp_new(t0);
fp_new(t1);
/* Make e = p. */
e->used = FP_DIGS;
dv_copy(e->dp, fp_prime_get(), FP_DIGS);
if (fp_prime_get_mod8() == 3 || fp_prime_get_mod8() == 7) {
/* Easy case, compute a^((p + 1)/4). */
bn_add_dig(e, e, 1);
bn_rsh(e, e, 2);
fp_exp(t0, a, e);
fp_sqr(t1, t0);
r = (fp_cmp(t1, a) == CMP_EQ);
fp_copy(c, t0);
} else {
int f = 0, m = 0;
/* First, check if there is a root. Compute t1 = a^((p - 1)/2). */
bn_rsh(e, e, 1);
fp_exp(t0, a, e);
if (fp_cmp_dig(t0, 1) != CMP_EQ) {
/* Nope, there is no square root. */
r = 0;
} else {
r = 1;
/* Find a quadratic non-residue modulo p, that is a number t2
* such that (t2 | p) = t2^((p - 1)/2)!= 1. */
do {
fp_rand(t1);
fp_exp(t0, t1, e);
} while (fp_cmp_dig(t0, 1) == CMP_EQ);
/* Write p - 1 as (e * 2^f), odd e. */
bn_lsh(e, e, 1);
while (bn_is_even(e)) {
bn_rsh(e, e, 1);
f++;
}
/* Compute t2 = t2^e. */
fp_exp(t1, t1, e);
/* Compute t1 = a^e, c = a^((e + 1)/2) = a^(e/2 + 1), odd e. */
bn_rsh(e, e, 1);
fp_exp(t0, a, e);
fp_mul(e->dp, t0, a);
fp_sqr(t0, t0);
fp_mul(t0, t0, a);
fp_copy(c, e->dp);
while (1) {
if (fp_cmp_dig(t0, 1) == CMP_EQ) {
break;
}
fp_copy(e->dp, t0);
for (m = 0; (m < f) && (fp_cmp_dig(t0, 1) != CMP_EQ); m++) {
fp_sqr(t0, t0);
}
fp_copy(t0, e->dp);
for (int i = 0; i < f - m - 1; i++) {
fp_sqr(t1, t1);
}
fp_mul(c, c, t1);
fp_sqr(t1, t1);
fp_mul(t0, t0, t1);
f = m;
}
}
}
}
CATCH_ANY {
THROW(ERR_CAUGHT);
}
FINALLY {
bn_free(e);
fp_free(t0);
fp_free(t1);
}
return r;
}
void fp_addm_low(dig_t *c, const dig_t *a, const dig_t *b) {
fp_addn_low(c, a, b);
if (fp_cmp(c, fp_prime_get()) != CMP_LT) {
fp_subn_low(c, c, fp_prime_get());
}
}
/**
Add two ECC points
@param P The point to add
@param Q The point to add
@param R [out] The destination of the double
@param modulus The modulus of the field the ECC curve is in
@param mp The "b" value from montgomery_setup()
@return CRYPT_OK on success
*/
static int tfm_ecc_projective_add_point(ecc_point *P, ecc_point *Q, ecc_point *R, void *modulus, void *Mp)
{
fp_int t1, t2, x, y, z;
fp_digit mp;
LTC_ARGCHK(P != NULL);
LTC_ARGCHK(Q != NULL);
LTC_ARGCHK(R != NULL);
LTC_ARGCHK(modulus != NULL);
LTC_ARGCHK(Mp != NULL);
mp = *((fp_digit*)Mp);
fp_init(&t1);
fp_init(&t2);
fp_init(&x);
fp_init(&y);
fp_init(&z);
/* should we dbl instead? */
fp_sub(modulus, Q->y, &t1);
if ( (fp_cmp(P->x, Q->x) == FP_EQ) &&
(Q->z != NULL && fp_cmp(P->z, Q->z) == FP_EQ) &&
(fp_cmp(P->y, Q->y) == FP_EQ || fp_cmp(P->y, &t1) == FP_EQ)) {
return tfm_ecc_projective_dbl_point(P, R, modulus, Mp);
}
fp_copy(P->x, &x);
fp_copy(P->y, &y);
fp_copy(P->z, &z);
/* if Z is one then these are no-operations */
if (Q->z != NULL) {
/* T1 = Z' * Z' */
fp_sqr(Q->z, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* X = X * T1 */
fp_mul(&t1, &x, &x);
fp_montgomery_reduce(&x, modulus, mp);
/* T1 = Z' * T1 */
fp_mul(Q->z, &t1, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* Y = Y * T1 */
fp_mul(&t1, &y, &y);
fp_montgomery_reduce(&y, modulus, mp);
}
/* T1 = Z*Z */
fp_sqr(&z, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* T2 = X' * T1 */
fp_mul(Q->x, &t1, &t2);
fp_montgomery_reduce(&t2, modulus, mp);
/* T1 = Z * T1 */
fp_mul(&z, &t1, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* T1 = Y' * T1 */
fp_mul(Q->y, &t1, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* Y = Y - T1 */
fp_sub(&y, &t1, &y);
if (fp_cmp_d(&y, 0) == FP_LT) {
fp_add(&y, modulus, &y);
}
/* T1 = 2T1 */
fp_add(&t1, &t1, &t1);
if (fp_cmp(&t1, modulus) != FP_LT) {
fp_sub(&t1, modulus, &t1);
}
/* T1 = Y + T1 */
fp_add(&t1, &y, &t1);
if (fp_cmp(&t1, modulus) != FP_LT) {
fp_sub(&t1, modulus, &t1);
}
/* X = X - T2 */
fp_sub(&x, &t2, &x);
if (fp_cmp_d(&x, 0) == FP_LT) {
fp_add(&x, modulus, &x);
}
/* T2 = 2T2 */
fp_add(&t2, &t2, &t2);
if (fp_cmp(&t2, modulus) != FP_LT) {
fp_sub(&t2, modulus, &t2);
}
/* T2 = X + T2 */
fp_add(&t2, &x, &t2);
if (fp_cmp(&t2, modulus) != FP_LT) {
fp_sub(&t2, modulus, &t2);
}
/* if Z' != 1 */
if (Q->z != NULL) {
/* Z = Z * Z' */
fp_mul(&z, Q->z, &z);
fp_montgomery_reduce(&z, modulus, mp);
}
/* Z = Z * X */
fp_mul(&z, &x, &z);
fp_montgomery_reduce(&z, modulus, mp);
/* T1 = T1 * X */
fp_mul(&t1, &x, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* X = X * X */
fp_sqr(&x, &x);
fp_montgomery_reduce(&x, modulus, mp);
/* T2 = T2 * x */
fp_mul(&t2, &x, &t2);
fp_montgomery_reduce(&t2, modulus, mp);
/* T1 = T1 * X */
fp_mul(&t1, &x, &t1);
fp_montgomery_reduce(&t1, modulus, mp);
/* X = Y*Y */
fp_sqr(&y, &x);
fp_montgomery_reduce(&x, modulus, mp);
/* X = X - T2 */
fp_sub(&x, &t2, &x);
if (fp_cmp_d(&x, 0) == FP_LT) {
fp_add(&x, modulus, &x);
}
/* T2 = T2 - X */
fp_sub(&t2, &x, &t2);
if (fp_cmp_d(&t2, 0) == FP_LT) {
fp_add(&t2, modulus, &t2);
}
/* T2 = T2 - X */
fp_sub(&t2, &x, &t2);
if (fp_cmp_d(&t2, 0) == FP_LT) {
fp_add(&t2, modulus, &t2);
}
/* T2 = T2 * Y */
fp_mul(&t2, &y, &t2);
fp_montgomery_reduce(&t2, modulus, mp);
/* Y = T2 - T1 */
fp_sub(&t2, &t1, &y);
if (fp_cmp_d(&y, 0) == FP_LT) {
fp_add(&y, modulus, &y);
}
/* Y = Y/2 */
if (fp_isodd(&y)) {
fp_add(&y, modulus, &y);
}
fp_div_2(&y, &y);
fp_copy(&x, R->x);
fp_copy(&y, R->y);
fp_copy(&z, R->z);
return CRYPT_OK;
}
int main(void)
{
fp_int d, e, n, c, m, e_m;
clock_t t1;
int x;
/* read in the parameters */
fp_read_radix(&n, "ce032e860a9809a5ec31e4b0fd4b546f8c40043e3d2ec3d8f49d8f2f3dd19e887094ee1af75caa1c2e6cd9ec78bf1dfd6280002ac8c30ecd72da2e4c59a28a9248048aaae2a8fa627f71bece979cebf9f8eee2bd594d4a4f2e791647573c7ec1fcbd320d3825be3fa8a17c97086fdae56f7086ce512b81cc2fe44161270ec5e9", 16);
fp_read_radix(&e, "10001", 16);
fp_read_radix(&m, "39f5a911250f45b99390e2df322b33c729099ab52b5879d06b00818cce57c649a66ed7eb6d8ae214d11caf9c81e83a7368cf0edb2b71dad791f13fecf546123b40377851e67835ade1d6be57f4de18a62db4cdb1880f4ab2e6a29acfd85ca22a13dc1f6fee2621ef0fc8689cd738e6f065c033ec7c148d8d348688af83d6f6bd", 16);
fp_read_radix(&c, "9ff70ea6968a04530e6b06bf01aa937209cc8450e76ac19477743de996ba3fb445923c947f8d0add8c57efa51d15485309918459da6c1e5a97f215193b797dce98db51bdb4639c2ecfa90ebb051e3a2daeffd27a7d6e62043703a7b15e0ada5170427b63099cd01ef52cd92d8723e5774bea32716aaa7f5adbae817fb12a5b50", 16);
/* test it */
fp_exptmod(&m, &e, &n, &e_m);
if (fp_cmp(&e_m, &c)) {
char buf[1024];
printf("Encrypted text not equal\n");
fp_toradix(&e_m, buf, 16);
printf("e_m == %s\n", buf);
return 0;
}
printf("CLOCKS_PER_SEC = %llu\n", (unsigned long long)CLOCKS_PER_SEC);
t1 = clock();
for (x = 0; x < 1000; x++) {
fp_exptmod(&m, &e, &n, &e_m);
}
t1 = clock() - t1;
printf("1000 RSA operations took %10.5g seconds\n", (double)t1 / (double)CLOCKS_PER_SEC);
printf("RSA encrypt/sec %10.5g\n", (double)CLOCKS_PER_SEC / ((double)t1 / 1000.0) );
/* read in the parameters */
fp_read_radix(&n, "a7f30e2e04d31acc6936916af1e404a4007adfb9e97864de28d1c7ba3034633bee2cd9d5da3ea3cdcdc9a6f3daf5702ef750f4c3aadb0e27410ac04532176795995148cdb4691bd09a8a846e3e24e073ce2f89b34dfeb2ee89b646923ca60ee3f73c4d5397478380425e7260f75dfdc54826e160395b0889b1162cf115a9773f", 16);
fp_read_radix(&d, "16d166f3c9a404d810d3611e6e8ed43293fe1db75c8906eb4810785a4b82529929dade1db7f11ac0335d5a59773e3167b022479eedefa514a0399db5c900750a56323cf9f5b0f21e7d60a46d75f3fcaabf30a63cbe34048b741a57ac36a13914afda798709dea5771f8d456cf72ec5f3afc1d88d023de40311143a36e7028739", 16);
fp_read_radix(&c, "7d216641c32543f5b8428bdd0b11d819cfbdb16f1df285247f677aa4d44de62ab064f4a0d060ec99cb94aa398113a4317f2c550d0371140b0fd2c88886cac771812e72faad4b7adf495b9b850b142ccd7f45c0a27f164c8c7731731c0015f69d0241812e769d961054618aeb9e8e8989dba95714a2cf56c9e525c5e34b5812dd", 16);
fp_read_radix(&m, "5f323bf0b394b98ffd78727dc9883bb4f42287def6b60fa2a964b2510bc55d61357bf5a6883d2982b268810f8fef116d3ae68ebb41fd10d65a0af4bec0530eb369f37c14b55c3be60223b582372fb6589b648d5a0c7252d1ae2dae5809785d993e9e5d0c4d9b0bcba0cde0d6671734747fba5483c735e1dab7df7b10ec6f62d8", 16);
/* test it */
fp_exptmod(&c, &d, &n, &e_m);
if (fp_cmp(&e_m, &m)) {
char buf[1024];
printf("Decrypted text not equal\n");
fp_toradix(&e_m, buf, 16);
printf("e_m == %s\n", buf);
return 0;
}
t1 = clock();
for (x = 0; x < 100; x++) {
fp_exptmod(&c, &d, &n, &e_m);
}
t1 = clock() - t1;
printf("100 RSA operations took %10.5g seconds\n", (double)t1 / (double)CLOCKS_PER_SEC);
printf("RSA decrypt/sec %10.5g\n", (double)CLOCKS_PER_SEC / ((double)t1 / 100.0) );
/* test half size */
fp_rshd(&n, n.used >> 1);
fp_rshd(&d, d.used >> 1);
fp_rshd(&c, c.used >> 1);
printf("n.used == %4d bits\n", n.used * DIGIT_BIT);
/* ensure n is odd */
n.dp[0] |= 1;
t1 = clock();
for (x = 0; x < 100; x++) {
fp_exptmod(&c, &d, &n, &e_m);
}
t1 = clock() - t1;
printf("100 RSA-half operations took %10.5g seconds\n", (double)t1 / (double)CLOCKS_PER_SEC);
printf("RSA decrypt/sec %10.5g (estimate of RSA-1024-CRT) \n", (double)CLOCKS_PER_SEC / ((double)t1 / 50.0) );
return 0;
}
static int
tfm_rsa_generate_key(RSA *rsa, int bits, BIGNUM *e, BN_GENCB *cb)
{
fp_int el, p, q, n, d, dmp1, dmq1, iqmp, t1, t2, t3;
int counter, ret, bitsp;
if (bits < 789)
return -1;
bitsp = (bits + 1) / 2;
ret = -1;
fp_init_multi(&el, &p, &q, &n, &n, &d, &dmp1, &dmq1, &iqmp, &t1, &t2, &t3, NULL);
BN2mpz(&el, e);
/* generate p and q so that p != q and bits(pq) ~ bits */
counter = 0;
do {
BN_GENCB_call(cb, 2, counter++);
CHECK(random_num(&p, bitsp), 0);
CHECK(fp_find_prime(&p), FP_YES);
fp_sub_d(&p, 1, &t1);
fp_gcd(&t1, &el, &t2);
} while(fp_cmp_d(&t2, 1) != 0);
BN_GENCB_call(cb, 3, 0);
counter = 0;
do {
BN_GENCB_call(cb, 2, counter++);
CHECK(random_num(&q, bits - bitsp), 0);
CHECK(fp_find_prime(&q), FP_YES);
if (fp_cmp(&p, &q) == 0) /* don't let p and q be the same */
continue;
fp_sub_d(&q, 1, &t1);
fp_gcd(&t1, &el, &t2);
} while(fp_cmp_d(&t2, 1) != 0);
/* make p > q */
if (fp_cmp(&p, &q) < 0) {
fp_int c;
fp_copy(&p, &c);
fp_copy(&q, &p);
fp_copy(&c, &q);
}
BN_GENCB_call(cb, 3, 1);
/* calculate n, n = p * q */
fp_mul(&p, &q, &n);
/* calculate d, d = 1/e mod (p - 1)(q - 1) */
fp_sub_d(&p, 1, &t1);
fp_sub_d(&q, 1, &t2);
fp_mul(&t1, &t2, &t3);
fp_invmod(&el, &t3, &d);
/* calculate dmp1 dmp1 = d mod (p-1) */
fp_mod(&d, &t1, &dmp1);
/* calculate dmq1 dmq1 = d mod (q-1) */
fp_mod(&d, &t2, &dmq1);
/* calculate iqmp iqmp = 1/q mod p */
fp_invmod(&q, &p, &iqmp);
/* fill in RSA key */
rsa->e = mpz2BN(&el);
rsa->p = mpz2BN(&p);
rsa->q = mpz2BN(&q);
rsa->n = mpz2BN(&n);
rsa->d = mpz2BN(&d);
rsa->dmp1 = mpz2BN(&dmp1);
rsa->dmq1 = mpz2BN(&dmq1);
rsa->iqmp = mpz2BN(&iqmp);
ret = 1;
out:
fp_zero_multi(&el, &p, &q, &n, &d, &dmp1,
&dmq1, &iqmp, &t1, &t2, &t3, NULL);
return ret;
}
static int
tfm_rsa_private_decrypt(int flen, const unsigned char* from,
unsigned char* to, RSA* rsa, int padding)
{
unsigned char *ptr;
int res;
int size;
fp_int in, out, n, e;
if (padding != RSA_PKCS1_PADDING)
return -1;
size = RSA_size(rsa);
if (flen > size)
return -2;
fp_init_multi(&in, &out, NULL);
BN2mpz(&n, rsa->n);
BN2mpz(&e, rsa->e);
fp_read_unsigned_bin(&in, rk_UNCONST(from), flen);
if(fp_isneg(&in) || fp_cmp(&in, &n) >= 0) {
size = -2;
goto out;
}
if (rsa->p && rsa->q && rsa->dmp1 && rsa->dmq1 && rsa->iqmp) {
fp_int p, q, dmp1, dmq1, iqmp;
BN2mpz(&p, rsa->p);
BN2mpz(&q, rsa->q);
BN2mpz(&dmp1, rsa->dmp1);
BN2mpz(&dmq1, rsa->dmq1);
BN2mpz(&iqmp, rsa->iqmp);
res = tfm_rsa_private_calculate(&in, &p, &q, &dmp1, &dmq1, &iqmp, &out);
fp_zero_multi(&p, &q, &dmp1, &dmq1, &iqmp, NULL);
if (res != 0) {
size = -3;
goto out;
}
} else {
fp_int d;
if(fp_isneg(&in) || fp_cmp(&in, &n) >= 0)
return -4;
BN2mpz(&d, rsa->d);
res = fp_exptmod(&in, &d, &n, &out);
fp_zero(&d);
if (res != 0) {
size = -5;
goto out;
}
}
ptr = to;
{
size_t ssize;
ssize = fp_unsigned_bin_size(&out);
assert(size >= ssize);
fp_to_unsigned_bin(&out, ptr);
size = ssize;
}
/* head zero was skipped by mp_int_to_unsigned */
if (*ptr != 2) {
size = -6;
goto out;
}
size--; ptr++;
while (size && *ptr != 0) {
size--; ptr++;
}
if (size == 0)
return -7;
size--; ptr++;
memmove(to, ptr, size);
out:
fp_zero_multi(&e, &n, &in, &out, NULL);
return size;
}
/* 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;
}
static int
tfm_rsa_public_decrypt(int flen, const unsigned char* from,
unsigned char* to, RSA* rsa, int padding)
{
unsigned char *p;
int res;
size_t size;
fp_int s, us, n, e;
if (padding != RSA_PKCS1_PADDING)
return -1;
if (flen > RSA_size(rsa))
return -2;
BN2mpz(&n, rsa->n);
BN2mpz(&e, rsa->e);
#if 0
/* Check that the exponent is larger then 3 */
if (mp_int_compare_value(&e, 3) <= 0) {
fp_zero_multi(&e, &n, NULL);
return -3;
}
#endif
fp_init_multi(&s, &us, NULL);
fp_read_unsigned_bin(&s, rk_UNCONST(from), flen);
if (fp_cmp(&s, &n) >= 0) {
fp_zero_multi(&e, &n, NULL);
return -4;
}
res = fp_exptmod(&s, &e, &n, &us);
fp_zero_multi(&s, &e, &n, NULL);
if (res != 0)
return -5;
p = to;
size = fp_unsigned_bin_size(&us);
assert(size <= RSA_size(rsa));
fp_to_unsigned_bin(&us, p);
fp_zero(&us);
/* head zero was skipped by fp_to_unsigned_bin */
if (*p == 0)
return -6;
if (*p != 1)
return -7;
size--; p++;
while (size && *p == 0xff) {
size--; p++;
}
if (size == 0 || *p != 0)
return -8;
size--; p++;
memmove(to, p, size);
return size;
}
static int
tfm_rsa_private_encrypt(int flen, const unsigned char* from,
unsigned char* to, RSA* rsa, int padding)
{
unsigned char *p, *p0;
int res;
int size;
fp_int in, out, n, e;
if (padding != RSA_PKCS1_PADDING)
return -1;
size = RSA_size(rsa);
if (size < RSA_PKCS1_PADDING_SIZE || size - RSA_PKCS1_PADDING_SIZE < flen)
return -2;
p0 = p = malloc(size);
*p++ = 0;
*p++ = 1;
memset(p, 0xff, size - flen - 3);
p += size - flen - 3;
*p++ = 0;
memcpy(p, from, flen);
p += flen;
assert((p - p0) == size);
BN2mpz(&n, rsa->n);
BN2mpz(&e, rsa->e);
fp_init_multi(&in, &out, NULL);
fp_read_unsigned_bin(&in, p0, size);
free(p0);
if(fp_isneg(&in) || fp_cmp(&in, &n) >= 0) {
size = -3;
goto out;
}
if (rsa->p && rsa->q && rsa->dmp1 && rsa->dmq1 && rsa->iqmp) {
fp_int p, q, dmp1, dmq1, iqmp;
BN2mpz(&p, rsa->p);
BN2mpz(&q, rsa->q);
BN2mpz(&dmp1, rsa->dmp1);
BN2mpz(&dmq1, rsa->dmq1);
BN2mpz(&iqmp, rsa->iqmp);
res = tfm_rsa_private_calculate(&in, &p, &q, &dmp1, &dmq1, &iqmp, &out);
fp_zero_multi(&p, &q, &dmp1, &dmq1, &iqmp, NULL);
if (res != 0) {
size = -4;
goto out;
}
} else {
fp_int d;
BN2mpz(&d, rsa->d);
res = fp_exptmod(&in, &d, &n, &out);
fp_zero(&d);
if (res != 0) {
size = -5;
goto out;
}
}
if (size > 0) {
size_t ssize;
ssize = fp_unsigned_bin_size(&out);
assert(size >= ssize);
fp_to_unsigned_bin(&out, to);
size = ssize;
}
out:
fp_zero_multi(&e, &n, &in, &out, NULL);
return size;
}
int fp3_cmp(fp3_t a, fp3_t b) {
return (fp_cmp(a[0], b[0]) == CMP_EQ) && (fp_cmp(a[1], b[1]) == CMP_EQ) &&
(fp_cmp(a[2], b[2]) == CMP_EQ) ? CMP_EQ : CMP_NE;
}
/* c = 1/a (mod b) for odd b only */
int fp_invmod(fp_int *a, fp_int *b, fp_int *c)
{
fp_int x, y, u, v, B, D;
int neg;
/* 2. [modified] b must be odd */
if (fp_iseven (b) == FP_YES) {
return fp_invmod_slow(a,b,c);
}
/* init all our temps */
fp_init(&x); fp_init(&y);
fp_init(&u); fp_init(&v);
fp_init(&B); fp_init(&D);
/* x == modulus, y == value to invert */
fp_copy(b, &x);
/* we need y = |a| */
fp_abs(a, &y);
/* 3. u=x, v=y, A=1, B=0, C=0,D=1 */
fp_copy(&x, &u);
fp_copy(&y, &v);
fp_set (&D, 1);
top:
/* 4. while u is even do */
while (fp_iseven (&u) == FP_YES) {
/* 4.1 u = u/2 */
fp_div_2 (&u, &u);
/* 4.2 if B is odd then */
if (fp_isodd (&B) == FP_YES) {
fp_sub (&B, &x, &B);
}
/* B = B/2 */
fp_div_2 (&B, &B);
}
/* 5. while v is even do */
while (fp_iseven (&v) == FP_YES) {
/* 5.1 v = v/2 */
fp_div_2 (&v, &v);
/* 5.2 if D is odd then */
if (fp_isodd (&D) == FP_YES) {
/* D = (D-x)/2 */
fp_sub (&D, &x, &D);
}
/* D = D/2 */
fp_div_2 (&D, &D);
}
/* 6. if u >= v then */
if (fp_cmp (&u, &v) != FP_LT) {
/* u = u - v, B = B - D */
fp_sub (&u, &v, &u);
fp_sub (&B, &D, &B);
} else {
/* v - v - u, D = D - B */
fp_sub (&v, &u, &v);
fp_sub (&D, &B, &D);
}
/* if not zero goto step 4 */
if (fp_iszero (&u) == FP_NO) {
goto top;
}
/* now a = C, b = D, gcd == g*v */
/* if v != 1 then there is no inverse */
if (fp_cmp_d (&v, 1) != FP_EQ) {
return FP_VAL;
}
/* b is now the inverse */
neg = a->sign;
while (D.sign == FP_NEG) {
fp_add (&D, b, &D);
}
fp_copy (&D, c);
c->sign = neg;
return FP_OKAY;
}
