#ifdef __VSDSP__
#include <vstypes.h>
#else
#include <inttypes.h>
typedef uint32_t u_int32;
typedef uint16_t u_int16;
#define __mem_y
#define memcpyYX memcpy
#endif
#include <stdio.h>
#include <string.h>

#define CHACHA20_TEST
#include "chacha20.h"

u_int16 testQuarterRoll() {
  static u_int32 tv[] = {
    0x11111111, 0x01020304,
    0x9b8d6f43, 0x01234567
  };
  static u_int32 rv[] = {
    0xea2a92f4, 0xcb1cf8ce,
    0x4581472e, 0x5881c4bb
  };
  u_int16 i, fails = 0;
  QuarterRoll(tv, tv + 1, tv + 2, tv + 3);
  printf("Quarter roll test:\n");
  for (i = 0; i < 4; i++) {
    if (tv[i] != rv[i]) {
      printf("\tFailed %u: Got:"
#ifdef __VSDSP__
             "0x%08lx != 0x%08lx should\n"
#else
             "0x%08x != 0x%08x should\n"
#endif
             , i, tv[i], rv[i]);
      fails++;
    }
  }
  if (fails)printf("%u FAILS\n", fails);
  else printf("PASSED\n");
  return fails;
}

u_int16 testSetKey() {
  static char tv[12*4];
  static u_int32 kv[12];
  static u_int32 rv[] = {
    0x61707865, 0x3320646e, 0x79622d32, 0x6b206574,
    0x03020100, 0x07060504, 0x0b0a0908, 0x0f0e0d0c,
    0x13121110, 0x17161514, 0x1b1a1918, 0x1f1e1d1c,
    0x00000001, 0x09000000, 0x4a000000, 0x00000000
  };
  u_int32 *st = chachaState, *r = rv;
  u_int16 i, fails = 0;
  /* Key */
  for (i = 0; i < 32; i++) {
    tv[i] = i;
  }
  /* Counter */
  tv[32] = 0x01;
  tv[33] = 0x00;
  tv[34] = 0x00;
  tv[35] = 0x00;
  /* Nonce */
  tv[36] = 0x00;
  tv[37] = 0x00;
  tv[38] = 0x00;
  tv[39] = 0x09;

  tv[40] = 0x00;
  tv[41] = 0x00;
  tv[42] = 0x00;
  tv[43] = 0x4a;

  tv[44] = 0x00;
  tv[45] = 0x00;
  tv[46] = 0x00;
  tv[47] = 0x00;
  for (i = 0; i < 12;i++) {
    kv[i] =
      (tv[i * 4 + 3] << 24)|
      (tv[i * 4 + 2] << 16)|
      (tv[i * 4 + 1] <<  8)|
      (tv[i * 4 + 0] <<  0);
    /* printf("0x%08x%s", kv[i], (i + 1)%4?", ":",\n"); */
  }
  Chacha20SetKey(kv, kv[8], kv + 9);
  printf("Set key test:\n");
  for (i = 0; i < 16; i++) {
    if (*st != *r) {
      printf("\tFailed %u: Got:"
#ifdef __VSDSP__
             "0x%08lx != 0x%08lx should\n"
#else
             "0x%08x != 0x%08x should\n"
#endif
             , i, *st, *r);
      fails++;
    }
    st++;
    r++;
  }
  if (fails)printf("%u FAILS\n", fails);
  else printf("PASSED\n");
  return fails;
}
u_int16 testChacha20Block() {
  u_int16 i, fails = 0;
  static u_int32 resultStateWork[] = {
    0x837778ab, 0xe238d763, 0xa67ae21e, 0x5950bb2f,
    0xc4f2d0c7, 0xfc62bb2f, 0x8fa018fc, 0x3f5ec7b7,
    0x335271c2, 0xf29489f3, 0xeabda8fc, 0x82e46ebd,
    0xd19c12b4, 0xb04e16de, 0x9e83d0cb, 0x4e3c50a2
  };
  static u_int32 resultState[] = {
    0xe4e7f110, 0x15593bd1, 0x1fdd0f50, 0xc47120a3,
    0xc7f4d1c7, 0x0368c033, 0x9aaa2204, 0x4e6cd4c3,
    0x466482d2, 0x09aa9f07, 0x05d7c214, 0xa2028bd9,
    0xd19c12b5, 0xb94e16de, 0xe883d0cb, 0x4e3c50a2
  };
  u_int32 *rsw = resultStateWork, *rs = resultState,
    *st = chachaState, *ws = chachaWork;
  Chacha20Block();
  printf("Chacha20 block test:\n");
  for (i = 0; i < 16; i++) {
    /* State is saved and only counter is updated. in encryption.
     * Work is copied from state, rolled and then old state is added to it.
     * Next block can then be encrypted by updating counter from state.
     */
    /*
    if (*rsw != *ws) {
      printf("\tFailed work %u: Got:"
#ifdef __VSDP__
             "0x%08lx != 0x%08lx should\n"
#else
             "0x%08x != 0x%08x should\n"
#endif
             , i, *ws, *rsw);
      fails++;
    }
    */
    if (*ws != *rs) {
      printf("\tFailed state %u: Got:"
#ifdef __VSDSP__
             "0x%08lx != 0x%08lx should\n"
#else
             "0x%08x != 0x%08x should\n"
#endif
             , i, *st, *rs);
      fails++;
    }
    st++;
    rs++;
    ws++;
    rsw++;
  }
  if (fails)printf("%u FAILS\n", fails);
  else printf("PASSED\n");
  return fails;
}

