Advertisement
SlowerPhoton

Untitled

Feb 10th, 2016
337
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 16.83 KB | None | 0 0
  1. #include<LiquidCrystal.h>
  2. #include <EEPROM.h>
  3.  
  4. LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
  5.  
  6. void createSubkeys(byte binaryKey[8]);
  7. void encodeMessage(byte binaryMessage[64/8], byte encrypted[64/8]);
  8. void f(byte R[32/8], byte K[48/8], byte outcome[32/8]);
  9. void lookUpInSBox(size_t which, byte *address, byte *binaryOutcome, size_t addressFrom);
  10.  
  11. void setup() {
  12.   Serial.begin( 9600 );
  13.  
  14.   lcd.begin(16, 2);
  15.   Serial.println(F("ready"));
  16.  
  17.   // get a key and make subkeys
  18.   byte *binaryKey;
  19.   binaryKey = new byte[8] {155, 100, 30, 0, 25, 222, 25, 25};
  20.   createSubkeys(binaryKey);
  21.  
  22.   byte *binaryMessage;
  23.   binaryMessage = new byte[8] {155, 100, 30, 0, 25, 222, 25, 25};
  24.   byte *C;
  25.   //C = new byte[8];
  26.   encodeMessage(binaryMessage, C);
  27.  
  28.  
  29.  
  30. }
  31.  
  32. void loop() {
  33.   lcd.setCursor(0,0);
  34.   lcd.print(F("fff"));
  35. }
  36.  
  37. // the real integer pow
  38. int ipow(int base, int exp)
  39. {
  40.     int result = 1;
  41.     while (exp)
  42.     {
  43.         if (exp & 1)
  44.             result *= base;
  45.         exp >>= 1;
  46.         base *= base;
  47.     }
  48.  
  49.     return result;
  50. }
  51.  
  52. // returns the value of n-th int from an array of bytes
  53. bool bitValue ( byte* bytes, size_t n )
  54. {
  55.   size_t byteNum = n / 8;
  56.   n %= 8;
  57.  
  58.   byte Byte = *(bytes + byteNum);
  59.  
  60.   Byte >>= (8-n-1);
  61.   byte mask = 1;
  62.   Byte &= mask;
  63.   return Byte;
  64. }
  65.  
  66. // inserts a bit at a given position
  67. void insertBit ( byte* bytes, size_t n, bool value )
  68. {
  69.   size_t byteNum = n / 8;
  70.   n %= 8;
  71.  
  72.   if ( value )
  73.   {
  74.       byte mask = ipow(2, 8-n-1);
  75.       *(bytes+byteNum) |= mask;
  76.   }
  77.   else
  78.   {
  79.       byte mask = 255-ipow(2, 8-n-1);
  80.       *(bytes+byteNum) &= mask;
  81.   }
  82. }
  83.  
  84. // size of binary is in bytes
  85. void divideBinary(byte binary[], size_t sizeOfBinary, byte LB[], byte RB[]) {
  86.   size_t half = sizeOfBinary / 2;
  87.  
  88.   // LB - first half
  89.   size_t i = 0;
  90.   for (; i < half; i++) {
  91.     LB[i] = binary[i];
  92.   }
  93.  
  94.   // RB - second half
  95.   for (; i < half * 2; i++) {
  96.     RB[i - half] = binary[i];
  97.   }
  98. }
  99.  
  100. // size of binary is in bits
  101. void divideBinaryBit(byte* binary, size_t nmOfBits, byte* LB, byte* RB) {
  102.   size_t half = nmOfBits / 2;
  103.  
  104.   // LB - first half
  105.   size_t i = 0;
  106.   for (; i < half; i++) {
  107.     bool tmp = bitValue(binary, i);
  108.     insertBit(LB, i, tmp);
  109.   }
  110.  
  111.   // RB - second half
  112.   for (; i < half * 2; i++) {
  113.     bool tmp = bitValue(binary, i);
  114.     insertBit(LB, i, tmp);
  115.   }
  116. }
  117.  
  118. void overwrite(byte what[], byte with[], size_t noOfIndexes){
  119.     for (size_t i = 0; i < noOfIndexes; i++)
  120.         what[i] = with[i];
  121. }
  122.  
  123. void overwriteBit(byte *what, byte *with, size_t fromWhat, size_t fromWith, size_t noOfBits){
  124.     for (size_t i = 0; i < noOfBits; i++)
  125.         insertBit( what, fromWhat + i, bitValue( with, fromWith + i ) );
  126. }
  127.  
  128. void overwriteBit(bool what[], byte *with, size_t fromWhat, size_t fromWith, size_t noOfBits){
  129.     for (size_t i = 0; i < noOfBits; i++)
  130.         what[i + fromWhat] = bitValue( with, fromWith + i );
  131. }
  132.  
  133. void overwriteBit(byte *what, bool with[], size_t fromWhat, size_t fromWith, size_t noOfBits){
  134.     for (size_t i = 0; i < noOfBits; i++){
  135.         insertBit( what, fromWhat + i, with[ fromWith + i] );
  136.     }
  137. }
  138.  
  139. /*void overwriteBit(byte *what, byte *with, size_t fromWhat, size_t fromWith, size_t noOfBits){
  140.     for (size_t i = 0; i < noOfBits; i++)
  141.         insertBit( what, fromWhat + i, with[ fromWith + i] );
  142. }*/
  143.  
  144.  
  145. bool XOR(bool a, bool b){
  146.     return a ^ b;
  147. }
  148.  
  149. /*
  150. void XOR(byte where[], byte arr1[], byte arr2[], size_t noOfBits){
  151.     for(size_t i = 0; i < noOfBits; i++){
  152.         where[i] = arr1[i] ^ arr2[i];
  153.     }
  154. }*/
  155.  
  156. int toDecimal(bool binary[], int noOfNumbers){
  157.     int powOf2 = 1;
  158.     int decimal = 0;
  159.  
  160.     for(int i = noOfNumbers-1; i >= 0; i--){
  161.         decimal += powOf2*binary[i];
  162.         powOf2*=2;
  163.     }
  164.  
  165.     return decimal;
  166. }
  167.  
  168. void debugArray(byte *arr, size_t from, size_t to){
  169.     for (size_t i = from; i < to; i++){
  170.       Serial.print('\t');
  171.       Serial.print(i);
  172.       Serial.print(": ");
  173.       Serial.println(bitValue(arr, i));
  174.     }
  175. }
  176.  
  177. void debugArray(bool arr[], size_t from, size_t to){
  178.     for (size_t i = from; i < to; i++){
  179.       Serial.print('\t');
  180.       Serial.print(i);
  181.       Serial.print(": ");
  182.       Serial.println(arr[i]);
  183.     }
  184. }
  185.  
  186. bool encr = true;
  187. int8_t debug = 0;
  188.  
  189. void lookUpInSBox(size_t which, byte *address, byte* binaryOutcome, size_t addressFrom){
  190.  
  191.     size_t* Byte;
  192.     Byte = new size_t {addressFrom/8};
  193.     addressFrom%=8;
  194.     address+=*Byte;
  195.     //if (debug == 0)Serial.println(*Byte);
  196.     delete Byte;
  197.  
  198.     const uint8_t SBoxes[8][4][16] PROGMEM = {
  199.  
  200.                              /*S1*/
  201.  
  202.    { {14,  4,  13,  1,   2, 15,  11,  8,   3, 10,   6, 12,   5,  9,   0,  7},
  203.      { 0, 15,   7,  4,  14,  2,  13,  1,  10,  6,  12, 11,   9,  5,   3,  8},
  204.      { 4,  1,  14,  8,  13,  6,   2, 11,  15, 12,   9,  7,   3, 10,   5,  0},
  205.      {15, 12,   8,  2,   4,  9,   1,  7,   5, 11,   3, 14,  10,  0,   6, 13}    },
  206.  
  207.                              /*S2*/
  208.  
  209.    { {15,  1,   8, 14,   6, 11,   3,  4,   9,  7,   2, 13,  12,  0,   5, 10,},
  210.      { 3, 13,   4,  7,  15,  2,   8, 14,  12,  0,   1, 10,   6,  9,  11,  5,},
  211.      { 0, 14,   7, 11,  10,  4,  13,  1,   5,  8,  12,  6,   9,  3,   2, 15,},
  212.      {13,  8,  10,  1,   3, 15,   4,  2,  11,  6,   7, 12,   0,  5,  14,  9}    },
  213.  
  214.                              /*S3*/
  215.  
  216.    { {10,  0,   9, 14,   6,  3,  15,  5,   1, 13,  12,  7,  11,  4,   2,  8,},
  217.      {13,  7,   0,  9,   3,  4,   6, 10,   2,  8,   5, 14,  12, 11,  15,  1,},
  218.      {13,  6,   4,  9,   8, 15,   3,  0,  11,  1,   2, 12,   5, 10,  14,  7,},
  219.      { 1, 10,  13,  0,   6,  9,   8,  7,   4, 15,  14,  3,  11,  5,   2, 12}     },
  220.  
  221.                              /*S4*/
  222.  
  223.    { { 7, 13,  14,  3,   0,  6,   9, 10,   1,  2,   8,  5,  11, 12,   4, 15,},
  224.      {13,  8,  11,  5,   6, 15,   0,  3,   4,  7,   2, 12,   1, 10,  14,  9,},
  225.      {10,  6,   9,  0,  12, 11,   7, 13,  15,  1,   3, 14,   5,  2,   8,  4,},
  226.      { 3, 15,   0,  6,  10,  1,  13,  8,   9,  4,   5, 11,  12,  7,   2, 14}     },
  227.  
  228.                              /*S5*/
  229.  
  230.    { { 2, 12,   4,  1,   7, 10,  11,  6,   8,  5,   3, 15,  13,  0,  14,  9,},
  231.      {14, 11,   2, 12,   4,  7,  13,  1,   5,  0,  15, 10,   3,  9,   8,  6,},
  232.      { 4,  2,   1, 11,  10, 13,   7,  8,  15,  9,  12,  5,   6,  3,   0, 14,},
  233.      {11,  8,  12,  7,   1, 14,   2, 13,   6, 15,   0,  9,  10,  4,   5,  3}     },
  234.  
  235.                              /*S6*/
  236.  
  237.    { {12,  1,  10, 15,   9,  2,   6,  8,   0, 13,   3,  4,  14,  7,   5, 11,},
  238.      {10, 15,   4,  2,   7, 12,   9,  5,   6,  1,  13, 14,   0, 11,   3,  8,},
  239.      { 9, 14,  15,  5,   2,  8,  12,  3,   7,  0,   4, 10,   1, 13,  11,  6,},
  240.      { 4,  3,   2, 12,   9,  5,  15, 10,  11, 14,   1,  7,   6,  0,   8, 13}     },
  241.  
  242.                              /*S7*/
  243.  
  244.    { { 4, 11,   2, 14,  15,  0,   8, 13,   3, 12,   9,  7,   5, 10,   6,  1,},
  245.      {13,  0,  11,  7,   4,  9,   1, 10,  14,  3,   5, 12,   2, 15,   8,  6,},
  246.      { 1,  4,  11, 13,  12,  3,   7, 14,  10, 15,   6,  8,   0,  5,   9,  2,},
  247.      { 6, 11,  13,  8,   1,  4,  10,  7,   9,  5,   0, 15,  14,  2,   3, 12}     },
  248.  
  249.                              /*S8*/
  250.  
  251.    { {13,  2,   8,  4,   6, 15,  11,  1,  10,  9,   3, 14,   5,  0,  12,  7,},
  252.      { 1, 15,  13,  8,  10,  3,   7,  4,  12,  5,   6, 11,   0, 14,   9,  2,},
  253.      { 7, 11,   4,  1,   9, 12,  14,  2,   0,  6,  10, 13,  15,  3,   5,  8,},
  254.      { 2,  1,  14,  7,   4, 10,   8, 13,  15, 12,   9,  0,   3,  5,   6, 11}     }
  255.  
  256.                         };
  257.  
  258.     // row corresponds to the binary number consisting of the first and last index of address
  259.     bool *row;
  260.     row = new bool[2];
  261.     row[0] = bitValue( address, 0 + addressFrom );
  262.     row[1] = bitValue( address, 5 + addressFrom );
  263.  
  264.      
  265.             /*if (debug == 0){
  266.               //debugArray(&answer, 0, 4);
  267.               Serial.println(*address);
  268.               Serial.println(addressFrom);
  269.               Serial.println(row[0]);
  270.               Serial.println(row[1]);
  271.             }debug++;*/
  272.  
  273.     // column corresponds to the middle 4 bits of address
  274.     bool *column;
  275.     column = new bool[4];
  276.     overwriteBit(column, address, 0, addressFrom + 1, 4);
  277.  
  278.     uint8_t number = SBoxes[which][toDecimal(row, 2)][toDecimal(column, 4)];
  279.     delete[] row; delete[] column;
  280.  
  281.     // convert number to binary number saved in answer
  282.     bool answer[4] = {0,0,0,0};
  283.     uint8_t powOf2 = 8;
  284.     for(short i = 0; i < 4; i++){
  285.         if(number >= powOf2){
  286.             answer[i] = 1;
  287.             number -= powOf2;
  288.         }
  289.  
  290.         powOf2/=2;
  291.     }
  292.  
  293.     // copy answer to binaryOutcome
  294.     overwriteBit(binaryOutcome, answer, 0, 0, 4);
  295. }
  296.  
  297. void f(byte R[32/8], byte K[48/8], byte outcome[32/8]){
  298.    
  299.     // expand each R from 32 to 48 bits to E
  300.     const uint8_t E_BIT[48] PROGMEM = {
  301.                  32,     1,    2,     3,     4,    5,
  302.                   4,     5,    6,     7,     8,    9,
  303.                   8,     9,   10,    11,    12,   13,
  304.                  12,    13,   14,    15,    16,   17,
  305.                  16,    17,   18,    19,    20,   21,
  306.                  20,    21,   22,    23,    24,   25,
  307.                  24,    25,   26,    27,    28,   29,
  308.                  28,    29,   30,    31,    32,    1
  309.     };
  310.  
  311.     byte *E;
  312.     E = new byte[48/8];  
  313.     for(size_t i = 0; i < 48; i++){
  314.         insertBit( E, i, bitValue( R, E_BIT[i]-1 ) );
  315.     }
  316.    
  317.     // XOR Kn and  E(Rn-1)
  318.     byte KxorE[48/8];
  319.     for (size_t i = 0; i < 48; i++){
  320.         insertBit( KxorE, i, bitValue( K, i ) ^ bitValue( E, i ) );
  321.     }
  322.     delete[] E;
  323.  
  324.     // each array of 6 bits from KxorE is used as an address in S boxes, thus obtaining a 4-bit number
  325.     byte S[32/8];
  326.     for(short i = 0; i < 48; i+=6){
  327.         byte answer; // only first 4 bits are always used
  328.         lookUpInSBox(i/6, KxorE, &answer, i); // save what we find in S-boxes into answer (its first 4 bits)  
  329.         overwriteBit(S, &answer, (i/6)*4, 0, 4);
  330.     }
  331.  
  332.     if (debug == 0){
  333.       debugArray(S, 0, 32);
  334.       debug = 3;
  335.     }
  336.  
  337.     // now we use P to permute S[] to finally create the outcome
  338.     const uint8_t P[32] PROGMEM = {
  339.                          16,   7,  20,  21,
  340.                          29,  12,  28,  17,
  341.                           1,  15,  23,  26,
  342.                           5,  18,  31,  10,
  343.                           2,   8,  24,  14,
  344.                          32,  27,   3,   9,
  345.                          19,  13,  30,   6,
  346.                          22,  11,   4,  25
  347.     };
  348.  
  349.     for (size_t i = 0; i < 32; i++){
  350.         //outcome[i] = S[P[i]-1];
  351.         insertBit( outcome, i, bitValue( S, P[i]-1 ) );
  352.     }
  353. }
  354.  
  355. void encodeMessage(byte binaryMessage[64/8], byte encrypted[64/8]){
  356.     // permute binaryMessage by IP to permutedMessage
  357.     const uint8_t IP[64] PROGMEM = {
  358.             58,    50,   42,    34,    26,   18,    10,    2,
  359.             60,    52,   44,    36,    28,   20,    12,    4,
  360.             62,    54,   46,    38,    30,   22,    14,    6,
  361.             64,    56,   48,    40,    32,   24,    16,    8,
  362.             57,    49,   41,    33,    25,   17,     9,    1,
  363.             59,    51,   43,    35,    27,   19,    11,    3,
  364.             61,    53,   45,    37,    29,   21,    13,    5,
  365.             63,    55,   47,    39,    31,   23,    15,    7
  366.     };
  367.  
  368.     byte *permutedMessage;
  369.     permutedMessage = new byte[64/8];
  370.  
  371.     for(int i = 0; i < 64; i++){
  372.         insertBit( permutedMessage, i, bitValue( binaryMessage, IP[i]-1 ) );
  373.     }
  374.     delete[] binaryMessage;
  375.  
  376.     // create L0 and R0 by dividing permutedMessage, the second index represents L [0] for first 4 bytes and  R for last 4
  377.     //byte LR[17][64/8]; // gonna be saved on EEPROM from index 96 to 17*64/8+96 = 232
  378.     for (size_t address = 96; address < 96+8; address++){
  379.       EEPROM.write( address, *(permutedMessage + address - 96) );
  380.     }
  381.     delete[] permutedMessage;
  382.  
  383.     for(short i = 1; i <= 16; i++){
  384.  
  385.         // subkeys are on EEPROM (first 16*48/8 bytes that is first 96) so we need to load the current row first
  386.         byte subkeys[48/8];
  387.         if (encr)
  388.           for (size_t address = (i-1)*6; address < (i-1)*6+6; address++){
  389.             subkeys[address%6] = EEPROM.read( address );
  390.           }
  391.         else
  392.           for (size_t address = (16-i)*6; address < (16-i)*6+6; address++){
  393.             subkeys[address%6] = EEPROM.read( address );
  394.           }
  395.  
  396.          // LR is on EEPROM so we need to load it first (begins at index 96, size is [17][64/8] bytes)
  397.          byte LR[2][8];
  398.          for (size_t address = 96+((i-1)*8); address < 96+((i-1)*8) + 16; address++){
  399.            *(LR[0]+address-96) = EEPROM.read( address );
  400.          }
  401.      
  402.         overwrite(LR[1], LR[0]+4, 64/8/2); // Ln = Rn-1
  403.         // Rn = Ln-1 + f(Rn-1,Kn):
  404.         byte outcome[32/8];
  405.         if(encr == true) // according to the choice of the user: encrypt or decrypt
  406.             f(LR[0]+4, subkeys, outcome); // call the function f for Rn-1,Kn
  407.         else
  408.             f(LR[0]+4, subkeys, outcome); // call the function f for Rn-1,Kn
  409.         //if (i == 1) debugArray(outcome, 0, 32);
  410.  
  411.         //XOR(addition, LR[0][i-1], outcome, 32);
  412.         for (size_t i = 0; i < 32; i++){
  413.             insertBit( outcome, i, bitValue( LR[0], i ) ^ bitValue( outcome, i ) );
  414.         }
  415.  
  416.         // addition, which correspond to Ln-1 + f(Rn-1,Kn) is now copied to Rn
  417.         overwrite(LR[1]+4, outcome, 32/8);
  418.  
  419.         // update LR on EEPROM
  420.         for (size_t address = 96+(i-1)*8; address < 96+(i-1)*8+16; address++){
  421.           EEPROM.write( address, *(LR[0] + address - 96) );
  422.         }
  423.        
  424.     }
  425.     //debugArray(LR[16], 0, 64);
  426.  
  427.     /*// R16L16 after the permutation by IP-1 corresponds to the encrypted/decrypted message C
  428.  
  429.     int IP_1[64] = {
  430.             40,     8,   48,    16,    56,   24,    64,   32,
  431.             39,     7,   47,    15,    55,   23,    63,   31,
  432.             38,     6,   46,    14,    54,   22,    62,   30,
  433.             37,     5,   45,    13,    53,   21,    61,   29,
  434.             36,     4,   44,    12,    52,   20,    60,   28,
  435.             35,     3,   43,    11,    51,   19,    59,   27,
  436.             34,     2,   42,    10,    50,   18,    58,   26,
  437.             33,     1,   41,     9,    49,   17,    57,   25
  438.     };
  439.  
  440.     bool R16L16[64];
  441.     overwrite(R16L16, LR[1][16], 32); // R16
  442.     overwrite(R16L16 + 32, LR[0][16], 32); // L16
  443.  
  444.     bool C[64];
  445.     permutation(IP_1, R16L16, C, 64);
  446.  
  447.     overwrite(encrypted, C, 64);*/
  448. }
  449.  
  450. #include <avr/pgmspace.h>
  451.  
  452. // rotates 28 bits to left
  453. void leftShift(byte *toShift, uint8_t noOfShifts, byte* destination, bool L) {
  454.   size_t fromBit; // because L and R share one byte, we need to consider the initial bit as well
  455.   if (L) fromBit = 0;
  456.   else fromBit = 28%8; // that's 4
  457.  
  458.   for (int i = 0; i < 28; i++) {
  459.     insertBit(destination, fromBit + (i - noOfShifts + 28) % 28, bitValue(toShift, i + fromBit));
  460.   }
  461. }
  462.  
  463. void createSubkeys(byte *binaryKey) {
  464.   const uint8_t pc_1[56] PROGMEM = {
  465.     57, 49, 41, 33, 25, 17,  9,
  466.     1, 58, 50, 42, 34, 26, 18,
  467.     10,  2, 59, 51, 43, 35, 27,
  468.     19, 11,  3, 60, 52, 44, 36,
  469.     63, 55, 47, 39, 31, 23, 15,
  470.     7, 62, 54, 46, 38, 30, 22,
  471.     14,  6, 61, 53, 45, 37, 29,
  472.     21, 13,  5, 28, 20, 12,  4
  473.   };
  474.   byte* keyPermutation;
  475.   keyPermutation = new byte[56/8];
  476.  
  477.  
  478.   // according to pc_1 create from 64-bit key 56-bit keyPermutation
  479.   for (size_t i = 0; i < 56; i++) {
  480.     bool tmp = bitValue(binaryKey, pc_1[i] - 1);
  481.     insertBit(keyPermutation, i, tmp);
  482.   }
  483.   delete[] binaryKey;
  484.    
  485.   // C and D will be saved here: [index] [7 bytes] - first 28 bits are left part, the rest right part
  486.   byte CD[16 + 1][56 / 8];
  487.  
  488.   // divide keyPermutation into halves to C0 a D0 - each consists of 28 bits
  489.   divideBinaryBit(keyPermutation, 56, CD[0], CD[0]+28);
  490.   delete[] keyPermutation;
  491.  
  492.   // from C0, D0 and shifts make C1, D1 -> C16, D16
  493.   const uint8_t shifts[16] PROGMEM = { 1, 1, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 1 };
  494.   for (size_t i = 1; i < 17; i++) {
  495.     leftShift(CD[i - 1], shifts[i - 1], CD[i], true);
  496.     leftShift(CD[i - 1]+3, shifts[i - 1], CD[i]+3, false); // we need to start from the beginning of the right half - that is 3 bytes + 4 bits (shift takes care of the bits)
  497.   }
  498.  
  499.   // each subKey out of 16 is made from one out of 16 CD with the use of pc_2
  500.   const uint8_t pc_2[48] PROGMEM = {
  501.     14,    17,   11,    24,     1,    5,
  502.     3,    28,   15,     6,    21,   10,
  503.     23,    19,   12,     4,    26,    8,
  504.     16,     7,   27,    20,    13,    2,
  505.     41,    52,   31,    37,    47,   55,
  506.     30,    40,   51,    45,    33,   48,
  507.     44,    49,   39,    56,    34,   53,
  508.     46,    42,   50,    36,    29,   32
  509.   };
  510.  
  511.   byte subkeys[16][48/8];
  512.   for (int i = 0; i < 16; i++) {
  513.     for (int j = 0; j < 48; j++) {
  514.       insertBit( subkeys[i], j, bitValue( CD[i+1], pc_2[j] - 1 ) );
  515.     }
  516.   }
  517.  
  518.   for (size_t address = 0; address < 16*6; address++){
  519.         EEPROM.write( address, *(subkeys[0] + address) );
  520.   }
  521. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement