File content as of revision 4:bfb662bab28e:

#include <string>
#include "mbed.h"
#include "mbed_i2c.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
//#include "ble/BLE.h"
//#include "ble/BLEProtocol.h"

#include "BLEDevice.h"
#include "DFUService.h"
#include "UARTService.h"
#include "ctc_aes.h"
#include "newSha256.h"
#include "time.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 CTR_SIZE 3
#define ENCRYP_MAC_SIZE 6
#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;

#ifdef DEBUG
unsigned char * hash_count = counter_bytes;
#endif
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;
unsigned char out[32];
unsigned char hash_counter[CTR_SIZE];

unsigned int packet_ctr; 

//unsigned int temp_ctr = {0x90,0x90,0x90};
//#define DEBUG 1
//#define HASH_CTR 1

volatile bool bleIsConnected = false;

void encryptCounter(void){
    
    printf("ENCRYPTING COUNTER\n\n\n");
    unsigned char encryp_count[BLOCK_SIZE];
    unsigned char e_b[32];
    /**nonce information*/
    for(int i = 0 ; i < BLOCK_SIZE; i++)
        encryp_count[i] = nonce_counter[i];
    
   /* printf("counter_bytes[5]: %02x\n" , counter_bytes[5]);
    printf("counter_bytes[6]: %02x\n" , counter_bytes[6]);
    printf("counter_bytes[7]: %02x\n" , counter_bytes[7]);*/
   // mbedtls_sha256(nonce_counter, sizeof(nonce_counter), e_b, 0);
    
    for(int i = 0; i < 3;)
        hash_counter[i] = e_b[i];
    
     //exit(0);
#ifdef DEBUG
    for(int i = 0; i < 3; i++){
        if(hash_counter[i] != encryp_count[i]){
            printf("FUCK\n");
            exit(0);      
        }
               
    }
        
    

    printf("NONCE COUNTER: \n");
    for(int i = 0; i < sizeof(nonce_counter); i++)
        printf("nonce_counter[%d]: %02x  ", i, nonce_counter[i]);
    printf("\n\n");
    
    printf("-----------------SMALLER SIZE HASH_COUNTER----------------\n");
    for(int i = 0; i < sizeof(hash_counter); i++)
        printf("hash_counter[%d]: %02x  ", i, hash_counter[i]);
    printf("\n\n");
    printf("-----------------LARGER SIZE HASH_COUNTER-----------------\n");
    mbedtls_sha256(nonce_counter, sizeof(nonce_counter), encryp_count, 0);
    for(int k = 0; k < sizeof(encryp_count); k++)
        printf("encryp_count[%d]: %02x  ", k, encryp_count[k]);
    printf("\n\n");
    
    
    printf("NONCE COUNTER: \n");
    for(int i = 0; i < sizeof(nonce_counter); i++)
        printf("nonce_counter[%d]: %02x  ", i, nonce_counter[i]);
        
    printf("\n\n");

    printf("PLAINTEXT: ");
    for(int a = 0; a < 8; a++)
       printf("counter_bytes[%d]: %02x   ", a, counter_bytes[a]);

    printf("\n\n\n\n");
    /*random number*/
    printf("SHA256(encryp_count): ");
    for(int i = 0; i < 32; i++)
        printf("%02x",encryp_count[i]);
    
    printf("\n\n");
#endif
    
    printf("SUCCESS\n");
    
}


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()
{
    printf("\nNonceCntr: ");
    for(j=0; j<BLOCK_SIZE; j++)
        printf("%02x ",nonce_counter[j]);
    
    unsigned char address[6];// = {0x00,0x00,0x00,0x00,0x00,0x00};
   // string s = address.str();
    //ble.getAddress(&(Gap::ADDR_TYPE_PUBLIC), address);
    Gap::addr_type_t gap_type = Gap::ADDR_TYPE_PUBLIC;
    ble.getAddress(&gap_type,address);
   
   printf("Mac address before: ");
   for(int mac = 0; mac < 6; mac++){
    printf("%02x: ",address[mac]);
   }
   printf("\n\n");

    //Encode the counter into the end of the nonce_counter
    /*for(int k = BLOCK_SIZE - counter_tx_len; k < BLOCK_SIZE-1; k++){
        nonce_counter[k] = k;
    }*/
   
   mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
    printf("\nNonceCntr: ");
    for(j=0; j<BLOCK_SIZE; j++)
        printf("%02x ",nonce_counter[j]);
    printf("\n\n");
    
    printf("\n------------------------OUTPUT---------------------------------\n");
    
    
    printf("\nHash_counter: ");
    for(int vvv = 5; vvv < 8; vvv++){
        hash_counter[vvv - 5] = out[vvv];
        printf("%02x ", hash_counter[vvv - 5]);
    }
    
    printf("\nCounter_bytes: ");
    for(int ll = 0; ll < sizeof(counter_bytes); ll++)
        printf("%02x ", counter_bytes[ll]);
    printf("\n\n");
    //exit(0);
    printf("SHA256(NONCE): ");
    for(int jjj=0; jjj<32; jjj++)
        printf("%02x ",out[jjj]);
        
    printf("\n\n");
    
   for(int kk = 0; kk < ENCRYP_MAC_SIZE; kk++){
       address[kk] = out[kk];
    }

    printf("ENCRYPTOR MAC ADDRESS AFTER: ");
    for(int v = 0; v < 6; v++){
        printf("%02x ",address[v]);
    }
    
    printf("\n\n");
    
    ble.setAddress(gap_type,address);    
        
    
    
   //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
    printf("\nPlaintext: ");
    for(int i =0; i<BLOCK_SIZE; i++)
        printf("%02x ", plain[i]);
        
    printf("\n\n");
    
    printf("\nCiphertxt: ");
    for(j=0; j<BLOCK_SIZE; j++)
        printf("%02x ",cipher[j]);

    printf("\n\n");
    
    //encryptCounter();
    //printf("?????\n");
}

