#include <idea.h>

#define u16 IDEA__U16
#define u32 IDEA__U32

/*
Compute that number m such that n*m=1 mod 0x10001.

This is done by computing a sequence of Ai, Xi, Bi, Yi as follows.
(Let K = 0x10001.)
  Initial state:
    A0 = 1
    X0 = K
    B0 = 0
    Y0 = n
  Important loop invariants:
    Ai Xi + Bi Yi = K
    K | (Y0 Bi + Xi)
    K | (Y0 Ai - Yi)
    Xi != Yi
    GCD(Xi,Yi) = 1
    Xi != 0
    Yi != 0
  Computation (j = i+1):
    if Xi=1, stop; return -Bi (mod K), since
      K | Y0 Bi + Xi = n Bi + 1, so n Bi = -1 mod K, so n (-Bi) = 1 mod K.
    if Yi=1, stop; return Ai (mod K), since
      K | Y0 Ai - Yi = n Ai - 1, so n Ai = 1 mod K
    if Xi > Yi:
      Xi / Yi = Qi remainder Ri (that is, Xi = Yi Qi + Ri, 0 <= Ri < Yi)
      Aj = Ai
      Xj = Ri
      Bj = Bi + Qi Ai
      Yj = Yi
    else (Xi < Yi)
      Yi / Xi = Qi remainder Ri (that is, Yi = Xi Qi + Ri, 0 <= Ri < Xi)
      Aj = Ai + Qi Bi
      Xj = Xi
      Bj = Bi
      Yj = Ri
Loop invariant preservation proofs:
  First case:
    Aj Xj + Bj Yj =
      Ai Ri + (Bi + Qi Ai) Yi =
      Ai Ri + Bi Yi + Qi Ai Yi =
      Ai (Ri + Qi Yi) + Bi Yi =
      Ai Xi + Bi Yi.
    Y0 Bj + Xj =
      Y0 (Bi + Qi Ai) + Ri =
      Y0 Bi + Y0 Qi Ai + Ri =
      Y0 Bi + Yi Qi + Ri + Y0 Qi Ai - Yi Qi =
      Y0 Bi + (Yi Qi + Ri) + (Y0 Qi Ai - Yi Qi) =
      Y0 Bi + Xi + Qi (Y0 Ai - Yi) =
      Y0 Bi + Xi + a multiple of K.
    Y0 Aj - Yj = Y0 Ai - Yi.
    Xj != Yj since Ri strictly < Yi.
    if GCD(Xj,Yj) = Gj, Gj|Xj and Gj|Yj, so Gj|Ri and Gj|Yi,
      so Gj|(Ri+QiYi), so Gj|Xi, so Gj|GCD(Xi,Yi), so Gj=1.
    Xj != 0 since Xj = Ri, and if Ri=0, then Yi|Xi.  But if GCD(Xi,Yi)=1
      and Yi|Xi, then Yi=1, which is a stop condition.
    Yj != 0 since Yj = Yi.
  Second case:
    Aj Xj + Bj Yj =
      (Ai + Qi Bi) Xi + Bi Ri =
      Ai Xi + Qi Bi Xi + Bi Ri =
      Ai Xi + Bi (Qi Xi + Ri) =
      Ai Xi + Bi Yi.
    Y0 Bj + Xj = Y0 Bi Xi.
    Y0 Aj - Yj =
      Y0 (Ai + Qi Bi) - Ri =
      Y0 Ai + Y0 Qi Bi - Ri =
      Y0 Ai - Xi Qi - Ri + Y0 Qi Bi + Xi Qi =
      Y0 Ai - (Xi Qi + Ri) + (Y0 Qi Bi + Xi Qi) =
      Y0 Ai - Yi + Qi (Y0 Bi + Xi) =
      Y0 Ai - Yi + a multiple of K.
    Xj != Yj since Ri strictly < Yi.
    if GCD(Xj,Yj) = Gj, Gj|Xj and Gj|Yj, so Gj|Xi and Gj|Ri,
      so Gj|(Ri+QiXi), so Gj|Yi, so Gj|GCD(Xi,Yi), so Gj=1.
    Xj != 0 since Xj = Xi.
    Yj != 0 since Yj = Ri, and if Ri=0, then Xi|Yi.  But if GCD(Xi,Yi)=1
      and Xi|Yi, then Xi=1, which is a stop condition.

Noting that the computation will always take alternate branches of the
Xi<Yi condition each time through, we unroll the loop and don't bother
doing the test (since we know Y0<X0).  We also do the first iteration
of the loop "by hand", as part of the initial setup, so that values can
be guaranteed to fit in a u16.  (Arguments 0=0x10000 and 1 are
special-cased.)  Note also that A and B are always < K (because of the
first loop invariant, and X and Y are always at least 1), so the
reduction mod K in the final return is trivial.  Also, R gets trivially
optimized away.
*/

