main.cpp

00001 #include <string>
00002 #include "mbed.h"
00003 #include "mbed_i2c.h"
00004 #include "inv_mpu.h"
00005 #include "inv_mpu_dmp_motion_driver.h"
00006 //#include "ble/BLE.h"
00007 //#include "ble/BLEProtocol.h"
00008 
00009 #include "BLEDevice.h"
00010 #include "DFUService.h"
00011 #include "UARTService.h"
00012 #include "ctc_aes.h"
00013 #include "newSha256.h"
00014 #include "time.h"
00015 
00016 #define BAUDR 115200
00017 //#define BAUDR 4000000
00018 #define LOG(...)    { pc.printf(__VA_ARGS__); }
00019 //#define LOG(...) 
00020 
00021 #define HR_OUT p4
00022 #define HR_LOP p6
00023 #define HR_LOM p5
00024 //#define LED_GREEN   p21
00025 //#define LED_RED     p22
00026 //#define LED_BLUE    p23
00027 //#define BUTTON_PIN  p17
00028 //#define BATTERY_PIN p1
00029 
00030 //#define MPU6050_SDA p12
00031 //#define MPU6050_SCL p13
00032 
00033 #define UART_TX     p9
00034 #define UART_RX     p11
00035 //#define UART_CTS    p8
00036 //#define UART_RTS    p10
00037 
00038 /* Starting sampling rate. */
00039 //#define DEFAULT_MPU_HZ  (100)
00040 
00041 //DigitalOut blue(LED_BLUE);
00042 //DigitalOut green(LED_GREEN);
00043 //DigitalOut red(LED_RED);
00044 
00045 //InterruptIn button(BUTTON_PIN);
00046 AnalogIn    hr_out(HR_OUT);
00047 DigitalIn   hr_lop(HR_LOP);
00048 DigitalIn   hr_lom(HR_LOM);
00049 Serial pc(UART_TX, UART_RX);
00050 
00051 //InterruptIn motion_probe(p14);
00052 
00053 BLEDevice  ble;
00054 UARTService *uartServicePtr;
00055 Ticker tx_timeout, sensor_timeout;
00056 
00057 //stuff for encryption
00058 uint8_t payload[31];
00059 #define CTR_SIZE 3
00060 #define ENCRYP_MAC_SIZE 6
00061 #define BLOCK_SIZE 16
00062 #define DATA_SIZE 256
00063 #define KEYLEN 256 //128, 192, 256
00064 int j;
00065 unsigned char nonce_counter[BLOCK_SIZE];
00066 unsigned char plain[BLOCK_SIZE];  
00067 unsigned char cipher[BLOCK_SIZE];
00068 unsigned char* counter_bytes = nonce_counter+BLOCK_SIZE/2;
00069 
00070 #ifdef DEBUG
00071 unsigned char * hash_count = counter_bytes;
00072 #endif
00073 size_t counter_tx_len = 3; 
00074 unsigned char key[KEYLEN/8];
00075 unsigned char iv[BLOCK_SIZE];//not used for ctr mode but required by setKey
00076 Aes ctx;
00077 unsigned char data[DATA_SIZE];
00078 unsigned int data_index = 0;
00079 unsigned int last_txd = 0;
00080 unsigned char out[32];
00081 unsigned char hash_counter[CTR_SIZE];
00082 
00083 unsigned int packet_ctr; 
00084 
00085 //unsigned int temp_ctr = {0x90,0x90,0x90};
00086 //#define DEBUG 1
00087 //#define HASH_CTR 1
00088 
00089 volatile bool bleIsConnected = false;
00090 
00091 void encryptCounter(void){
00092     
00093     printf("ENCRYPTING COUNTER\n\n\n");
00094     unsigned char encryp_count[BLOCK_SIZE];
00095     unsigned char e_b[32];
00096     /**nonce information*/
00097     for(int i = 0 ; i < BLOCK_SIZE; i++)
00098         encryp_count[i] = nonce_counter[i];
00099     
00100    /* printf("counter_bytes[5]: %02x\n" , counter_bytes[5]);
00101     printf("counter_bytes[6]: %02x\n" , counter_bytes[6]);
00102     printf("counter_bytes[7]: %02x\n" , counter_bytes[7]);*/
00103    // mbedtls_sha256(nonce_counter, sizeof(nonce_counter), e_b, 0);
00104     
00105     for(int i = 0; i < 3;)
00106         hash_counter[i] = e_b[i];
00107     
00108      //exit(0);
00109 #ifdef DEBUG
00110     for(int i = 0; i < 3; i++){
00111         if(hash_counter[i] != encryp_count[i]){
00112             printf("FUCK\n");
00113             exit(0);      
00114         }
00115                
00116     }
00117         
00118     
00119 
00120     printf("NONCE COUNTER: \n");
00121     for(int i = 0; i < sizeof(nonce_counter); i++)
00122         printf("nonce_counter[%d]: %02x  ", i, nonce_counter[i]);
00123     printf("\n\n");
00124     
00125     printf("-----------------SMALLER SIZE HASH_COUNTER----------------\n");
00126     for(int i = 0; i < sizeof(hash_counter); i++)
00127         printf("hash_counter[%d]: %02x  ", i, hash_counter[i]);
00128     printf("\n\n");
00129     printf("-----------------LARGER SIZE HASH_COUNTER-----------------\n");
00130     mbedtls_sha256(nonce_counter, sizeof(nonce_counter), encryp_count, 0);
00131     for(int k = 0; k < sizeof(encryp_count); k++)
00132         printf("encryp_count[%d]: %02x  ", k, encryp_count[k]);
00133     printf("\n\n");
00134     
00135     
00136     printf("NONCE COUNTER: \n");
00137     for(int i = 0; i < sizeof(nonce_counter); i++)
00138         printf("nonce_counter[%d]: %02x  ", i, nonce_counter[i]);
00139         
00140     printf("\n\n");
00141 
00142     printf("PLAINTEXT: ");
00143     for(int a = 0; a < 8; a++)
00144        printf("counter_bytes[%d]: %02x   ", a, counter_bytes[a]);
00145 
00146     printf("\n\n\n\n");
00147     /*random number*/
00148     printf("SHA256(encryp_count): ");
00149     for(int i = 0; i < 32; i++)
00150         printf("%02x",encryp_count[i]);
00151     
00152     printf("\n\n");
00153 #endif
00154     
00155     printf("SUCCESS\n");
00156     
00157 }
00158 
00159 
00160 void initAES(void)
00161 {
00162    //initial nonce and counter
00163     for(int i=0; i<BLOCK_SIZE; i++)
00164     {
00165         nonce_counter[i]=i<8?i:0;
00166         iv[i]=0;
00167     }
00168      
00169      //initialize key   
00170     for(int i=0;i<KEYLEN/8;i++)
00171         key[i] = i;
00172 
00173     AesSetKey(&ctx, key, KEYLEN/8, iv, AES_ENCRYPTION); 
00174 }
00175 
00176 void encrypt()
00177 {
00178     printf("\nNonceCntr: ");
00179     for(j=0; j<BLOCK_SIZE; j++)
00180         printf("%02x ",nonce_counter[j]);
00181     
00182     unsigned char address[6];// = {0x00,0x00,0x00,0x00,0x00,0x00};
00183    // string s = address.str();
00184     //ble.getAddress(&(Gap::ADDR_TYPE_PUBLIC), address);
00185     Gap::addr_type_t gap_type = Gap::ADDR_TYPE_PUBLIC;
00186     ble.getAddress(&gap_type,address);
00187    
00188    printf("Mac address before: ");
00189    for(int mac = 0; mac < 6; mac++){
00190     printf("%02x: ",address[mac]);
00191    }
00192    printf("\n\n");
00193 
00194     //Encode the counter into the end of the nonce_counter
00195     /*for(int k = BLOCK_SIZE - counter_tx_len; k < BLOCK_SIZE-1; k++){
00196         nonce_counter[k] = k;
00197     }*/
00198    
00199    mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
00200     printf("\nNonceCntr: ");
00201     for(j=0; j<BLOCK_SIZE; j++)
00202         printf("%02x ",nonce_counter[j]);
00203     printf("\n\n");
00204     
00205     printf("\n------------------------OUTPUT---------------------------------\n");
00206     
00207     
00208     printf("\nHash_counter: ");
00209     for(int vvv = 5; vvv < 8; vvv++){
00210         hash_counter[vvv - 5] = out[vvv];
00211         printf("%02x ", hash_counter[vvv - 5]);
00212     }
00213     
00214     printf("\nCounter_bytes: ");
00215     for(int ll = 0; ll < sizeof(counter_bytes); ll++)
00216         printf("%02x ", counter_bytes[ll]);
00217     printf("\n\n");
00218     //exit(0);
00219     printf("SHA256(NONCE): ");
00220     for(int jjj=0; jjj<32; jjj++)
00221         printf("%02x ",out[jjj]);
00222         
00223     printf("\n\n");
00224     
00225    for(int kk = 0; kk < ENCRYP_MAC_SIZE; kk++){
00226        address[kk] = out[kk];
00227     }
00228 
00229     printf("ENCRYPTOR MAC ADDRESS AFTER: ");
00230     for(int v = 0; v < 6; v++){
00231         printf("%02x ",address[v]);
00232     }
00233     
00234     printf("\n\n");
00235     
00236     ble.setAddress(gap_type,address);    
00237         
00238     
00239     
00240    //make sure the following lines are uncommented for full cryptocop
00241     //if(nonce_counter[7]%2){
00242     AesEncrypt(&ctx, nonce_counter, cipher);
00243     //}
00244     for(int i=0;i<BLOCK_SIZE;i++)
00245         cipher[i]^=plain[i];
00246     
00247     //print plaintext and cipher text
00248     printf("\nPlaintext: ");
00249     for(int i =0; i<BLOCK_SIZE; i++)
00250         printf("%02x ", plain[i]);
00251         
00252     printf("\n\n");
00253     
00254     printf("\nCiphertxt: ");
00255     for(j=0; j<BLOCK_SIZE; j++)
00256         printf("%02x ",cipher[j]);
00257 
00258     printf("\n\n");
00259     
00260     //encryptCounter();
00261     //printf("?????\n");
00262 }
00263 
00264 void sense(void)
00265 {
00266 /*
00267 #ifdef DEBUG
00268     printf("COUNTERS AT BEGINNING OF TUX PACKET\n\n\n");
00269     printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
00270     printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
00271     printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
00272 #endif    
00273   */  
00274     data[data_index]=(unsigned char)(hr_out.read()*512);
00275     //printf("%d, %f\n",data[data_index], hr_out.read());  
00276     data_index = (data_index+1)%DATA_SIZE;  
00277     
00278 /*#ifdef DEBUG    
00279     printf("COUNTERS AT END OF TUX PACKET\n\n\n");
00280     printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
00281     printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
00282     printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
00283 #endif
00284 */
00285 }
00286 
00287 void tx_packet(void)
00288 {
00289 
00290 #ifdef DEBUG    
00291     packet_ctr++;
00292     printf("PACKET COUNTER: %d\n\n",packet_ctr);
00293 #endif
00294    
00295         
00296     ble.clearAdvertisingPayload();
00297     
00298     //fill plain text buffer
00299     for(int i =0; i<BLOCK_SIZE; i++)
00300     {
00301         plain[i] = data[last_txd];
00302         last_txd = (last_txd + 1) % DATA_SIZE;
00303     }
00304     encrypt();
00305 #ifdef DEBUG
00306     unsigned char p[BLOCK_SIZE];
00307     for(int v = 0; v < BLOCK_SIZE; v++)
00308         p[v] = v;
00309 #endif
00310     //build payload
00311     memcpy(payload, cipher, BLOCK_SIZE);
00312 
00313 #ifdef DEBUG
00314     printf(" BYTES PRINTING\n");
00315     for(int ctr = 0; ctr < sizeof(counter_bytes); ctr++){
00316         printf("counter_bytes[%d]: %02x", ctr, counter_bytes[ctr]);
00317     }
00318     
00319     unsigned char eurek[3] = {0x90, 0x90, 0x90};
00320 #endif
00321     //Second half of the nonce_ctr will contian the ctr
00322     
00323     printf("-------------------WHAT WE CARE ABOUT--------------------------\n");
00324 #ifndef HASH_CTR
00325     printf("-------------------BEFORE ENCRYPTION---------------------------");
00326 
00327     //memcpy(payload+BLOCK_SIZE,counter_bytes+BLOCK_SIZE/2-counter_tx_len,counter_tx_len);
00328     memcpy(payload+BLOCK_SIZE, hash_counter, counter_tx_len);
00329     /* setup advertising */
00330         
00331     ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
00332                                      payload, BLOCK_SIZE+counter_tx_len); 
00333 #endif 
00334                                      
00335     //print payload
00336     printf("\nPayload:   ");
00337     for(int i=0; i<BLOCK_SIZE+counter_tx_len; i++)
00338         printf("%02x ",payload[i]);
00339     ble.startAdvertising();
00340     
00341     //increment counter     
00342    j=7;
00343    int num_cycles = 0;
00344    
00345    
00346    printf("COUNTER_BYTES PRINTING\n\n\n");
00347     do
00348     {
00349        // printf("num_cycles: %d",num_cycles);
00350         
00351         printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
00352         printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
00353         printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
00354        for(j = 5; j <= 7; j++)
00355         counter_bytes[j]++;
00356         
00357        printf("BEFORE RANDOMIZATION: counter_bytes[7]: %02x  \n", counter_bytes[7]);
00358        //encryptCounter();
00359        printf("AFTER RANDOMIZATION counter counter_bytes[7]: %02x  \n", counter_bytes[7]);
00360        
00361         printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
00362         printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
00363         printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
00364         printf("WHILE\n");
00365        
00366     } while(counter_bytes[j--] == 0);
00367     
00368 #ifdef DEBUG
00369     if(counter_bytes[7] == 0x99 || counter_bytes[7] == 153){
00370         exit(0);
00371     }
00372 #endif
00373 
00374 
00375     
00376 #ifdef DEBUG
00377 
00378 
00379     printf("Printing hash_count!!!\n"); 
00380     for(int hash = 0; hash < 32; hash++){
00381            printf("hash_count[%d]: %02x",hash,hash_count[hash]);
00382     }
00383 #endif
00384     printf("\n\n");
00385 }
00386 
00387 void bleInitComplete(void)
00388 {
00389     /* setup advertising */
00390     
00391     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00392     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00393     
00394     ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
00395                                      (const uint8_t*)"bob is ready", sizeof("bob is ready"));
00396                                      
00397     ble.setAdvertisingInterval(9600); /* 2s; in multiples of 0.625ms. */
00398     ble.startAdvertising();
00399 }
00400 
00401 int main(void)
00402 {
00403 
00404     packet_ctr = 0;
00405    // pc.baud(BAUDR);
00406     printf("---- ENCRYPTULATOR ACTIVIZE ----\n");
00407 
00408     initAES();
00409 
00410     printf("Bring up the BLE radio\n");
00411     ble.init();
00412     
00413     srand(time(NULL));
00414     
00415     //replace with sensor data call
00416     for(int i =0; i<BLOCK_SIZE; i++)
00417         plain[i] = i+3;
00418     
00419     //uart stuff
00420     DFUService dfu(ble);                                 
00421     UARTService uartService(ble);
00422     uartServicePtr = &uartService;
00423 
00424     bleInitComplete();
00425     
00426     //maybe replace with something inside sensor read?  Basically trigger sending
00427     sensor_timeout.attach(&sense, 0.025);
00428     tx_timeout.attach(&tx_packet, 0.4);
00429     
00430     printf("EXITING ENTIRE LOOP\n\n\n");
00431    // exit(0);
00432     
00433     while (true) {
00434             ble.waitForEvent();
00435     }
00436     
00437 }