File content as of revision 13:8b706583610a:

#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 "Hashes/sfh_mbed.h"

#include "ctc_aes.h"

#define UART_TX     p9
#define UART_RX     p11

#define LOG(...)    { pc.printf(__VA_ARGS__); }

//#define DEBUG 1


#define TABLE_SIZE 3000
#define SLIDING_WINDOW_SIZE 300

DigitalOut alivenessLED(LED1, 1);
Ticker     ticker;

Serial pc(UART_TX, UART_RX);

UARTService *uartServicePtr;

//stuff for encryption
uint8_t payload[31];
#define BLOCK_SIZE 16
#define KEYLEN 256 //128, 192, 256
int j;
unsigned char nonce_counter[BLOCK_SIZE];
unsigned char plain[BLOCK_SIZE];  
unsigned char cipher[BLOCK_SIZE];
unsigned char* counter_bytes = nonce_counter+BLOCK_SIZE/2;
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
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)
{
    alivenessLED = !alivenessLED; /* Do blinky on LED1 while we're waiting for BLE events. This is optional. */
}
void initAES(void)
{
    for(int i=0;i<KEYLEN/8;i++)
        key[i] = i;
    for(int i=0; i<BLOCK_SIZE-3; i++)
        nonce_counter[i]=i<8?i:0;
    for(int i=0; i<BLOCK_SIZE;i++)
        iv[i]=0;
    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++)
        nonce_counter[BLOCK_SIZE-3+(i-((params->advertisingDataLen)-counter_tx_len))] = params->advertisingData[i];
      */
           
    //print nonce_counter
    printf("\nNonceCtr:  ");
    for(int i=0;i<BLOCK_SIZE;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
    printf("\nCiphertxt: ");
    for(int i=0; i < BLOCK_SIZE; i++)
        printf("%02x ", cipher[i]);
        
    
    //build key stream
    AesEncrypt(&ctx, nonce_counter, plain);
    //print key
    printf("\nKey:       ");
    for(int i=0; i<BLOCK_SIZE; i++)
        printf("%02x ", plain[i]);
    
    //decrypt into plain (destroying key)
    for(int i=0;i<BLOCK_SIZE;i++)
        plain[i]^=cipher[i];  
}

/*
 * This function is called every time we scan an advertisement.
 */
void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
    
    /* 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)
    //{
        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++)
             printf("%02x:", params->peerAddr[addr_length-i-1]);
        //print payload
        printf("\nPayload:  ");
        for(int i=0; i < params->advertisingDataLen; i++) 
                printf(" %02x", params->advertisingData[i]);
               
        //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
        printf("\nPlaintext: ");
        for(int i=0; i<BLOCK_SIZE; i++)
            printf("%02x ", plain[i]);
        
        //print close of round
        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 */
    printf("Crap, the BLE radio is broken\n");
}

/**
 * Callback triggered when the ble initialization process has finished
 */
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    /* Setup and start scanning */
    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);
    
   
    
    printf("---- DECRYPTULATOR ACTIVIZE ----\n");
    initAES();
    
    ticker.attach(periodicCallback, 1);  /* flash the LED because reasons */

    printf("Bring up the BLE radio\n");
    BLE &ble = BLE::Instance();
    ble.init(bleInitComplete);

    UARTService uartService(ble);
    uartServicePtr = &uartService;
    //uartService.retargetStdout();

    while (true) {
        ble.waitForEvent();
    }
}