Diff: main.cpp

Revision:

3:b48570121d3f

Parent:

2:e1a6b317241e

--- a/main.cpp	Sat May 28 22:10:43 2016 +0000
+++ b/main.cpp	Wed May 10 18:21:02 2017 +0000
@@ -1,13 +1,17 @@
-#include<string>
+#include <string>
 #include "mbed.h"
 #include "mbed_i2c.h"
 #include "inv_mpu.h"
 #include "inv_mpu_dmp_motion_driver.h"
+//#include "ble/BLE.h"
+//#include "ble/BLEProtocol.h"
 
 #include "BLEDevice.h"
 #include "DFUService.h"
 #include "UARTService.h"
 #include "ctc_aes.h"
+#include "newSha256.h"
+#include "time.h"
 
 #define BAUDR 115200
 //#define BAUDR 4000000
@@ -52,6 +56,8 @@
 
 //stuff for encryption
 uint8_t payload[31];
+#define CTR_SIZE 3
+#define ENCRYP_MAC_SIZE 6
 #define BLOCK_SIZE 16
 #define DATA_SIZE 256
 #define KEYLEN 256 //128, 192, 256
@@ -60,6 +66,10 @@
 unsigned char plain[BLOCK_SIZE];  
 unsigned char cipher[BLOCK_SIZE];
 unsigned char* counter_bytes = nonce_counter+BLOCK_SIZE/2;
+
+#ifdef DEBUG
+unsigned char * hash_count = counter_bytes;
+#endif
 size_t counter_tx_len = 3; 
 unsigned char key[KEYLEN/8];
 unsigned char iv[BLOCK_SIZE];//not used for ctr mode but required by setKey
@@ -67,10 +77,86 @@
 unsigned char data[DATA_SIZE];
 unsigned int data_index = 0;
 unsigned int last_txd = 0;
+unsigned char out[32];
+unsigned char hash_counter[CTR_SIZE];
 
+unsigned int packet_ctr; 
+
+//unsigned int temp_ctr = {0x90,0x90,0x90};
+//#define DEBUG 1
+//#define HASH_CTR 1
 
 volatile bool bleIsConnected = false;
 
