Diff: main.cpp

Revision:

13:8b706583610a

Parent:

12:dbbf0ddc9b12

--- a/main.cpp	Sat May 28 22:07:19 2016 +0000
+++ b/main.cpp	Tue May 09 03:06:55 2017 +0000
@@ -1,11 +1,13 @@
-
+#include <stdio.h>
 #include "mbed.h"
 //#include "toolchain.h"
 #include "ble/BLE.h"
+#include "ble/BLEProtocol.h"
 #include "TMP_nrf51/TMP_nrf51.h"
+#include "newSha256.h"
+#include "UARTService.h"
 
-
-#include "UARTService.h"
+#include "Hashes/sfh_mbed.h"
 
 #include "ctc_aes.h"
 
@@ -14,6 +16,12 @@
 
 #define LOG(...)    { pc.printf(__VA_ARGS__); }
 
+//#define DEBUG 1
+
+
+#define TABLE_SIZE 3000
+#define SLIDING_WINDOW_SIZE 300
+
 DigitalOut alivenessLED(LED1, 1);
 Ticker     ticker;
 
@@ -35,6 +43,60 @@
 unsigned char iv[BLOCK_SIZE];//not used for ctr mode but required by setKey
 Aes ctx;
 
+#define DECRYP_HASH_SIZE 6
+#define HASH_CTR_SIZE 255
+
+bool isSameMAC(const Gap::AdvertisementCallbackParams_t *params){
+    
+    for(int i=(params->advertisingDataLen)-counter_tx_len; i < params->advertisingDataLen; i++){
+        nonce_counter[BLOCK_SIZE-3+(i-((params->advertisingDataLen)-counter_tx_len))] = params->advertisingData[i];
+        printf("params->advertisingData[%d]: %02x\n", i,params->advertisingData[i]);
+        }
+        printf("\n");
+        printf("nonce_counter done\n");
+#ifdef DEBUG
+        printf("\n\n");
+#endif
+
+     /*Dummy MAC address with NoOPS*/
+     BLEProtocol::AddressBytes_t alpha = {0x90,0x90,0x90,0x90,0x90,0x90};
+     
+     for(int k = 0; k< DECRYP_HASH_SIZE; k ++){
+        alpha[k] = params->peerAddr[k];
+#ifdef DEBUG
+        printf("alpha[%d]: %02x  peerAddr[%d]: %02x",k,alpha[k],k,params->peerAddr[k]);
+#endif 
+     }
+     
+     bool encryptedMac = true;
+     
+     unsigned char pre_out[32];
+     unsigned char out[32];
+     
+     do{
+        mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
+        /*rehash*/
+        
+        for(int idx = 0; idx < DECRYP_HASH_SIZE; idx++){
+//#ifdef DEBUG
+            printf("out[%d]: %02x - alpha[%d]: %02x\n",idx,out[idx],idx,alpha[idx]);
+//#endif
+            if(out[idx] == alpha[idx]){
+                /*check all bytes*/
+                continue;
+            }
+            else{
+                printf("NOT ENCRYPTULATOR! EXITING DECRYPTION\n\n");
+                encryptedMac = false;
+                break;
+            }
+        }
+    }while(0);   
+    
+    
+    return encryptedMac;
+
+}
 
 void periodicCallback(void)
 {
@@ -51,36 +113,60 @@
     AesSetKey(&ctx, key, KEYLEN/8, iv, AES_ENCRYPTION);
 }  
 
