File content as of revision 2:e1a6b317241e:

#include<string>
#include "mbed.h"
#include "mbed_i2c.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

#include "BLEDevice.h"
#include "DFUService.h"
#include "UARTService.h"
#include "ctc_aes.h"

#define BAUDR 115200
//#define BAUDR 4000000
#define LOG(...)    { pc.printf(__VA_ARGS__); }
//#define LOG(...) 

#define HR_OUT p4
#define HR_LOP p6
#define HR_LOM p5
//#define LED_GREEN   p21
//#define LED_RED     p22
//#define LED_BLUE    p23
//#define BUTTON_PIN  p17
//#define BATTERY_PIN p1

//#define MPU6050_SDA p12
//#define MPU6050_SCL p13

#define UART_TX     p9
#define UART_RX     p11
//#define UART_CTS    p8
//#define UART_RTS    p10

/* Starting sampling rate. */
//#define DEFAULT_MPU_HZ  (100)

//DigitalOut blue(LED_BLUE);
//DigitalOut green(LED_GREEN);
//DigitalOut red(LED_RED);

//InterruptIn button(BUTTON_PIN);
AnalogIn    hr_out(HR_OUT);
DigitalIn   hr_lop(HR_LOP);
DigitalIn   hr_lom(HR_LOM);
Serial pc(UART_TX, UART_RX);

//InterruptIn motion_probe(p14);

BLEDevice  ble;
UARTService *uartServicePtr;
Ticker tx_timeout, sensor_timeout;

//stuff for encryption
uint8_t payload[31];
#define BLOCK_SIZE 16
#define DATA_SIZE 256
#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;
unsigned char data[DATA_SIZE];
unsigned int data_index = 0;
unsigned int last_txd = 0;


volatile bool bleIsConnected = false;

void initAES(void)
{
   //initial nonce and counter
    for(int i=0; i<BLOCK_SIZE; i++)
    {
        nonce_counter[i]=i<8?i:0;
        iv[i]=0;
    }
     
     //initialize key   
    for(int i=0;i<KEYLEN/8;i++)
        key[i] = i;

    AesSetKey(&ctx, key, KEYLEN/8, iv, AES_ENCRYPTION); 
}

void encrypt()
{
    LOG("\nNonceCntr: ");
    for(j=0; j<BLOCK_SIZE; j++)
        LOG("%02x ",nonce_counter[j]);
    
    
    
    //generate keystream... stick in cipher
   //make sure the following lines are uncommented for full cryptocop
    //if(nonce_counter[7]%2){
      //  AesEncrypt(&ctx, nonce_counter, cipher);
   // }
    for(int i=0;i<BLOCK_SIZE;i++)
        cipher[i]^=plain[i];
    
    //print plaintext and cipher text
    LOG("\nPlaintext: ");
    for(int i =0; i<BLOCK_SIZE; i++)
        LOG("%02x ", plain[i]);
    LOG("\nCiphertxt: ");
    for(j=0; j<BLOCK_SIZE; j++)
        LOG("%02x ",cipher[j]);

}

void sense(void)
{
    data[data_index]=(unsigned char)(hr_out.read()*512);
    LOG("%d, %f\n",data[data_index], hr_out.read());  
    data_index = (data_index+1)%DATA_SIZE;  
}
void tx_packet(void)
{
    ble.clearAdvertisingPayload();
    //fill plain text buffer
    for(int i =0; i<BLOCK_SIZE; i++)
    {
        plain[i] = data[last_txd];
        last_txd = (last_txd + 1) % DATA_SIZE;
    }
    encrypt();
    
    //build payload
    memcpy(payload, cipher, BLOCK_SIZE);
    memcpy(payload+BLOCK_SIZE,counter_bytes+BLOCK_SIZE/2-counter_tx_len,counter_tx_len);
    /* setup advertising */
    ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
                                     payload, BLOCK_SIZE+counter_tx_len); 
                                     
    //print payload
    LOG("\nPayload:   ");
    for(int i=0; i<BLOCK_SIZE+counter_tx_len; i++)
        LOG("%02x ",payload[i]);
    ble.startAdvertising();
    //increment counter
    j=7;
    do
    {
        counter_bytes[j]++;
    } while(counter_bytes[j--] == 0);
    
    LOG("\n\n");
}

void bleInitComplete(void)
{
    /* setup advertising */
    
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    
    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.startAdvertising();
}

int main(void)
{

    pc.baud(BAUDR);
    LOG("---- ENCRYPTULATOR ACTIVIZE ----\n");

    initAES();

    LOG("Bring up the BLE radio\n");
    ble.init();
    
    
    //replace with sensor data call
    for(int i =0; i<BLOCK_SIZE; i++)
        plain[i] = i+3;
    
    //uart stuff
    DFUService dfu(ble);                                 
    UARTService uartService(ble);
    uartServicePtr = &uartService;

    bleInitComplete();
    
    //maybe replace with something inside sensor read?  Basically trigger sending
    sensor_timeout.attach(&sense, 0.025);
    tx_timeout.attach(&tx_packet, 0.4);
    
    
    while (true) {
            ble.waitForEvent();
    }
}