u_int16 testChacha20Encrypt() {
  /* First set key vector.
     Comment out the printf from testSetKey()
     to extarct in right form. And change 0x09 from noce to 0x00
     o  Key = 00:01:02:03:04:05:06:07:08:09:0a:0b:0c:0d:0e:0f:
              10:11:12:13:14:15:16:17:18:19:1a:1b:1c:1d:1e:1f.
   o  Nonce = (00:00:00:00:00:00:00:4a:00:00:00:00).
   o  Initial Counter = 1.
  */
#ifdef __VSDSP__
  u_int32 kv[] = {

  };
#else
   u_int32 kv[] = {
     0x03020100, 0x07060504, 0x0b0a0908, 0x0f0e0d0c,
     0x13121110, 0x17161514, 0x1b1a1918, 0x1f1e1d1c,
     0x00000001, 0x00000000, 0x4a000000, 0x00000000
  };
#endif
   static char plaintext[] =
     "Ladies and Gentlemen of the class of '99: If I c"
     "ould offer you only one tip for the future, suns"
     "creen would be it.";
   static const char chiphertext[] = {
     0x6e, 0x2e, 0x35, 0x9a, 0x25, 0x68, 0xf9, 0x80,
     0x41, 0xba, 0x07, 0x28, 0xdd, 0x0d, 0x69, 0x81,
     0xe9, 0x7e, 0x7a, 0xec, 0x1d, 0x43, 0x60, 0xc2,
     0x0a, 0x27, 0xaf, 0xcc, 0xfd, 0x9f, 0xae, 0x0b,   
     0xf9, 0x1b, 0x65, 0xc5, 0x52, 0x47, 0x33, 0xab,
     0x8f, 0x59, 0x3d, 0xab, 0xcd, 0x62, 0xb3, 0x57,
     0x16, 0x39, 0xd6, 0x24, 0xe6, 0x51, 0x52, 0xab,
     0x8f, 0x53, 0x0c, 0x35, 0x9f, 0x08, 0x61, 0xd8,
     0x07, 0xca, 0x0d, 0xbf, 0x50, 0x0d, 0x6a, 0x61,
     0x56, 0xa3, 0x8e, 0x08, 0x8a, 0x22, 0xb6, 0x5e,
     0x52, 0xbc, 0x51, 0x4d, 0x16, 0xcc, 0xf8, 0x06,
     0x81, 0x8c, 0xe9, 0x1a, 0xb7, 0x79, 0x37, 0x36,
     0x5a, 0xf9, 0x0b, 0xbf, 0x74, 0xa3, 0x5b, 0xe6,
     0xb4, 0x0b, 0x8e, 0xed, 0xf2, 0x78, 0x5e, 0x42,
     0x87, 0x4d
   };
   const char *r = chiphertext;
   char *s = plaintext;
   size_t len = strlen(plaintext);
   u_int16 i, fails = 0;
   Chacha20SetKey(kv, kv[8], kv + 9);
   Chacha20Encrypt(plaintext, len);
   printf("Chacha20 encryption test:\n");
   for (i = 0; i < len; i++) {
     if (*s != *r) {
       printf("\tFailed cipher %u: Got:"
              "0x%02x != 0x%02x should\n"
              , i, *s, *r);
       fails++;
     }
     s++;
     r++;
   }
   if (fails)printf("%u FAILS\n", fails);
   else printf("PASSED\n");

   Chacha20SetKey(kv, kv[8], kv + 9);
   Chacha20Encrypt(plaintext, len);
   printf("Chacha20 decryption test:\n\nPlaintext:\n%s\n\n",
          plaintext);
   
   return fails;

}
u_int16 test2BlockEnc() {
  static char msg[64];
  static u_int32 rv1[] = {
    0xade0b876, 0x903df1a0, 0xe56a5d40, 0x28bd8653,
    0xb819d2bd, 0x1aed8da0, 0xccef36a8, 0xc70d778b,
    0x7c5941da, 0x8d485751, 0x3fe02477, 0x374ad8b8,
    0xf4b8436a, 0x1ca11815, 0x69b687c3, 0x8665eeb2
  };
  static char t1[] = {
    0x76, 0xb8, 0xe0, 0xad, 0xa0, 0xf1, 0x3d, 0x90,
    0x40, 0x5d, 0x6a, 0xe5, 0x53, 0x86, 0xbd, 0x28,
    0xbd, 0xd2, 0x19, 0xb8, 0xa0, 0x8d, 0xed, 0x1a,
    0xa8, 0x36, 0xef, 0xcc, 0x8b, 0x77, 0x0d, 0xc7,
    0xda, 0x41, 0x59, 0x7c, 0x51, 0x57, 0x48, 0x8d,
    0x77, 0x24, 0xe0, 0x3f, 0xb8, 0xd8, 0x4a, 0x37,
    0x6a, 0x43, 0xb8, 0xf4, 0x15, 0x18, 0xa1, 0x1c,
    0xc3, 0x87, 0xb6, 0x69, 0xb2, 0xee, 0x65, 0x86
  };
  static char t2[] = {
    0x9f, 0x07, 0xe7, 0xbe, 0x55, 0x51, 0x38, 0x7a,
    0x98, 0xba, 0x97, 0x7c, 0x73, 0x2d, 0x08, 0x0d,
    0xcb, 0x0f, 0x29, 0xa0, 0x48, 0xe3, 0x65, 0x69,
    0x12, 0xc6, 0x53, 0x3e, 0x32, 0xee, 0x7a, 0xed,
    0x29, 0xb7, 0x21, 0x76, 0x9c, 0xe6, 0x4e, 0x43,
    0xd5, 0x71, 0x33, 0xb0, 0x74, 0xd8, 0x39, 0xd5,
    0x31, 0xed, 0x1f, 0x28, 0x51, 0x0a, 0xfb, 0x45,
    0xac, 0xe1, 0x0a, 0x1f, 0x4b, 0x79, 0x4d, 0x6f
  };
  static u_int32 rv2[] = {
    0xbee7079f, 0x7a385155, 0x7c97ba98, 0x0d082d73,
    0xa0290fcb, 0x6965e348, 0x3e53c612, 0xed7aee32,
    0x7621b729, 0x434ee69c, 0xb03371d5, 0xd539d874,
    0x281fed31, 0x45fb0a51, 0x1f0ae1ac, 0x6f4d794b
  };
  static u_int32 key[] = {
    0, 0, 0, 0, 0, 0, 0, 0
  };
  u_int32 *r, *s;
  u_int16 i, fails = 0;
  char *rt, *t = msg;
  for (i = 0; i < 64; i++) {
    *t++ = 0;
  }
  Chacha20SetKey(key, 0, key);
  Chacha20Encrypt(msg, 64);
  printf("Chacha20 2 block test:\n");
  s = chachaWork;
  r = rv1;
  for (i = 0; i < 16; i++) {
    if (*r != *s) {
       printf("\tFailed cipher state %u: Got:"
#ifdef __VSDSP__
              "0x%08lx != 0x%08lx should\n"
#else
              "0x%08x != 0x%08x should\n"
#endif
              , i, *s, *r);
       fails++;
    }
    s++;
    r++;
  }
  t = msg;
  rt = t1;
  for (i = 0; i < 64;i++) {
    if (*t != *rt) {
       printf("\tFailed cipher %u: Got:"
              "0x%02x != 0x%02x should\n"
              , i, *t, *rt);
       fails++;
     }
    t++;
    rt++;
  }
  t = msg;
  for (i = 0; i < 64; i++) {
    *t++ = 0;
  }
  Chacha20Encrypt(msg, 64);
  s = chachaWork;
  r = rv2;
  for (i = 0; i < 16; i++) {
    if (*r != *s) {
      printf("\tFailed cipher state %u: Got:"
#ifdef __VSDSP__
	     "0x%08lx != 0x%08lx should\n"
#else
	     "0x%08x != 0x%08x should\n"
#endif
	     , i, *s, *r);
      fails++;
    }
    s++;
    r++;
  }
  t = msg;
  rt = t2;
  for (i = 0; i < 64;i++) {
    if (*t != *rt) {
      printf("\tFailed cipher %u: Got:"
	     "0x%02x != 0x%02x should\n"
	     , i, *t, *rt);
      fails++;
    }
    t++;
    rt++;
  }
  if (fails)printf("%u FAILS\n", fails);
  else printf("PASSED\n");
  return fails;
}
int main() {
  if (testQuarterRoll() == 0)
    if (testSetKey() == 0)
      if (testChacha20Block() == 0)
        if (testChacha20Encrypt() == 0)
	  if (test2BlockEnc() == 0)
	    printf("All OK.\n");
  
  printf("Tests done.\n");
  return 0;
}
