#include <avr/io.h>

#define MesSize 16

void DivideByTwo ( uint8_t *x )
{
//Divides multiple-precision integer x by 2 by shifting to right by one bit
uint16_t d=0;
uint8_t i;

for ( i=0; i<MesSize; i++ )
{
d |= x[i];
x[i] = ( ( d >> 1 ) & 0xFF );
if ( d & 0x01)
d = 0x100;
else
d = 0;
}
}

void aSubtract ( uint8_t *a, uint8_t *b )
{
//Computes a = a - b
uint8_t i;
uint8_t borrow=0;
int16_t d;

for ( i=MesSize-1; i>=0; i-- )
{
d = a[i] - b[i] - borrow;
if ( d < 0 )
{
d = d + 0x100;
borrow = 1;
}
else
borrow = 0;
a[i] = d & 0xFF;
}
}

void aModAdd ( uint8_t *a, uint8_t *b, uint8_t *m )
{
//Computes a = (a + b) mod m
uint8_t i;
uint16_t d=0;
uint8_t tmp=1;

//1. Add a = a + b
for ( i=MesSize-1; i>=0; i-- )
{
d = a[i] + b[i] + d;
a[i] = d & 0xFF;
d >>= 8;
}
//2. If a > m then a = a - m
while ( tmp )
{
for ( i=0; i<MesSize; i++ )
{
if ( a[i] != m[i] )
break;
}
if ( a[i] >= m[i] )
aSubtract( a, m );
else
tmp=0;
}
}

uint8_t aIsZero ( uint8_t *a )
{
//Returns true if a is zero
uint8_t i;

for ( i=MesSize-1; i>=0; i-- )
{
if ( a[i] != 0 )
return 0;
}

return 1;
}

void aModMult ( uint8_t *w, uint8_t *abX, uint8_t *abY, uint8_t *abMod )
{
//Returns w = (x * y) mod m
uint8_t x[MesSize];
uint8_t y[MesSize];
uint16_t nBits;
uint8_t i;

//1. Set w = 0, and temps x = abX, y = abY
for ( i=0; i<MesSize; i++ )
{
x[i] = abX[i];
y[i] = abY[i];
w[i] = 0;
}
//2. From LS bit to MS bit of X do:
for ( nBits=MesSize*8; nBits>0; nBits-- )
{
//2.1 if x is odd then w = (w + y) mod m
if ( ( x[MesSize - 1] & 0x01 ) != 0 )
aModAdd ( w, y, abMod );
//2.2 x = x / 2
DivideByTwo ( x );
//2.3 if x != 0 then y = (y + y) mod m
if ( aIsZero ( x ) )
break;
aModAdd( y, y, abMod );
}
}

void aModExp ( uint8_t *a, uint8_t *abBase, uint8_t *abExponent, uint8_t *abModulus )
{
//Computes a = b^e mod m and returns the result in a byte array as a VARIANT
uint8_t e[MesSize];
uint8_t s[MesSize];
uint16_t nBits;
uint8_t i;

//Perform right-to-left binary exponentiation
//1. Set A = 1, S = b
//NB s and e are trashed so use copies
for ( i=0; i<MesSize; i++ )
{
s[i] = abBase[i];
e[i] = abExponent[i];
a[i] = 0;
}
a[MesSize-1] = 1;
//2. While e != 0 do:
for ( nBits=MesSize*8; nBits>1; nBits-- )
{
//2.1 if e is odd then A = A*S mod m
if ( ( e[MesSize - 1] & 0x01) != 0 )
aModMult ( a, a, s, abModulus );
//2.2 e = e / 2
DivideByTwo ( e );
//2.3 if e != 0 then S = S*S mod m
if ( aIsZero( e ) )
break;
aModMult( s, s, s, abModulus );
}
}

int main ( int argc, char *argv[] )
{
uint8_t rsa_mod[MesSize];
uint8_t rsa_key[MesSize];
uint8_t in_mes[MesSize];
uint8_t out_mes[MesSize];


aModExp ( out_mes, in_mes, rsa_key, rsa_mod );

return 0;
}