File content as of revision 12:dbbf0ddc9b12:

#include "mbed.h"
//#include "toolchain.h"
#include "ble/BLE.h"
#include "TMP_nrf51/TMP_nrf51.h"


#include "UARTService.h"

#include "ctc_aes.h"

#define UART_TX     p9
#define UART_RX     p11

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

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;


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 decrypt(const Gap::AdvertisementCallbackParams_t *params)
{
    //puts decrypted data into GLOBAL plain variable.
    
    
    //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
    LOG("\nNonceCtr:  ");
    for(int i=0;i<BLOCK_SIZE;i++)
        LOG("%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];
    
    //print cipher
    LOG("\nCiphertxt: ");
    for(int i=0; i < BLOCK_SIZE; i++)
        LOG("%02x ", cipher[i]);
        
    
    //build key stream
    AesEncrypt(&ctx, nonce_counter, plain);
    //print key
    LOG("\nKey:       ");
    for(int i=0; i<BLOCK_SIZE; i++)
        LOG("%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;

    //print from addr
    //if(params->peerAddr[addr_length-1] == 0xfc)
    //{
        LOG("\nFrom: ");
        for(int i=0; i<addr_length; i++)
             LOG("%02x:", params->peerAddr[addr_length-i-1]);
        //print payload
        LOG("\nPayload:  ");
        for(int i=0; i < params->advertisingDataLen; i++) 
                LOG(" %02x", params->advertisingData[i]);
               
    
        decrypt(params);
      
        //print plaintext
        LOG("\nPlaintext: ");
        for(int i=0; i<BLOCK_SIZE; i++)
            LOG("%02x ", plain[i]);
        
        //print close of round
        LOG("\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");
}

/**
 * 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(500 /* scan interval */, 500 /* scan window */);
    ble.gap().startScan(advertisementCallback);
}

int main(void)
{
    //use 115200 for term 4M for energy
    pc.baud(115200);
    
    LOG("---- DECRYPTULATOR ACTIVIZE ----\n");
    initAES();
    
    ticker.attach(periodicCallback, 1);  /* flash the LED because reasons */

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

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

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