static inline u16 multinv(u16) __attribute__ ((__const__));
static inline u16 multinv(u16 n)
{
 u16 a;
 u16 x;
 u16 b;
 u16 y;
 u16 q;

 if (n < 2) return(n);
 a = 1;
 x = 0x10001 % n;
 b = 0x10001 / n;
 y = n;
 while (1)
  { if (x == 1) return(0x10001-b);
    q = y / x;
    y %= x;
    a += q * b;
    if (y == 1) return(a);
    q = x / y;
    x %= y;
    b += q * a;
  }
}

#define mneg(n) (0xffff&(1U-(u16)(n)))
#define addinv(n) (0xffff&-(u16)(n))

static inline u16 mult(u16, u16) __attribute__ ((__const__));
static inline u16 mult(u16 a, u16 b)
{
 u32 p;

 if (! a) return(0xffff&(1-b));
 if (! b) return(0xffff&(1-a));
 p = a * (u32)b;
 return(0xffff&(p%0x10001)); /* XXX */
}

#define add(a,b) (0xffff&(((u16)(a))+((u16)(b))))

#define xor(a,b) (((u16)(a))^((u16)(b)))

void idea_setkey_e(const void *key, IDEA_KEY *ks)
{
 u32 kb[4];
 u32 kt;
 u16 *zp;
 int i;

 for (i=0;i<4;i++)
  { kb[i] = ((((const unsigned char *)key)[(i*4)  ] & 0xff) << 24) |
	    ((((const unsigned char *)key)[(i*4)+1] & 0xff) << 16) |
	    ((((const unsigned char *)key)[(i*4)+2] & 0xff) <<  8) |
	    ((((const unsigned char *)key)[(i*4)+3] & 0xff)      );
  }
 zp = &ks->Z[0];
 for (i=0;i<6;i++)
  { *zp++ = (kb[0] >> 16) & 0xffff;
    *zp++ = kb[0] & 0xffff;
    *zp++ = (kb[1] >> 16) & 0xffff;
    *zp++ = kb[1] & 0xffff;
    *zp++ = (kb[2] >> 16) & 0xffff;
    *zp++ = kb[2] & 0xffff;
    *zp++ = (kb[3] >> 16) & 0xffff;
    *zp++ = kb[3] & 0xffff;
    kt = kb[0];
    kb[0] = ((kt    << 25) | (kb[1] >> 7)) & 0xffffffff;
    kb[1] = ((kb[1] << 25) | (kb[2] >> 7)) & 0xffffffff;
    kb[2] = ((kb[2] << 25) | (kb[3] >> 7)) & 0xffffffff;
    kb[3] = ((kb[3] << 25) | (kt    >> 7)) & 0xffffffff;
  }
 *zp++ = (kb[0] >> 16) & 0xffff;
 *zp++ = kb[0] & 0xffff;
 *zp++ = (kb[1] >> 16) & 0xffff;
 *zp++ = kb[1] & 0xffff;
}

void idea_setkey_d(const void *key, IDEA_KEY *ks)
{
 idea_setkey_e(key,ks);
 idea_keycvt_e_to_d(ks,ks);
}

void idea_keycvt_e_to_d(const IDEA_KEY *kse, IDEA_KEY *ksd)
{
 IDEA_KEY k;
 int i;
 u16 *fp;
 u16 *tp;

 k = *kse;
 fp = &k.Z[0];
 tp = &ksd->Z[48];
 for (i=0;i<52;i++) ksd->Z[i] = 0;
 tp[0] = multinv(fp[0]);
 tp[1] = addinv(fp[1]);
 tp[2] = addinv(fp[2]);
 tp[3] = multinv(fp[3]);
 for (i=7;i>0;i--,fp+=6,tp-=6)
  { tp[-1] = fp[5];
    tp[-2] = fp[4];
    tp[-3] = multinv(fp[9]);
    tp[-4] = addinv(fp[7]);
    tp[-5] = addinv(fp[8]);
    tp[-6] = multinv(fp[6]);
  }
 tp[-1] = fp[5];
 tp[-2] = fp[4];
 tp[-3] = multinv(fp[9]);
 tp[-4] = addinv(fp[8]);
 tp[-5] = addinv(fp[7]);
 tp[-6] = multinv(fp[6]);
}

