Advertisement
aspsda

grain128

May 20th, 2022
1,117
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. uint8_t lfsr[128];
  2. uint8_t nfsr[128];
  3. uint8_t init_rounds = 0;
  4.  
  5. void init_grain( uint8_t *key, uint8_t *iv)
  6. {
  7.   for (int i = 0; i < 12; i++) {
  8.     for (int j = 0; j < 8; j++) {
  9.       lfsr[8 * i + j] = (iv[i] & (1 << (7-j))) >> (7-j);
  10.  
  11.     }
  12.   }
  13.  
  14.   for (int i = 96; i < 128; i++) {
  15.     lfsr[i] = 1;
  16.   }
  17.  
  18.   //lfsr[127] = 0;
  19.  
  20.   for (int i = 0; i < 16; i++) {
  21.     for (int j = 0; j < 8; j++) {
  22.       nfsr[8 * i + j] = (key[i] & (1 << (7-j))) >> (7-j);
  23.     }
  24.   }
  25. }
  26.  
  27. uint8_t next_lfsr_fb()
  28. {
  29.   /* f(x) = 1 + x^32 + x^47 + x^58 + x^90 + x^121 + x^128 */
  30.   return lfsr[96] ^ lfsr[81] ^ lfsr[70] ^ lfsr[38] ^ lfsr[7] ^ lfsr[0];
  31. }
  32.  
  33. uint8_t next_nfsr_fb()
  34. {
  35.   return nfsr[96] ^ nfsr[91] ^ nfsr[56] ^ nfsr[26] ^ nfsr[0] ^ (nfsr[84] & nfsr[68]) ^
  36.       (nfsr[67] & nfsr[3]) ^ (nfsr[65] & nfsr[61]) ^ (nfsr[59] & nfsr[27]) ^
  37.       (nfsr[48] & nfsr[40]) ^ (nfsr[18] & nfsr[17]) ^ (nfsr[13] & nfsr[11]);
  38. }
  39.  
  40. uint8_t next_h()
  41. {
  42.   // h(x) = x0x1 + x2x3 + x4x5 + x6x7 + x0x4x8
  43.   #define x0 nfsr[12]  // bi+12
  44.   #define x1 lfsr[8]   // si+8
  45.   #define x2 lfsr[13]  // si+13
  46.   #define x3 lfsr[20]  // si+20
  47.   #define x4 nfsr[95]  // bi+95
  48.   #define x5 lfsr[42]  // si+42
  49.   #define x6 lfsr[60]  // si+60
  50.   #define x7 lfsr[79]  // si+79
  51.   #define x8 lfsr[95]  // si+95
  52.  
  53.   uint8_t h_out = (x0 & x1) ^ (x2 & x3) ^ (x4 & x5) ^ (x6 & x7) ^ (x0 & x4 & x8);
  54.   return h_out;
  55. }
  56.  
  57. uint8_t shift(uint8_t fsr[128], uint8_t fb)
  58. {
  59.   uint8_t out = fsr[0];
  60.   for (int i = 0; i < 127; i++) {
  61.     fsr[i] = fsr[i+1];
  62.   }
  63.   fsr[127] = fb;
  64.  
  65.   return out;
  66. }
  67.  
  68. uint8_t next_z()
  69. {
  70.   uint8_t lfsr_fb = next_lfsr_fb();
  71.   uint8_t nfsr_fb = next_nfsr_fb();
  72.   uint8_t h_out = next_h();
  73.  
  74.   /* y = h + s_{i+93} + sum(b_{i+j}), j \in A */
  75.   uint8_t A[] = {2, 15, 36, 45, 64, 73, 89};
  76.  
  77.   uint8_t nfsr_tmp = 0;
  78.   for (int i = 0; i < 7; i++) {
  79.     nfsr_tmp ^= nfsr[A[i]];
  80.   }
  81.  
  82.   uint8_t y = nfsr_tmp ^ h_out ^ lfsr[93];
  83.  
  84.   uint8_t lfsr_out;
  85.  
  86.   /* feedback y if we are in the initialization instance */
  87.   if (init_rounds) {
  88.     lfsr_out = shift(lfsr, lfsr_fb ^ y);
  89.     shift(nfsr, nfsr_fb ^ lfsr_out ^ y);
  90.   } else {
  91.     lfsr_out = shift(lfsr, lfsr_fb);
  92.     shift(nfsr, nfsr_fb ^ lfsr_out);
  93.   }
  94.  
  95.   return y;
  96. }
  97.  
  98. void print_stream(uint8_t *stream, uint8_t byte_size, char* data_, uint8_t frameSize)
  99. {
  100.   uint8_t keystream[byte_size];
  101.   for (int i = 0; i < byte_size; i++) {
  102.     uint8_t yi = 0;
  103.     for (int j = 0; j < 8; j++) {
  104.       yi = (yi << 1) ^ stream[i * 8 + j];
  105.     }
  106.     keystream[i] = yi;
  107.   }
  108.  
  109.   char _cipher[frameSize];
  110.   int y = 0;
  111.   for (int x = 0; x < frameSize; x++) {
  112.     if (x < 6) {
  113.       _cipher[x] = data_[x];
  114.     }
  115.     else {
  116.       _cipher[x] = data_[x] ^ keystream[y++];
  117.     }
  118.   }
  119.   send_it(_cipher, frameSize, data_);
  120. }
  121.  
  122. void generate_keystream(char* data_, uint8_t frameSize, uint8_t payloadSize)
  123. {
  124.     uint8_t ks[payloadSize * 8];
  125.  
  126.     /* generate keystream */
  127.     for (int i = 0; i < payloadSize * 8; i++) {
  128.       ks[i] = next_z();
  129.     }
  130.     print_stream(ks, payloadSize, data_, frameSize);
  131. }
  132.  
  133.  
  134. void encryption_start(char* data_, uint8_t frameSize, uint8_t payloadSize)
  135. {
  136.   //uint8_t key[] = {0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, 0xde, 0xf0};
  137.   //uint8_t iv[] = {0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78};/* initialize grain and skip output */
  138.   init_rounds = 1;
  139.   for (int i = 0; i < 256; i++) {
  140.     next_z();
  141.   }
  142.   init_rounds = 0;
  143.   generate_keystream(data_, frameSize, payloadSize);
  144. }
Advertisement
RAW Paste Data Copied
Advertisement