+void encryptCounter(void){
+    
+    printf("ENCRYPTING COUNTER\n\n\n");
+    unsigned char encryp_count[BLOCK_SIZE];
+    unsigned char e_b[32];
+    /**nonce information*/
+    for(int i = 0 ; i < BLOCK_SIZE; i++)
+        encryp_count[i] = nonce_counter[i];
+    
+   /* printf("counter_bytes[5]: %02x\n" , counter_bytes[5]);
+    printf("counter_bytes[6]: %02x\n" , counter_bytes[6]);
+    printf("counter_bytes[7]: %02x\n" , counter_bytes[7]);*/
+   // mbedtls_sha256(nonce_counter, sizeof(nonce_counter), e_b, 0);
+    
+    for(int i = 0; i < 3;)
+        hash_counter[i] = e_b[i];
+    
+     //exit(0);
+#ifdef DEBUG
+    for(int i = 0; i < 3; i++){
+        if(hash_counter[i] != encryp_count[i]){
+            printf("FUCK\n");
+            exit(0);      
+        }
+               
+    }
+        
+    
+
+    printf("NONCE COUNTER: \n");
+    for(int i = 0; i < sizeof(nonce_counter); i++)
+        printf("nonce_counter[%d]: %02x  ", i, nonce_counter[i]);
+    printf("\n\n");
+    
+    printf("-----------------SMALLER SIZE HASH_COUNTER----------------\n");
+    for(int i = 0; i < sizeof(hash_counter); i++)
+        printf("hash_counter[%d]: %02x  ", i, hash_counter[i]);
+    printf("\n\n");
+    printf("-----------------LARGER SIZE HASH_COUNTER-----------------\n");
+    mbedtls_sha256(nonce_counter, sizeof(nonce_counter), encryp_count, 0);
+    for(int k = 0; k < sizeof(encryp_count); k++)
+        printf("encryp_count[%d]: %02x  ", k, encryp_count[k]);
+    printf("\n\n");
+    
+    
+    printf("NONCE COUNTER: \n");
+    for(int i = 0; i < sizeof(nonce_counter); i++)
+        printf("nonce_counter[%d]: %02x  ", i, nonce_counter[i]);
+        
+    printf("\n\n");
+
+    printf("PLAINTEXT: ");
+    for(int a = 0; a < 8; a++)
+       printf("counter_bytes[%d]: %02x   ", a, counter_bytes[a]);
+
+    printf("\n\n\n\n");
+    /*random number*/
+    printf("SHA256(encryp_count): ");
+    for(int i = 0; i < 32; i++)
+        printf("%02x",encryp_count[i]);
+    
+    printf("\n\n");
+#endif
+    
+    printf("SUCCESS\n");
+    
+}
+
+
 void initAES(void)
 {
    //initial nonce and counter
@@ -89,39 +175,126 @@
 
 void encrypt()
 {
-    LOG("\nNonceCntr: ");
+    printf("\nNonceCntr: ");
     for(j=0; j<BLOCK_SIZE; j++)
-        LOG("%02x ",nonce_counter[j]);
+        printf("%02x ",nonce_counter[j]);
     
+    unsigned char address[6];// = {0x00,0x00,0x00,0x00,0x00,0x00};
+   // string s = address.str();
+    //ble.getAddress(&(Gap::ADDR_TYPE_PUBLIC), address);
+    Gap::addr_type_t gap_type = Gap::ADDR_TYPE_PUBLIC;
+    ble.getAddress(&gap_type,address);
+   
+   printf("Mac address before: ");
+   for(int mac = 0; mac < 6; mac++){
+    printf("%02x: ",address[mac]);
+   }
+   printf("\n\n");
+
+    //Encode the counter into the end of the nonce_counter
+    /*for(int k = BLOCK_SIZE - counter_tx_len; k < BLOCK_SIZE-1; k++){
+        nonce_counter[k] = k;
+    }*/
+   
+   mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
+    printf("\nNonceCntr: ");
+    for(j=0; j<BLOCK_SIZE; j++)
+        printf("%02x ",nonce_counter[j]);
+    printf("\n\n");
+    
+    printf("\n------------------------OUTPUT---------------------------------\n");
     
     
-    //generate keystream... stick in cipher
+    printf("\nHash_counter: ");
+    for(int vvv = 5; vvv < 8; vvv++){
+        hash_counter[vvv - 5] = out[vvv];
+        printf("%02x ", hash_counter[vvv - 5]);
+    }
+    
+    printf("\nCounter_bytes: ");
+    for(int ll = 0; ll < sizeof(counter_bytes); ll++)
+        printf("%02x ", counter_bytes[ll]);
+    printf("\n\n");
+    //exit(0);
+    printf("SHA256(NONCE): ");
+    for(int jjj=0; jjj<32; jjj++)
+        printf("%02x ",out[jjj]);
+        
+    printf("\n\n");
+    
+   for(int kk = 0; kk < ENCRYP_MAC_SIZE; kk++){
+       address[kk] = out[kk];
+    }
+
+    printf("ENCRYPTOR MAC ADDRESS AFTER: ");
+    for(int v = 0; v < 6; v++){
+        printf("%02x ",address[v]);
+    }
+    
+    printf("\n\n");
+    
+    ble.setAddress(gap_type,address);    
+        
+    
+    
    //make sure the following lines are uncommented for full cryptocop
     //if(nonce_counter[7]%2){
-      //  AesEncrypt(&ctx, nonce_counter, cipher);
-   // }
+    AesEncrypt(&ctx, nonce_counter, cipher);
+    //}
     for(int i=0;i<BLOCK_SIZE;i++)
         cipher[i]^=plain[i];
     
     //print plaintext and cipher text
-    LOG("\nPlaintext: ");
+    printf("\nPlaintext: ");
     for(int i =0; i<BLOCK_SIZE; i++)
-        LOG("%02x ", plain[i]);
-    LOG("\nCiphertxt: ");
+        printf("%02x ", plain[i]);
+        
+    printf("\n\n");
+    
+    printf("\nCiphertxt: ");
     for(j=0; j<BLOCK_SIZE; j++)
-        LOG("%02x ",cipher[j]);
+        printf("%02x ",cipher[j]);
 
+    printf("\n\n");
+    
+    //encryptCounter();
+    //printf("?????\n");
 }
 
 void sense(void)
 {
+/*
+#ifdef DEBUG
+    printf("COUNTERS AT BEGINNING OF TUX PACKET\n\n\n");
+    printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
+    printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
+    printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
+#endif    
+  */  
     data[data_index]=(unsigned char)(hr_out.read()*512);
-    LOG("%d, %f\n",data[data_index], hr_out.read());  
+    //printf("%d, %f\n",data[data_index], hr_out.read());  
     data_index = (data_index+1)%DATA_SIZE;  
+    
+/*#ifdef DEBUG    
+    printf("COUNTERS AT END OF TUX PACKET\n\n\n");
+    printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
+    printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
+    printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
+#endif
+*/
 }
+
 void tx_packet(void)
 {
+
+#ifdef DEBUG    
+    packet_ctr++;
+    printf("PACKET COUNTER: %d\n\n",packet_ctr);
+#endif
+   
+        
     ble.clearAdvertisingPayload();
+    
     //fill plain text buffer
     for(int i =0; i<BLOCK_SIZE; i++)
     {
@@ -129,27 +302,86 @@
         last_txd = (last_txd + 1) % DATA_SIZE;
     }
     encrypt();
-    
+#ifdef DEBUG
+    unsigned char p[BLOCK_SIZE];
+    for(int v = 0; v < BLOCK_SIZE; v++)
+        p[v] = v;
+#endif
     //build payload
     memcpy(payload, cipher, BLOCK_SIZE);
-    memcpy(payload+BLOCK_SIZE,counter_bytes+BLOCK_SIZE/2-counter_tx_len,counter_tx_len);
+
+#ifdef DEBUG
+    printf(" BYTES PRINTING\n");
+    for(int ctr = 0; ctr < sizeof(counter_bytes); ctr++){
+        printf("counter_bytes[%d]: %02x", ctr, counter_bytes[ctr]);
+    }
+    
+    unsigned char eurek[3] = {0x90, 0x90, 0x90};
+#endif
+    //Second half of the nonce_ctr will contian the ctr
+    
+    printf("-------------------WHAT WE CARE ABOUT--------------------------\n");
+#ifndef HASH_CTR
+    printf("-------------------BEFORE ENCRYPTION---------------------------");
+
+    //memcpy(payload+BLOCK_SIZE,counter_bytes+BLOCK_SIZE/2-counter_tx_len,counter_tx_len);
+    memcpy(payload+BLOCK_SIZE, hash_counter, counter_tx_len);
     /* setup advertising */
+        
     ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
                                      payload, BLOCK_SIZE+counter_tx_len); 
+#endif 
                                      
     //print payload
-    LOG("\nPayload:   ");
+    printf("\nPayload:   ");
     for(int i=0; i<BLOCK_SIZE+counter_tx_len; i++)
-        LOG("%02x ",payload[i]);
+        printf("%02x ",payload[i]);
     ble.startAdvertising();
-    //increment counter
-    j=7;
+    
+    //increment counter     
+   j=7;
+   int num_cycles = 0;
+   
+   
+   printf("COUNTER_BYTES PRINTING\n\n\n");
     do
     {
+       // printf("num_cycles: %d",num_cycles);
+        
+        printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
+        printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
+        printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
+       for(j = 5; j <= 7; j++)
         counter_bytes[j]++;
+        
+       printf("BEFORE RANDOMIZATION: counter_bytes[7]: %02x  \n", counter_bytes[7]);
+       //encryptCounter();
+       printf("AFTER RANDOMIZATION counter counter_bytes[7]: %02x  \n", counter_bytes[7]);
+       
+        printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
+        printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
+        printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
+        printf("WHILE\n");
+       
     } while(counter_bytes[j--] == 0);
     
-    LOG("\n\n");
+#ifdef DEBUG
+    if(counter_bytes[7] == 0x99 || counter_bytes[7] == 153){
+        exit(0);
+    }
+#endif
+
+
+    
+#ifdef DEBUG
+
+
+    printf("Printing hash_count!!!\n"); 
+    for(int hash = 0; hash < 32; hash++){
+           printf("hash_count[%d]: %02x",hash,hash_count[hash]);
+    }
+#endif
+    printf("\n\n");
 }
 
 void bleInitComplete(void)
@@ -162,21 +394,23 @@
     ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
                                      (const uint8_t*)"bob is ready", sizeof("bob is ready"));
                                      
-    ble.setAdvertisingInterval(6400); /* 2s; in multiples of 0.625ms. */
+    ble.setAdvertisingInterval(9600); /* 2s; in multiples of 0.625ms. */
     ble.startAdvertising();
 }
 
 int main(void)
 {
 
-    pc.baud(BAUDR);
-    LOG("---- ENCRYPTULATOR ACTIVIZE ----\n");
+    packet_ctr = 0;
+   // pc.baud(BAUDR);
+    printf("---- ENCRYPTULATOR ACTIVIZE ----\n");
 
     initAES();
 
-    LOG("Bring up the BLE radio\n");
+    printf("Bring up the BLE radio\n");
     ble.init();
     
+    srand(time(NULL));
     
     //replace with sensor data call
     for(int i =0; i<BLOCK_SIZE; i++)
@@ -193,8 +427,11 @@
     sensor_timeout.attach(&sense, 0.025);
     tx_timeout.attach(&tx_packet, 0.4);
     
+    printf("EXITING ENTIRE LOOP\n\n\n");
+   // exit(0);
     
     while (true) {
             ble.waitForEvent();
     }
+    
 }