extern void idea_keycvt_d_to_e(const IDEA_KEY *ksd, IDEA_KEY *kse)
{
 /* This happens to be the same transformation as ..._e_to_d */
 idea_keycvt_e_to_d(ksd,kse);
}

/*
 * Basic encryption block [(+)=add, (*)=mult, (x)=xor]
 *     A         B         C         D
 *     |         |         |         |
 *    (*)--------|---------|---------|------------ ks1
 *     |        (+)--------|---------|------------ ks2
 *     |         |        (+)--------|------------ ks3
 *     |         |         |        (*)----------- ks4
 *     +---(x)---|---------+         |
 *     |    |    +---------|---(x)---+
 *     |   (*)---|---------|----|----|------------ ks5
 *     |    +----|---------|---(+)   |
 *     |    |    |         |   (*)---|------------ ks6
 *     |   (+)---|---------|----+    |
 *    (x)---|----|---------|----+    |
 *     |    |    |        (x)---+    |
 *     |    +---(x)        |         |
 *     |    +----|---------|--------(x)
 *     |         |         |         |
 *     |           \     /           |
 *     |              X              |
 *     |           /     \           |
 *     |         |         |         |
 * Encryption includes eight of these rounds, followed by a mini-round
 *  including just the first four operations.  Decryption is the same
 *  as encryption, except the key schedule is shuffled around and the
 *  values are switched with their multiplicative or additive inverses,
 *  depending on whether the element participates in a multiply or an
 *  add.
 */
void idea_crypt(const void *blkin, const IDEA_KEY *k, void *blkout)
{
 int r;
 u16 x1;
 u16 x2;
 u16 x3;
 u16 x4;
 u16 s1;
 u16 s2;
 u16 s3;
 u16 s4;
 u16 s5;
 u16 s6;
 u16 s7;
 u16 s8;
 u16 s9;
 u16 s10;
 u16 s11;
 u16 s12;
 u16 s13;
 u16 s14;
 const u16 *zp;

 x1 = (((const unsigned char *)blkin)[0] << 8) |
      (((const unsigned char *)blkin)[1]     );
 x2 = (((const unsigned char *)blkin)[2] << 8) |
      (((const unsigned char *)blkin)[3]     );
 x3 = (((const unsigned char *)blkin)[4] << 8) |
      (((const unsigned char *)blkin)[5]     );
 x4 = (((const unsigned char *)blkin)[6] << 8) |
      (((const unsigned char *)blkin)[7]     );
 zp = &k->Z[0];
 for (r=8;r>0;r--)
  { s1 = mult(x1,*zp++);
    s2 = add(x2,*zp++);
    s3 = add(x3,*zp++);
    s4 = mult(x4,*zp++);
    s5 = xor(s1,s3);
    s6 = xor(s2,s4);
    s7 = mult(s5,*zp++);
    s8 = add(s6,s7);
    s9 = mult(s8,*zp++);
    s10 = add(s7,s9);
    s11 = xor(s1,s9);
    s12 = xor(s3,s9);
    s13 = xor(s2,s10);
    s14 = xor(s4,s10);
    x1 = s11;
    x2 = s12;
    x3 = s13;
    x4 = s14;
  }
  { u16 t; t=x2; x2=x3; x3=t;
  }
 x1 = mult(x1,*zp++);
 x2 = add(x2,*zp++);
 x3 = add(x3,*zp++);
 x4 = mult(x4,*zp++);
 ((unsigned char *)blkout)[0] = x1 >> 8;
 ((unsigned char *)blkout)[1] = x1 & 0xff;
 ((unsigned char *)blkout)[2] = x2 >> 8;
 ((unsigned char *)blkout)[3] = x2 & 0xff;
 ((unsigned char *)blkout)[4] = x3 >> 8;
 ((unsigned char *)blkout)[5] = x3 & 0xff;
 ((unsigned char *)blkout)[6] = x4 >> 8;
 ((unsigned char *)blkout)[7] = x4 & 0xff;
}