+
+
+
+
+/*
+void buildLookupTable(unsigned char * nonce){
+    unsigned char nonce_ctr_cpy[BLOCK_SIZE];
+    unsigned char ctr_cpy = nonce_ctr_cpy + BLOCK_SIZE
+    for(int i = 0; i < BLOCK_SIZE; i++)
+        nonce_ctr_cpy = i;
+    int ** hashLookup = new int[HASH_CTR_SIZE];   
+    mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
+
+    for(int i = 0; i < HASH_CTR_SIZE; i++){
+       hashLookup[i] = out[i % 32];    
+    }
+}*/
+
 void decrypt(const Gap::AdvertisementCallbackParams_t *params)
 {
     //puts decrypted data into GLOBAL plain variable.
     
-    
+   // pair<uint32_t, uint32_t> * lookupTable = new pair<uint32_t,uint32_t> [300000];
+    pair<BLEProtocol::AddressBytes_t,unsigned char *>;
     //get coutner
-    for(int i=(params->advertisingDataLen)-counter_tx_len; i < params->advertisingDataLen; i++)
+    /*for(int i=(params->advertisingDataLen)-counter_tx_len; i < params->advertisingDataLen; i++)
         nonce_counter[BLOCK_SIZE-3+(i-((params->advertisingDataLen)-counter_tx_len))] = params->advertisingData[i];
+      */
            
     //print nonce_counter
-    LOG("\nNonceCtr:  ");
+    printf("\nNonceCtr:  ");
     for(int i=0;i<BLOCK_SIZE;i++)
-        LOG("%02x ", nonce_counter[i]);
+        printf("%02x ", nonce_counter[i]);
          
     //get cipher text
     for(int i=0; i < (params->advertisingDataLen) - (counter_tx_len + 2); i++)
         cipher[i] = params->advertisingData[i+2];
     
+     
+     //AddressBytes ble_mac = params->peerAddr;
+
+     
     //print cipher
-    LOG("\nCiphertxt: ");
+    printf("\nCiphertxt: ");
     for(int i=0; i < BLOCK_SIZE; i++)
-        LOG("%02x ", cipher[i]);
+        printf("%02x ", cipher[i]);
         
     
     //build key stream
     AesEncrypt(&ctx, nonce_counter, plain);
     //print key
-    LOG("\nKey:       ");
+    printf("\nKey:       ");
     for(int i=0; i<BLOCK_SIZE; i++)
-        LOG("%02x ", plain[i]);
+        printf("%02x ", plain[i]);
     
     //decrypt into plain (destroying key)
     for(int i=0;i<BLOCK_SIZE;i++)
@@ -95,38 +181,65 @@
     
     /* Search for the manufacturer specific data with matching application-ID */
     int addr_length = 6;
-
+    //int * cachedMAC = new int[3];
     //print from addr
     //if(params->peerAddr[addr_length-1] == 0xfc)
     //{
-        LOG("\nFrom: ");
+        BLEDevice ble;
+        unsigned char address[addr_length];
+        Gap::addr_type_t gap_type = Gap::ADDR_TYPE_PUBLIC;
+        ble.getAddress(&gap_type,address);
+        printf("\nDecryptor MAC: ");
+        for(int jj = 0; jj < addr_length; jj++)
+            printf("%02x:",address[jj]);
+        
+        
+        
+        
+        printf("\nFrom: ");
         for(int i=0; i<addr_length; i++)
-             LOG("%02x:", params->peerAddr[addr_length-i-1]);
+             printf("%02x:", params->peerAddr[addr_length-i-1]);
         //print payload
-        LOG("\nPayload:  ");
+        printf("\nPayload:  ");
         for(int i=0; i < params->advertisingDataLen; i++) 
-                LOG(" %02x", params->advertisingData[i]);
+                printf(" %02x", params->advertisingData[i]);
                
-    
-        decrypt(params);
+        //cache the beginning MAC address
+        
+        /*
+        for(int c = 0; c < 3; c++){
+            cachedMAC[c] = params->peerAddr[addr_length-c-1];   
+        }       
+        */       
+               
+               
+       if(isSameMAC(params)){
+           printf("MAC ADDRESS IDENTIFIED!!!!\n");
+           exit(0); 
+           decrypt(params);
+        }
+      
+        
       
         //print plaintext
-        LOG("\nPlaintext: ");
+        printf("\nPlaintext: ");
         for(int i=0; i<BLOCK_SIZE; i++)
-            LOG("%02x ", plain[i]);
+            printf("%02x ", plain[i]);
         
         //print close of round
-        LOG("\n\n");
+        printf("\n\n");
     //}
 }
 
+
+
 /**
  * This function is called when the ble initialization process has failed
  */
 void onBleInitError(BLE &ble, ble_error_t error)
 {
     /* Initialization error handling should go here */
-    LOG("Crap, the BLE radio is broken\n");
+    printf("Crap, the BLE radio is broken\n");
 }
 
 /**
@@ -149,21 +262,24 @@
     }
 
     /* Setup and start scanning */
-    ble.gap().setScanParams(500 /* scan interval */, 500 /* scan window */);
+    ble.gap().setScanParams(50 /* scan interval */, 50 /* scan window */);
     ble.gap().startScan(advertisementCallback);
 }
 
 int main(void)
 {
+   
     //use 115200 for term 4M for energy
-    pc.baud(115200);
+    //pc.baud(115200);
     
-    LOG("---- DECRYPTULATOR ACTIVIZE ----\n");
+   
+    
+    printf("---- DECRYPTULATOR ACTIVIZE ----\n");
     initAES();
     
     ticker.attach(periodicCallback, 1);  /* flash the LED because reasons */
 
-    LOG("Bring up the BLE radio\n");
+    printf("Bring up the BLE radio\n");
     BLE &ble = BLE::Instance();
     ble.init(bleInitComplete);