Varun Bahl / Mbed 2 deprecated Encryptulator2

Encryp_u

Dependencies:   BLE_API_Encryp CyaSSL-Encryp eMPL_MPU6050 mbed

Fork of Encryptulator2 by Mobius IoT

main.cpp@4:bfb662bab28e, 2017-05-10 (annotated)

Committer:
vbahl2
Date:
Wed May 10 18:23:44 2017 +0000
Revision:
4:bfb662bab28e
Parent:
3:b48570121d3f 
en

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vbahl2 3:b48570121d3f 1 #include <string>
yihui 0:26da608265f8 2 #include "mbed.h"
yihui 0:26da608265f8 3 #include "mbed_i2c.h"
yihui 0:26da608265f8 4 #include "inv_mpu.h"
yihui 0:26da608265f8 5 #include "inv_mpu_dmp_motion_driver.h"
vbahl2 3:b48570121d3f 6 //#include "ble/BLE.h"
vbahl2 3:b48570121d3f 7 //#include "ble/BLEProtocol.h"
yihui 0:26da608265f8 8
yihui 0:26da608265f8 9 #include "BLEDevice.h"
yihui 0:26da608265f8 10 #include "DFUService.h"
yihui 0:26da608265f8 11 #include "UARTService.h"
budoguyiii 2:e1a6b317241e 12 #include "ctc_aes.h"
vbahl2 3:b48570121d3f 13 #include "newSha256.h"
vbahl2 3:b48570121d3f 14 #include "time.h"
yihui 0:26da608265f8 15
budoguyiii 2:e1a6b317241e 16 #define BAUDR 115200
budoguyiii 2:e1a6b317241e 17 //#define BAUDR 4000000
yihui 0:26da608265f8 18 #define LOG(...) { pc.printf(__VA_ARGS__); }
budoguyiii 2:e1a6b317241e 19 //#define LOG(...)
yihui 0:26da608265f8 20
budoguyiii 2:e1a6b317241e 21 #define HR_OUT p4
budoguyiii 2:e1a6b317241e 22 #define HR_LOP p6
budoguyiii 2:e1a6b317241e 23 #define HR_LOM p5
budoguyiii 2:e1a6b317241e 24 //#define LED_GREEN p21
budoguyiii 2:e1a6b317241e 25 //#define LED_RED p22
budoguyiii 2:e1a6b317241e 26 //#define LED_BLUE p23
budoguyiii 2:e1a6b317241e 27 //#define BUTTON_PIN p17
budoguyiii 2:e1a6b317241e 28 //#define BATTERY_PIN p1
yihui 0:26da608265f8 29
budoguyiii 2:e1a6b317241e 30 //#define MPU6050_SDA p12
budoguyiii 2:e1a6b317241e 31 //#define MPU6050_SCL p13
yihui 0:26da608265f8 32
yihui 0:26da608265f8 33 #define UART_TX p9
yihui 0:26da608265f8 34 #define UART_RX p11
budoguyiii 2:e1a6b317241e 35 //#define UART_CTS p8
budoguyiii 2:e1a6b317241e 36 //#define UART_RTS p10
yihui 0:26da608265f8 37
yihui 0:26da608265f8 38 /* Starting sampling rate. */
budoguyiii 2:e1a6b317241e 39 //#define DEFAULT_MPU_HZ (100)
yihui 0:26da608265f8 40
budoguyiii 2:e1a6b317241e 41 //DigitalOut blue(LED_BLUE);
budoguyiii 2:e1a6b317241e 42 //DigitalOut green(LED_GREEN);
budoguyiii 2:e1a6b317241e 43 //DigitalOut red(LED_RED);
yihui 0:26da608265f8 44
budoguyiii 2:e1a6b317241e 45 //InterruptIn button(BUTTON_PIN);
budoguyiii 2:e1a6b317241e 46 AnalogIn hr_out(HR_OUT);
budoguyiii 2:e1a6b317241e 47 DigitalIn hr_lop(HR_LOP);
budoguyiii 2:e1a6b317241e 48 DigitalIn hr_lom(HR_LOM);
yihui 0:26da608265f8 49 Serial pc(UART_TX, UART_RX);
yihui 0:26da608265f8 50
budoguyiii 2:e1a6b317241e 51 //InterruptIn motion_probe(p14);
yihui 0:26da608265f8 52
yihui 0:26da608265f8 53 BLEDevice ble;
yihui 0:26da608265f8 54 UARTService *uartServicePtr;
budoguyiii 2:e1a6b317241e 55 Ticker tx_timeout, sensor_timeout;
yihui 0:26da608265f8 56
budoguyiii 2:e1a6b317241e 57 //stuff for encryption
budoguyiii 2:e1a6b317241e 58 uint8_t payload[31];
vbahl2 3:b48570121d3f 59 #define CTR_SIZE 3
vbahl2 3:b48570121d3f 60 #define ENCRYP_MAC_SIZE 6
budoguyiii 2:e1a6b317241e 61 #define BLOCK_SIZE 16
budoguyiii 2:e1a6b317241e 62 #define DATA_SIZE 256
budoguyiii 2:e1a6b317241e 63 #define KEYLEN 256 //128, 192, 256
budoguyiii 2:e1a6b317241e 64 int j;
budoguyiii 2:e1a6b317241e 65 unsigned char nonce_counter[BLOCK_SIZE];
budoguyiii 2:e1a6b317241e 66 unsigned char plain[BLOCK_SIZE];
budoguyiii 2:e1a6b317241e 67 unsigned char cipher[BLOCK_SIZE];
budoguyiii 2:e1a6b317241e 68 unsigned char* counter_bytes = nonce_counter+BLOCK_SIZE/2;
vbahl2 3:b48570121d3f 69
vbahl2 3:b48570121d3f 70 #ifdef DEBUG
vbahl2 3:b48570121d3f 71 unsigned char * hash_count = counter_bytes;
vbahl2 3:b48570121d3f 72 #endif
budoguyiii 2:e1a6b317241e 73 size_t counter_tx_len = 3;
budoguyiii 2:e1a6b317241e 74 unsigned char key[KEYLEN/8];
budoguyiii 2:e1a6b317241e 75 unsigned char iv[BLOCK_SIZE];//not used for ctr mode but required by setKey
budoguyiii 2:e1a6b317241e 76 Aes ctx;
budoguyiii 2:e1a6b317241e 77 unsigned char data[DATA_SIZE];
budoguyiii 2:e1a6b317241e 78 unsigned int data_index = 0;
budoguyiii 2:e1a6b317241e 79 unsigned int last_txd = 0;
vbahl2 3:b48570121d3f 80 unsigned char out[32];
vbahl2 3:b48570121d3f 81 unsigned char hash_counter[CTR_SIZE];
yihui 0:26da608265f8 82
vbahl2 3:b48570121d3f 83 unsigned int packet_ctr;
vbahl2 3:b48570121d3f 84
vbahl2 3:b48570121d3f 85 //unsigned int temp_ctr = {0x90,0x90,0x90};
vbahl2 3:b48570121d3f 86 //#define DEBUG 1
vbahl2 3:b48570121d3f 87 //#define HASH_CTR 1
yihui 0:26da608265f8 88
budoguyiii 2:e1a6b317241e 89 volatile bool bleIsConnected = false;
yihui 0:26da608265f8 90
vbahl2 3:b48570121d3f 91 void encryptCounter(void){
vbahl2 3:b48570121d3f 92
vbahl2 3:b48570121d3f 93 printf("ENCRYPTING COUNTER\n\n\n");
vbahl2 3:b48570121d3f 94 unsigned char encryp_count[BLOCK_SIZE];
vbahl2 3:b48570121d3f 95 unsigned char e_b[32];
vbahl2 3:b48570121d3f 96 /**nonce information*/
vbahl2 3:b48570121d3f 97 for(int i = 0 ; i < BLOCK_SIZE; i++)
vbahl2 3:b48570121d3f 98 encryp_count[i] = nonce_counter[i];
vbahl2 3:b48570121d3f 99
vbahl2 3:b48570121d3f 100 /* printf("counter_bytes[5]: %02x\n" , counter_bytes[5]);
vbahl2 3:b48570121d3f 101 printf("counter_bytes[6]: %02x\n" , counter_bytes[6]);
vbahl2 3:b48570121d3f 102 printf("counter_bytes[7]: %02x\n" , counter_bytes[7]);*/
vbahl2 3:b48570121d3f 103 // mbedtls_sha256(nonce_counter, sizeof(nonce_counter), e_b, 0);
vbahl2 3:b48570121d3f 104
vbahl2 3:b48570121d3f 105 for(int i = 0; i < 3;)
vbahl2 3:b48570121d3f 106 hash_counter[i] = e_b[i];
vbahl2 3:b48570121d3f 107
vbahl2 3:b48570121d3f 108 //exit(0);
vbahl2 3:b48570121d3f 109 #ifdef DEBUG
vbahl2 3:b48570121d3f 110 for(int i = 0; i < 3; i++){
vbahl2 3:b48570121d3f 111 if(hash_counter[i] != encryp_count[i]){
vbahl2 3:b48570121d3f 112 printf("FUCK\n");
vbahl2 3:b48570121d3f 113 exit(0);
vbahl2 3:b48570121d3f 114 }
vbahl2 3:b48570121d3f 115
vbahl2 3:b48570121d3f 116 }
vbahl2 3:b48570121d3f 117
vbahl2 3:b48570121d3f 118
vbahl2 3:b48570121d3f 119
vbahl2 3:b48570121d3f 120 printf("NONCE COUNTER: \n");
vbahl2 3:b48570121d3f 121 for(int i = 0; i < sizeof(nonce_counter); i++)
vbahl2 3:b48570121d3f 122 printf("nonce_counter[%d]: %02x ", i, nonce_counter[i]);
vbahl2 3:b48570121d3f 123 printf("\n\n");
vbahl2 3:b48570121d3f 124
vbahl2 3:b48570121d3f 125 printf("-----------------SMALLER SIZE HASH_COUNTER----------------\n");
vbahl2 3:b48570121d3f 126 for(int i = 0; i < sizeof(hash_counter); i++)
vbahl2 3:b48570121d3f 127 printf("hash_counter[%d]: %02x ", i, hash_counter[i]);
vbahl2 3:b48570121d3f 128 printf("\n\n");
vbahl2 3:b48570121d3f 129 printf("-----------------LARGER SIZE HASH_COUNTER-----------------\n");
vbahl2 3:b48570121d3f 130 mbedtls_sha256(nonce_counter, sizeof(nonce_counter), encryp_count, 0);
vbahl2 3:b48570121d3f 131 for(int k = 0; k < sizeof(encryp_count); k++)
vbahl2 3:b48570121d3f 132 printf("encryp_count[%d]: %02x ", k, encryp_count[k]);
vbahl2 3:b48570121d3f 133 printf("\n\n");
vbahl2 3:b48570121d3f 134
vbahl2 3:b48570121d3f 135
vbahl2 3:b48570121d3f 136 printf("NONCE COUNTER: \n");
vbahl2 3:b48570121d3f 137 for(int i = 0; i < sizeof(nonce_counter); i++)
vbahl2 3:b48570121d3f 138 printf("nonce_counter[%d]: %02x ", i, nonce_counter[i]);
vbahl2 3:b48570121d3f 139
vbahl2 3:b48570121d3f 140 printf("\n\n");
vbahl2 3:b48570121d3f 141
vbahl2 3:b48570121d3f 142 printf("PLAINTEXT: ");
vbahl2 3:b48570121d3f 143 for(int a = 0; a < 8; a++)
vbahl2 3:b48570121d3f 144 printf("counter_bytes[%d]: %02x ", a, counter_bytes[a]);
vbahl2 3:b48570121d3f 145
vbahl2 3:b48570121d3f 146 printf("\n\n\n\n");
vbahl2 3:b48570121d3f 147 /*random number*/
vbahl2 3:b48570121d3f 148 printf("SHA256(encryp_count): ");
vbahl2 3:b48570121d3f 149 for(int i = 0; i < 32; i++)
vbahl2 3:b48570121d3f 150 printf("%02x",encryp_count[i]);
vbahl2 3:b48570121d3f 151
vbahl2 3:b48570121d3f 152 printf("\n\n");
vbahl2 3:b48570121d3f 153 #endif
vbahl2 3:b48570121d3f 154
vbahl2 3:b48570121d3f 155 printf("SUCCESS\n");
vbahl2 3:b48570121d3f 156
vbahl2 3:b48570121d3f 157 }
vbahl2 3:b48570121d3f 158
vbahl2 3:b48570121d3f 159
budoguyiii 2:e1a6b317241e 160 void initAES(void)
yihui 0:26da608265f8 161 {
budoguyiii 2:e1a6b317241e 162 //initial nonce and counter
budoguyiii 2:e1a6b317241e 163 for(int i=0; i<BLOCK_SIZE; i++)
budoguyiii 2:e1a6b317241e 164 {
budoguyiii 2:e1a6b317241e 165 nonce_counter[i]=i<8?i:0;
budoguyiii 2:e1a6b317241e 166 iv[i]=0;
budoguyiii 2:e1a6b317241e 167 }
budoguyiii 2:e1a6b317241e 168
budoguyiii 2:e1a6b317241e 169 //initialize key
budoguyiii 2:e1a6b317241e 170 for(int i=0;i<KEYLEN/8;i++)
budoguyiii 2:e1a6b317241e 171 key[i] = i;
yihui 0:26da608265f8 172
budoguyiii 2:e1a6b317241e 173 AesSetKey(&ctx, key, KEYLEN/8, iv, AES_ENCRYPTION);
yihui 0:26da608265f8 174 }
yihui 0:26da608265f8 175
budoguyiii 2:e1a6b317241e 176 void encrypt()
yihui 0:26da608265f8 177 {
vbahl2 3:b48570121d3f 178 printf("\nNonceCntr: ");
budoguyiii 2:e1a6b317241e 179 for(j=0; j<BLOCK_SIZE; j++)
vbahl2 3:b48570121d3f 180 printf("%02x ",nonce_counter[j]);
budoguyiii 2:e1a6b317241e 181
vbahl2 3:b48570121d3f 182 unsigned char address[6];// = {0x00,0x00,0x00,0x00,0x00,0x00};
vbahl2 3:b48570121d3f 183 // string s = address.str();
vbahl2 3:b48570121d3f 184 //ble.getAddress(&(Gap::ADDR_TYPE_PUBLIC), address);
vbahl2 3:b48570121d3f 185 Gap::addr_type_t gap_type = Gap::ADDR_TYPE_PUBLIC;
vbahl2 3:b48570121d3f 186 ble.getAddress(&gap_type,address);
vbahl2 3:b48570121d3f 187
vbahl2 3:b48570121d3f 188 printf("Mac address before: ");
vbahl2 3:b48570121d3f 189 for(int mac = 0; mac < 6; mac++){
vbahl2 3:b48570121d3f 190 printf("%02x: ",address[mac]);
vbahl2 3:b48570121d3f 191 }
vbahl2 3:b48570121d3f 192 printf("\n\n");
vbahl2 3:b48570121d3f 193
vbahl2 3:b48570121d3f 194 //Encode the counter into the end of the nonce_counter
vbahl2 3:b48570121d3f 195 /*for(int k = BLOCK_SIZE - counter_tx_len; k < BLOCK_SIZE-1; k++){
vbahl2 3:b48570121d3f 196 nonce_counter[k] = k;
vbahl2 3:b48570121d3f 197 }*/
vbahl2 3:b48570121d3f 198
vbahl2 3:b48570121d3f 199 mbedtls_sha256(nonce_counter, sizeof(nonce_counter), out, 0);
vbahl2 3:b48570121d3f 200 printf("\nNonceCntr: ");
vbahl2 3:b48570121d3f 201 for(j=0; j<BLOCK_SIZE; j++)
vbahl2 3:b48570121d3f 202 printf("%02x ",nonce_counter[j]);
vbahl2 3:b48570121d3f 203 printf("\n\n");
vbahl2 3:b48570121d3f 204
vbahl2 3:b48570121d3f 205 printf("\n------------------------OUTPUT---------------------------------\n");
budoguyiii 2:e1a6b317241e 206
budoguyiii 2:e1a6b317241e 207
vbahl2 3:b48570121d3f 208 printf("\nHash_counter: ");
vbahl2 3:b48570121d3f 209 for(int vvv = 5; vvv < 8; vvv++){
vbahl2 3:b48570121d3f 210 hash_counter[vvv - 5] = out[vvv];
vbahl2 3:b48570121d3f 211 printf("%02x ", hash_counter[vvv - 5]);
vbahl2 3:b48570121d3f 212 }
vbahl2 3:b48570121d3f 213
vbahl2 3:b48570121d3f 214 printf("\nCounter_bytes: ");
vbahl2 3:b48570121d3f 215 for(int ll = 0; ll < sizeof(counter_bytes); ll++)
vbahl2 3:b48570121d3f 216 printf("%02x ", counter_bytes[ll]);
vbahl2 3:b48570121d3f 217 printf("\n\n");
vbahl2 3:b48570121d3f 218 //exit(0);
vbahl2 3:b48570121d3f 219 printf("SHA256(NONCE): ");
vbahl2 3:b48570121d3f 220 for(int jjj=0; jjj<32; jjj++)
vbahl2 3:b48570121d3f 221 printf("%02x ",out[jjj]);
vbahl2 3:b48570121d3f 222
vbahl2 3:b48570121d3f 223 printf("\n\n");
vbahl2 3:b48570121d3f 224
vbahl2 3:b48570121d3f 225 for(int kk = 0; kk < ENCRYP_MAC_SIZE; kk++){
vbahl2 3:b48570121d3f 226 address[kk] = out[kk];
vbahl2 3:b48570121d3f 227 }
vbahl2 3:b48570121d3f 228
vbahl2 3:b48570121d3f 229 printf("ENCRYPTOR MAC ADDRESS AFTER: ");
vbahl2 3:b48570121d3f 230 for(int v = 0; v < 6; v++){
vbahl2 3:b48570121d3f 231 printf("%02x ",address[v]);
vbahl2 3:b48570121d3f 232 }
vbahl2 3:b48570121d3f 233
vbahl2 3:b48570121d3f 234 printf("\n\n");
vbahl2 3:b48570121d3f 235
vbahl2 3:b48570121d3f 236 ble.setAddress(gap_type,address);
vbahl2 3:b48570121d3f 237
vbahl2 3:b48570121d3f 238
vbahl2 3:b48570121d3f 239
budoguyiii 2:e1a6b317241e 240 //make sure the following lines are uncommented for full cryptocop
budoguyiii 2:e1a6b317241e 241 //if(nonce_counter[7]%2){
vbahl2 3:b48570121d3f 242 AesEncrypt(&ctx, nonce_counter, cipher);
vbahl2 3:b48570121d3f 243 //}
budoguyiii 2:e1a6b317241e 244 for(int i=0;i<BLOCK_SIZE;i++)
budoguyiii 2:e1a6b317241e 245 cipher[i]^=plain[i];
budoguyiii 2:e1a6b317241e 246
budoguyiii 2:e1a6b317241e 247 //print plaintext and cipher text
vbahl2 3:b48570121d3f 248 printf("\nPlaintext: ");
budoguyiii 2:e1a6b317241e 249 for(int i =0; i<BLOCK_SIZE; i++)
vbahl2 3:b48570121d3f 250 printf("%02x ", plain[i]);
vbahl2 3:b48570121d3f 251
vbahl2 3:b48570121d3f 252 printf("\n\n");
vbahl2 3:b48570121d3f 253
vbahl2 3:b48570121d3f 254 printf("\nCiphertxt: ");
budoguyiii 2:e1a6b317241e 255 for(j=0; j<BLOCK_SIZE; j++)
vbahl2 3:b48570121d3f 256 printf("%02x ",cipher[j]);
yihui 0:26da608265f8 257
vbahl2 3:b48570121d3f 258 printf("\n\n");
vbahl2 3:b48570121d3f 259
vbahl2 3:b48570121d3f 260 //encryptCounter();
vbahl2 3:b48570121d3f 261 //printf("?????\n");
yihui 0:26da608265f8 262 }
yihui 0:26da608265f8 263
budoguyiii 2:e1a6b317241e 264 void sense(void)
budoguyiii 2:e1a6b317241e 265 {
vbahl2 3:b48570121d3f 266 /*
vbahl2 3:b48570121d3f 267 #ifdef DEBUG
vbahl2 3:b48570121d3f 268 printf("COUNTERS AT BEGINNING OF TUX PACKET\n\n\n");
vbahl2 3:b48570121d3f 269 printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
vbahl2 3:b48570121d3f 270 printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
vbahl2 3:b48570121d3f 271 printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
vbahl2 3:b48570121d3f 272 #endif
vbahl2 3:b48570121d3f 273 */
budoguyiii 2:e1a6b317241e 274 data[data_index]=(unsigned char)(hr_out.read()*512);
vbahl2 3:b48570121d3f 275 //printf("%d, %f\n",data[data_index], hr_out.read());
budoguyiii 2:e1a6b317241e 276 data_index = (data_index+1)%DATA_SIZE;
vbahl2 3:b48570121d3f 277
vbahl2 3:b48570121d3f 278 /*#ifdef DEBUG
vbahl2 3:b48570121d3f 279 printf("COUNTERS AT END OF TUX PACKET\n\n\n");
vbahl2 3:b48570121d3f 280 printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
vbahl2 3:b48570121d3f 281 printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
vbahl2 3:b48570121d3f 282 printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
vbahl2 3:b48570121d3f 283 #endif
vbahl2 3:b48570121d3f 284 */
budoguyiii 2:e1a6b317241e 285 }
vbahl2 3:b48570121d3f 286
budoguyiii 2:e1a6b317241e 287 void tx_packet(void)
yihui 0:26da608265f8 288 {
vbahl2 3:b48570121d3f 289
vbahl2 3:b48570121d3f 290 #ifdef DEBUG
vbahl2 3:b48570121d3f 291 packet_ctr++;
vbahl2 3:b48570121d3f 292 printf("PACKET COUNTER: %d\n\n",packet_ctr);
vbahl2 3:b48570121d3f 293 #endif
vbahl2 3:b48570121d3f 294
vbahl2 3:b48570121d3f 295
budoguyiii 2:e1a6b317241e 296 ble.clearAdvertisingPayload();
vbahl2 3:b48570121d3f 297
budoguyiii 2:e1a6b317241e 298 //fill plain text buffer
budoguyiii 2:e1a6b317241e 299 for(int i =0; i<BLOCK_SIZE; i++)
budoguyiii 2:e1a6b317241e 300 {
budoguyiii 2:e1a6b317241e 301 plain[i] = data[last_txd];
budoguyiii 2:e1a6b317241e 302 last_txd = (last_txd + 1) % DATA_SIZE;
budoguyiii 2:e1a6b317241e 303 }
budoguyiii 2:e1a6b317241e 304 encrypt();
vbahl2 3:b48570121d3f 305 #ifdef DEBUG
vbahl2 3:b48570121d3f 306 unsigned char p[BLOCK_SIZE];
vbahl2 3:b48570121d3f 307 for(int v = 0; v < BLOCK_SIZE; v++)
vbahl2 3:b48570121d3f 308 p[v] = v;
vbahl2 3:b48570121d3f 309 #endif
budoguyiii 2:e1a6b317241e 310 //build payload
budoguyiii 2:e1a6b317241e 311 memcpy(payload, cipher, BLOCK_SIZE);
vbahl2 3:b48570121d3f 312
vbahl2 3:b48570121d3f 313 #ifdef DEBUG
vbahl2 3:b48570121d3f 314 printf(" BYTES PRINTING\n");
vbahl2 3:b48570121d3f 315 for(int ctr = 0; ctr < sizeof(counter_bytes); ctr++){
vbahl2 3:b48570121d3f 316 printf("counter_bytes[%d]: %02x", ctr, counter_bytes[ctr]);
vbahl2 3:b48570121d3f 317 }
vbahl2 3:b48570121d3f 318
vbahl2 3:b48570121d3f 319 unsigned char eurek[3] = {0x90, 0x90, 0x90};
vbahl2 3:b48570121d3f 320 #endif
vbahl2 3:b48570121d3f 321 //Second half of the nonce_ctr will contian the ctr
vbahl2 3:b48570121d3f 322
vbahl2 3:b48570121d3f 323 printf("-------------------WHAT WE CARE ABOUT--------------------------\n");
vbahl2 3:b48570121d3f 324 #ifndef HASH_CTR
vbahl2 3:b48570121d3f 325 printf("-------------------BEFORE ENCRYPTION---------------------------");
vbahl2 3:b48570121d3f 326
vbahl2 3:b48570121d3f 327 //memcpy(payload+BLOCK_SIZE,counter_bytes+BLOCK_SIZE/2-counter_tx_len,counter_tx_len);
vbahl2 3:b48570121d3f 328 memcpy(payload+BLOCK_SIZE, hash_counter, counter_tx_len);
budoguyiii 2:e1a6b317241e 329 /* setup advertising */
vbahl2 3:b48570121d3f 330
budoguyiii 2:e1a6b317241e 331 ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
budoguyiii 2:e1a6b317241e 332 payload, BLOCK_SIZE+counter_tx_len);
vbahl2 3:b48570121d3f 333 #endif
budoguyiii 2:e1a6b317241e 334
budoguyiii 2:e1a6b317241e 335 //print payload
vbahl2 3:b48570121d3f 336 printf("\nPayload: ");
budoguyiii 2:e1a6b317241e 337 for(int i=0; i<BLOCK_SIZE+counter_tx_len; i++)
vbahl2 3:b48570121d3f 338 printf("%02x ",payload[i]);
budoguyiii 2:e1a6b317241e 339 ble.startAdvertising();
vbahl2 3:b48570121d3f 340
vbahl2 3:b48570121d3f 341 //increment counter
vbahl2 3:b48570121d3f 342 j=7;
vbahl2 3:b48570121d3f 343 int num_cycles = 0;
vbahl2 3:b48570121d3f 344
vbahl2 3:b48570121d3f 345
vbahl2 3:b48570121d3f 346 printf("COUNTER_BYTES PRINTING\n\n\n");
budoguyiii 2:e1a6b317241e 347 do
budoguyiii 2:e1a6b317241e 348 {
vbahl2 3:b48570121d3f 349 // printf("num_cycles: %d",num_cycles);
vbahl2 3:b48570121d3f 350
vbahl2 3:b48570121d3f 351 printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
vbahl2 3:b48570121d3f 352 printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
vbahl2 3:b48570121d3f 353 printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
vbahl2 3:b48570121d3f 354 for(j = 5; j <= 7; j++)
budoguyiii 2:e1a6b317241e 355 counter_bytes[j]++;
vbahl2 3:b48570121d3f 356
vbahl2 3:b48570121d3f 357 printf("BEFORE RANDOMIZATION: counter_bytes[7]: %02x \n", counter_bytes[7]);
vbahl2 3:b48570121d3f 358 //encryptCounter();
vbahl2 3:b48570121d3f 359 printf("AFTER RANDOMIZATION counter counter_bytes[7]: %02x \n", counter_bytes[7]);
vbahl2 3:b48570121d3f 360
vbahl2 3:b48570121d3f 361 printf("counter_bytes[5]: %02x\n", counter_bytes[5]);
vbahl2 3:b48570121d3f 362 printf("counter_bytes[6]: %02x\n", counter_bytes[6]);
vbahl2 3:b48570121d3f 363 printf("counter_bytes[7]: %02x\n",counter_bytes[7]);
vbahl2 3:b48570121d3f 364 printf("WHILE\n");
vbahl2 3:b48570121d3f 365
budoguyiii 2:e1a6b317241e 366 } while(counter_bytes[j--] == 0);
budoguyiii 2:e1a6b317241e 367
vbahl2 3:b48570121d3f 368 #ifdef DEBUG
vbahl2 3:b48570121d3f 369 if(counter_bytes[7] == 0x99 || counter_bytes[7] == 153){
vbahl2 3:b48570121d3f 370 exit(0);
vbahl2 3:b48570121d3f 371 }
vbahl2 3:b48570121d3f 372 #endif
vbahl2 3:b48570121d3f 373
vbahl2 3:b48570121d3f 374
vbahl2 3:b48570121d3f 375
vbahl2 3:b48570121d3f 376 #ifdef DEBUG
vbahl2 3:b48570121d3f 377
vbahl2 3:b48570121d3f 378
vbahl2 3:b48570121d3f 379 printf("Printing hash_count!!!\n");
vbahl2 3:b48570121d3f 380 for(int hash = 0; hash < 32; hash++){
vbahl2 3:b48570121d3f 381 printf("hash_count[%d]: %02x",hash,hash_count[hash]);
vbahl2 3:b48570121d3f 382 }
vbahl2 3:b48570121d3f 383 #endif
vbahl2 3:b48570121d3f 384 printf("\n\n");
yihui 0:26da608265f8 385 }
yihui 0:26da608265f8 386
budoguyiii 2:e1a6b317241e 387 void bleInitComplete(void)
yihui 0:26da608265f8 388 {
budoguyiii 2:e1a6b317241e 389 /* setup advertising */
budoguyiii 2:e1a6b317241e 390
budoguyiii 2:e1a6b317241e 391 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
budoguyiii 2:e1a6b317241e 392 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
budoguyiii 2:e1a6b317241e 393
budoguyiii 2:e1a6b317241e 394 ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
budoguyiii 2:e1a6b317241e 395 (const uint8_t*)"bob is ready", sizeof("bob is ready"));
budoguyiii 2:e1a6b317241e 396
vbahl2 3:b48570121d3f 397 ble.setAdvertisingInterval(9600); /* 2s; in multiples of 0.625ms. */
budoguyiii 2:e1a6b317241e 398 ble.startAdvertising();
yihui 0:26da608265f8 399 }
yihui 0:26da608265f8 400
yihui 0:26da608265f8 401 int main(void)
yihui 0:26da608265f8 402 {
yihui 0:26da608265f8 403
vbahl2 3:b48570121d3f 404 packet_ctr = 0;
vbahl2 3:b48570121d3f 405 // pc.baud(BAUDR);
vbahl2 3:b48570121d3f 406 printf("---- ENCRYPTULATOR ACTIVIZE ----\n");
yihui 0:26da608265f8 407
budoguyiii 2:e1a6b317241e 408 initAES();
yihui 0:26da608265f8 409
vbahl2 3:b48570121d3f 410 printf("Bring up the BLE radio\n");
yihui 0:26da608265f8 411 ble.init();
budoguyiii 2:e1a6b317241e 412
vbahl2 3:b48570121d3f 413 srand(time(NULL));
budoguyiii 2:e1a6b317241e 414
budoguyiii 2:e1a6b317241e 415 //replace with sensor data call
budoguyiii 2:e1a6b317241e 416 for(int i =0; i<BLOCK_SIZE; i++)
budoguyiii 2:e1a6b317241e 417 plain[i] = i+3;
budoguyiii 2:e1a6b317241e 418
budoguyiii 2:e1a6b317241e 419 //uart stuff
yihui 0:26da608265f8 420 DFUService dfu(ble);
yihui 0:26da608265f8 421 UARTService uartService(ble);
yihui 0:26da608265f8 422 uartServicePtr = &uartService;
yihui 0:26da608265f8 423
budoguyiii 2:e1a6b317241e 424 bleInitComplete();
yihui 0:26da608265f8 425
budoguyiii 2:e1a6b317241e 426 //maybe replace with something inside sensor read? Basically trigger sending
budoguyiii 2:e1a6b317241e 427 sensor_timeout.attach(&sense, 0.025);
budoguyiii 2:e1a6b317241e 428 tx_timeout.attach(&tx_packet, 0.4);
yihui 0:26da608265f8 429
vbahl2 3:b48570121d3f 430 printf("EXITING ENTIRE LOOP\n\n\n");
vbahl2 3:b48570121d3f 431 // exit(0);
yihui 0:26da608265f8 432
yihui 0:26da608265f8 433 while (true) {
yihui 0:26da608265f8 434 ble.waitForEvent();
yihui 0:26da608265f8 435 }
vbahl2 3:b48570121d3f 436
yihui 0:26da608265f8 437 }