void sense(void)
{
/*
#ifdef DEBUG
    printf("COUNTERS AT BEGINNING OF TUX PACKET\n\n\n");
    printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
    printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
    printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
#endif    
  */  
    data[data_index]=(unsigned char)(hr_out.read()*512);
    //printf("%d, %f\n",data[data_index], hr_out.read());  
    data_index = (data_index+1)%DATA_SIZE;  
    
/*#ifdef DEBUG    
    printf("COUNTERS AT END OF TUX PACKET\n\n\n");
    printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
    printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
    printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
#endif
*/
}

void tx_packet(void)
{

#ifdef DEBUG    
    packet_ctr++;
    printf("PACKET COUNTER: %d\n\n",packet_ctr);
#endif
   
        
    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();
#ifdef DEBUG
    unsigned char p[BLOCK_SIZE];
    for(int v = 0; v < BLOCK_SIZE; v++)
        p[v] = v;
#endif
    //build payload
    memcpy(payload, cipher, BLOCK_SIZE);

#ifdef DEBUG
    printf(" BYTES PRINTING\n");
    for(int ctr = 0; ctr < sizeof(counter_bytes); ctr++){
        printf("counter_bytes[%d]: %02x", ctr, counter_bytes[ctr]);
    }
    
    unsigned char eurek[3] = {0x90, 0x90, 0x90};
#endif
    //Second half of the nonce_ctr will contian the ctr
    
    printf("-------------------WHAT WE CARE ABOUT--------------------------\n");
#ifndef HASH_CTR
    printf("-------------------BEFORE ENCRYPTION---------------------------");

    //memcpy(payload+BLOCK_SIZE,counter_bytes+BLOCK_SIZE/2-counter_tx_len,counter_tx_len);
    memcpy(payload+BLOCK_SIZE, hash_counter, counter_tx_len);
    /* setup advertising */
        
    ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
                                     payload, BLOCK_SIZE+counter_tx_len); 
#endif 
                                     
    //print payload
    printf("\nPayload:   ");
    for(int i=0; i<BLOCK_SIZE+counter_tx_len; i++)
        printf("%02x ",payload[i]);
    ble.startAdvertising();
    
    //increment counter     
   j=7;
   int num_cycles = 0;
   
   
   printf("COUNTER_BYTES PRINTING\n\n\n");
    do
    {
       // printf("num_cycles: %d",num_cycles);
        
        printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
        printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
        printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
       for(j = 5; j <= 7; j++)
        counter_bytes[j]++;
        
       printf("BEFORE RANDOMIZATION: counter_bytes[7]: %02x  \n", counter_bytes[7]);
       //encryptCounter();
       printf("AFTER RANDOMIZATION counter counter_bytes[7]: %02x  \n", counter_bytes[7]);
       
        printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
        printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
        printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
        printf("WHILE\n");
       
    } while(counter_bytes[j--] == 0);
    
#ifdef DEBUG
    if(counter_bytes[7] == 0x99 || counter_bytes[7] == 153){
        exit(0);
    }
#endif


    
#ifdef DEBUG


    printf("Printing hash_count!!!\n"); 
    for(int hash = 0; hash < 32; hash++){
           printf("hash_count[%d]: %02x",hash,hash_count[hash]);
    }
#endif
    printf("\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(9600); /* 2s; in multiples of 0.625ms. */
    ble.startAdvertising();
}

int main(void)
{

    packet_ctr = 0;
   // pc.baud(BAUDR);
    printf("---- ENCRYPTULATOR ACTIVIZE ----\n");

    initAES();

    printf("Bring up the BLE radio\n");
    ble.init();
    
    srand(time(NULL));
    
    //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);
    
    printf("EXITING ENTIRE LOOP\n\n\n");
   // exit(0);
    
    while (true) {
            ble.waitForEvent();
    }
    
}