main.cpp

00001 #include <stdio.h>
00002 #include "mbed.h"
00003 //#include "toolchain.h"
00004 #include "ble/BLE.h"
00005 #include "ble/BLEProtocol.h"
00006 #include "TMP_nrf51/TMP_nrf51.h"
00007 #include "newSha256.h"
00008 #include "UARTService.h"
00009 
00010 #include "Hashes/sfh_mbed.h"
00011 
00012 #include "ctc_aes.h"
00013 
00014 #define UART_TX     p9
00015 #define UART_RX     p11
00016 
00017 #define LOG(...)    { pc.printf(__VA_ARGS__); }
00018 
00019 //#define DEBUG 1
00020 
00021 
00022 #define TABLE_SIZE 3000
00023 #define SLIDING_WINDOW_SIZE 300
00024 
00025 DigitalOut alivenessLED(LED1, 1);
00026 Ticker     ticker;
00027 
00028 Serial pc(UART_TX, UART_RX);
00029 
00030 UARTService *uartServicePtr;
00031 
00032 //stuff for encryption
00033 uint8_t payload[31];
00034 #define BLOCK_SIZE 16
00035 #define KEYLEN 256 //128, 192, 256
00036 int j;
00037 unsigned char nonce_counter[BLOCK_SIZE];
00038 unsigned char plain[BLOCK_SIZE];  
00039 unsigned char cipher[BLOCK_SIZE];
00040 unsigned char* counter_bytes = nonce_counter+BLOCK_SIZE/2;
00041 size_t counter_tx_len = 3; 
00042 unsigned char key[KEYLEN/8];
00043 unsigned char iv[BLOCK_SIZE];//not used for ctr mode but required by setKey
00044 Aes ctx;
00045 
00046 #define DECRYP_HASH_SIZE 6
00047 #define HASH_CTR_SIZE 255
00048 
00049 bool isSameMAC(const Gap::AdvertisementCallbackParams_t *params){
00050     
00051     for(int i=(params->advertisingDataLen)-counter_tx_len; i < params->advertisingDataLen; i++){
00052         nonce_counter[BLOCK_SIZE-3+(i-((params->advertisingDataLen)-counter_tx_len))] = params->advertisingData[i];
00053         printf("params->advertisingData[%d]: %02x\n", i,params->advertisingData[i]);
00054         }
00055         printf("\n");
00056         printf("nonce_counter done\n");
00057 #ifdef DEBUG
00058         printf("\n\n");
00059 #endif
00060 
00061      /*Dummy MAC address with NoOPS*/
00062      BLEProtocol::AddressBytes_t alpha = {0x90,0x90,0x90,0x90,0x90,0x90};
00063      
00064      for(int k = 0; k< DECRYP_HASH_SIZE; k ++){
00065         alpha[k] = params->peerAddr[k];
00066 #ifdef DEBUG
00067         printf("alpha[%d]: %02x  peerAddr[%d]: %02x",k,alpha[k],k,params->peerAddr[k]);
00068 #endif 
00069      }
00070      
00071      bool encryptedMac = true;
00072      
00073      unsigned char pre_out[32];
00074      unsigned char out[32];
00075      
00076      do{
00077         mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
00078         /*rehash*/
00079         
00080         for(int idx = 0; idx < DECRYP_HASH_SIZE; idx++){
00081 //#ifdef DEBUG
00082             printf("out[%d]: %02x - alpha[%d]: %02x\n",idx,out[idx],idx,alpha[idx]);
00083 //#endif
00084             if(out[idx] == alpha[idx]){
00085                 /*check all bytes*/
00086                 continue;
00087             }
00088             else{
00089                 printf("NOT ENCRYPTULATOR! EXITING DECRYPTION\n\n");
00090                 encryptedMac = false;
00091                 break;
00092             }
00093         }
00094     }while(0);   
00095     
00096     
00097     return encryptedMac;
00098 
00099 }
00100 
00101 void periodicCallback(void)
00102 {
00103     alivenessLED = !alivenessLED; /* Do blinky on LED1 while we're waiting for BLE events. This is optional. */
00104 }
00105 void initAES(void)
00106 {
00107     for(int i=0;i<KEYLEN/8;i++)
00108         key[i] = i;
00109     for(int i=0; i<BLOCK_SIZE-3; i++)
00110         nonce_counter[i]=i<8?i:0;
00111     for(int i=0; i<BLOCK_SIZE;i++)
00112         iv[i]=0;
00113     AesSetKey(&ctx, key, KEYLEN/8, iv, AES_ENCRYPTION);
00114 }  
00115 
00116 
00117 
00118 
00119 
00120 /*
00121 void buildLookupTable(unsigned char * nonce){
00122     unsigned char nonce_ctr_cpy[BLOCK_SIZE];
00123     unsigned char ctr_cpy = nonce_ctr_cpy + BLOCK_SIZE
00124     for(int i = 0; i < BLOCK_SIZE; i++)
00125         nonce_ctr_cpy = i;
00126     int ** hashLookup = new int[HASH_CTR_SIZE];   
00127     mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
00128 
00129     for(int i = 0; i < HASH_CTR_SIZE; i++){
00130        hashLookup[i] = out[i % 32];    
00131     }
00132 }*/
00133 
00134 void decrypt(const Gap::AdvertisementCallbackParams_t *params)
00135 {
00136     //puts decrypted data into GLOBAL plain variable.
00137     
00138    // pair<uint32_t, uint32_t> * lookupTable = new pair<uint32_t,uint32_t> [300000];
00139     pair<BLEProtocol::AddressBytes_t,unsigned char *>;
00140     //get coutner
00141     /*for(int i=(params->advertisingDataLen)-counter_tx_len; i < params->advertisingDataLen; i++)
00142         nonce_counter[BLOCK_SIZE-3+(i-((params->advertisingDataLen)-counter_tx_len))] = params->advertisingData[i];
00143       */
00144            
00145     //print nonce_counter
00146     printf("\nNonceCtr:  ");
00147     for(int i=0;i<BLOCK_SIZE;i++)
00148         printf("%02x ", nonce_counter[i]);
00149          
00150     //get cipher text
00151     for(int i=0; i < (params->advertisingDataLen) - (counter_tx_len + 2); i++)
00152         cipher[i] = params->advertisingData[i+2];
00153     
00154      
00155      //AddressBytes ble_mac = params->peerAddr;
00156 
00157      
00158     //print cipher
00159     printf("\nCiphertxt: ");
00160     for(int i=0; i < BLOCK_SIZE; i++)
00161         printf("%02x ", cipher[i]);
00162         
00163     
00164     //build key stream
00165     AesEncrypt(&ctx, nonce_counter, plain);
00166     //print key
00167     printf("\nKey:       ");
00168     for(int i=0; i<BLOCK_SIZE; i++)
00169         printf("%02x ", plain[i]);
00170     
00171     //decrypt into plain (destroying key)
00172     for(int i=0;i<BLOCK_SIZE;i++)
00173         plain[i]^=cipher[i];  
00174 }
00175 
00176 /*
00177  * This function is called every time we scan an advertisement.
00178  */
00179 void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
00180 {
00181     
00182     /* Search for the manufacturer specific data with matching application-ID */
00183     int addr_length = 6;
00184     //int * cachedMAC = new int[3];
00185     //print from addr
00186     //if(params->peerAddr[addr_length-1] == 0xfc)
00187     //{
00188         BLEDevice ble;
00189         unsigned char address[addr_length];
00190         Gap::addr_type_t gap_type = Gap::ADDR_TYPE_PUBLIC;
00191         ble.getAddress(&gap_type,address);
00192         printf("\nDecryptor MAC: ");
00193         for(int jj = 0; jj < addr_length; jj++)
00194             printf("%02x:",address[jj]);
00195         
00196         
00197         
00198         
00199         printf("\nFrom: ");
00200         for(int i=0; i<addr_length; i++)
00201              printf("%02x:", params->peerAddr[addr_length-i-1]);
00202         //print payload
00203         printf("\nPayload:  ");
00204         for(int i=0; i < params->advertisingDataLen; i++) 
00205                 printf(" %02x", params->advertisingData[i]);
00206                
00207         //cache the beginning MAC address
00208         
00209         /*
00210         for(int c = 0; c < 3; c++){
00211             cachedMAC[c] = params->peerAddr[addr_length-c-1];   
00212         }       
00213         */       
00214                
00215                
00216        if(isSameMAC(params)){
00217            printf("MAC ADDRESS IDENTIFIED!!!!\n");
00218            exit(0); 
00219            decrypt(params);
00220         }
00221       
00222         
00223       
00224         //print plaintext
00225         printf("\nPlaintext: ");
00226         for(int i=0; i<BLOCK_SIZE; i++)
00227             printf("%02x ", plain[i]);
00228         
00229         //print close of round
00230         printf("\n\n");
00231     //}
00232 }
00233 
00234 
00235 
00236 /**
00237  * This function is called when the ble initialization process has failed
00238  */
00239 void onBleInitError(BLE &ble, ble_error_t error)
00240 {
00241     /* Initialization error handling should go here */
00242     printf("Crap, the BLE radio is broken\n");
00243 }
00244 
00245 /**
00246  * Callback triggered when the ble initialization process has finished
00247  */
00248 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
00249 {
00250     BLE&        ble   = params->ble;
00251     ble_error_t error = params->error;
00252 
00253     if (error != BLE_ERROR_NONE) {
00254         /* In case of error, forward the error handling to onBleInitError */
00255         onBleInitError(ble, error);
00256         return;
00257     }
00258 
00259     /* Ensure that it is the default instance of BLE */
00260     if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
00261         return;
00262     }
00263 
00264     /* Setup and start scanning */
00265     ble.gap().setScanParams(50 /* scan interval */, 50 /* scan window */);
00266     ble.gap().startScan(advertisementCallback);
00267 }
00268 
00269 int main(void)
00270 {
00271    
00272     //use 115200 for term 4M for energy
00273     //pc.baud(115200);
00274     
00275    
00276     
00277     printf("---- DECRYPTULATOR ACTIVIZE ----\n");
00278     initAES();
00279     
00280     ticker.attach(periodicCallback, 1);  /* flash the LED because reasons */
00281 
00282     printf("Bring up the BLE radio\n");
00283     BLE &ble = BLE::Instance();
00284     ble.init(bleInitComplete);
00285 
00286     UARTService uartService(ble);
00287     uartServicePtr = &uartService;
00288     //uartService.retargetStdout();
00289 
00290     while (true) {
00291         ble.waitForEvent();
00292     }
00293 }