Diff: main.cpp

Revision:

12:dbbf0ddc9b12

Parent:

11:16f67d5752e1

Child:

13:8b706583610a

--- a/main.cpp	Tue Jan 12 10:20:26 2016 +0000
+++ b/main.cpp	Sat May 28 22:07:19 2016 +0000
@@ -1,67 +1,123 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2006-2015 ARM Limited
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 
 #include "mbed.h"
-#include "toolchain.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)
 {
-    struct AdvertisingData_t {
-        uint8_t                        length; /* doesn't include itself */
-        GapAdvertisingData::DataType_t dataType;
-        uint8_t                        data[0];
-    } PACKED;
-
-    struct ApplicationData_t {
-        uint16_t applicationSpecificId;             /* An ID used to identify temperature value
-                                                       in the manufacture specific AD data field */
-        TMP_nrf51::TempSensorValue_t tmpSensorValue; /* User defined application data */
-    } PACKED;
-
-    static const uint16_t APP_SPECIFIC_ID_TEST = 0xFEFE;
-
+    
     /* Search for the manufacturer specific data with matching application-ID */
-    AdvertisingData_t *pAdvData;
-    size_t index = 0;
-    while (index < params->advertisingDataLen) {
-        pAdvData = (AdvertisingData_t *)&params->advertisingData[index];
-        if (pAdvData->dataType == GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA) {
-            ApplicationData_t *pAppData = (ApplicationData_t *)pAdvData->data;
-            if (pAppData->applicationSpecificId == APP_SPECIFIC_ID_TEST) {
-                /* dump information on the console. */
-                printf("From [%02x %02x %02x], ", params->peerAddr[2], params->peerAddr[1], params->peerAddr[0]);
-                printf("Temp is %f\r\n", (TMP_nrf51::TempSensorValue_t)pAppData->tmpSensorValue);
-                break;
-            }
-        }
-        index += (pAdvData->length + 1);
-    }
+    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");
+    //}
 }
 
 /**
@@ -70,6 +126,7 @@
 void onBleInitError(BLE &ble, ble_error_t error)
 {
     /* Initialization error handling should go here */
+    LOG("Crap, the BLE radio is broken\n");
 }
 
 /**
@@ -92,17 +149,28 @@
     }
 
     /* Setup and start scanning */
-    ble.gap().setScanParams(1800 /* scan interval */, 1500 /* scan window */);
+    ble.gap().setScanParams(500 /* scan interval */, 500 /* scan window */);
     ble.gap().startScan(advertisementCallback);
 }
 
 int main(void)
 {
-    ticker.attach(periodicCallback, 1);  /* trigger sensor polling every 2 seconds */
+    //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();
     }