Varun Bahl
Encryp_u
Dependencies: BLE_API_Encryp CyaSSL-Encryp eMPL_MPU6050 mbed
Fork of
Encryptulator2
by 
Mobius IoT
Diff: main.cpp
- Revision:
- 2:e1a6b317241e
- Parent:
- 0:26da608265f8
- Child:
- 3:b48570121d3f
--- a/main.cpp Wed Apr 22 07:47:17 2015 +0000
+++ b/main.cpp Sat May 28 22:10:43 2016 +0000
@@ -1,4 +1,4 @@
-
+#include<string>
#include "mbed.h"
#include "mbed_i2c.h"
#include "inv_mpu.h"
@@ -7,295 +7,194 @@
#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 LED_GREEN p21
-#define LED_RED p22
-#define LED_BLUE p23
-#define BUTTON_PIN p17
-#define BATTERY_PIN p1
+#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 MPU6050_SDA p12
+//#define MPU6050_SCL p13
#define UART_TX p9
#define UART_RX p11
-#define UART_CTS p8
-#define UART_RTS p10
+//#define UART_CTS p8
+//#define UART_RTS p10
/* Starting sampling rate. */
-#define DEFAULT_MPU_HZ (100)
+//#define DEFAULT_MPU_HZ (100)
-DigitalOut blue(LED_BLUE);
-DigitalOut green(LED_GREEN);
-DigitalOut red(LED_RED);
+//DigitalOut blue(LED_BLUE);
+//DigitalOut green(LED_GREEN);
+//DigitalOut red(LED_RED);
-InterruptIn button(BUTTON_PIN);
-AnalogIn battery(BATTERY_PIN);
+//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);
+//InterruptIn motion_probe(p14);
BLEDevice ble;
UARTService *uartServicePtr;
+Ticker tx_timeout, sensor_timeout;
-volatile bool bleIsConnected = false;
-volatile uint8_t tick_event = 0;
-volatile uint8_t motion_event = 0;
-static signed char board_orientation[9] = {
- 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1
-};
+//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;
-void check_i2c_bus(void);
-unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
+volatile bool bleIsConnected = false;
-void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
+void initAES(void)
{
- LOG("Connected!\n");
- bleIsConnected = true;
-}
+ //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;
-void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
-{
- LOG("Disconnected!\n");
- LOG("Restarting the advertising process\n");
- ble.startAdvertising();
- bleIsConnected = false;
+ AesSetKey(&ctx, key, KEYLEN/8, iv, AES_ENCRYPTION);
}
-void tick(void)
+void encrypt()
{
- green = !green;
-}
+ 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 detect(void)
-{
- LOG("Button pressed\n");
-
- blue = !blue;
}
-void motion_interrupt_handle(void)
+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)
{
- motion_event = 1;
+ 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 tap_cb(unsigned char direction, unsigned char count)
+void bleInitComplete(void)
{
- LOG("Tap motion detected\n");
+ /* 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();
}
-void android_orient_cb(unsigned char orientation)
-{
- LOG("Oriention changed\n");
-}
-
-
int main(void)
{
- blue = 1;
- green = 1;
- red = 1;
- pc.baud(115200);
- LOG("---- Seeed Tiny BLE ----\n");
-
- Ticker ticker;
- ticker.attach(tick, 3);
+ pc.baud(BAUDR);
+ LOG("---- ENCRYPTULATOR ACTIVIZE ----\n");
- button.fall(detect);
+ initAES();
- LOG("Initialising the nRF51822\n");
+ LOG("Bring up the BLE radio\n");
ble.init();
- ble.onDisconnection(disconnectionCallback);
- ble.onConnection(connectionCallback);
-
-
- /* setup advertising */
- ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
- ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
- ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
- (const uint8_t *)"smurfs", sizeof("smurfs"));
- ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
- (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
+
+
+ //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;
- //uartService.retargetStdout();
- ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
- ble.startAdvertising();
-
- check_i2c_bus();
-
- mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
- motion_probe.fall(motion_interrupt_handle);
-
-
- if (mpu_init(0)) {
-// LOG("failed to initialize mpu6050\r\n");
- }
-
- /* Get/set hardware configuration. Start gyro. */
- /* Wake up all sensors. */
- mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
- /* Push both gyro and accel data into the FIFO. */
- mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
- mpu_set_sample_rate(DEFAULT_MPU_HZ);
+ bleInitComplete();
- /* Read back configuration in case it was set improperly. */
- unsigned char accel_fsr;
- unsigned short gyro_rate, gyro_fsr;
- mpu_get_sample_rate(&gyro_rate);
- mpu_get_gyro_fsr(&gyro_fsr);
- mpu_get_accel_fsr(&accel_fsr);
+ //maybe replace with something inside sensor read? Basically trigger sending
+ sensor_timeout.attach(&sense, 0.025);
+ tx_timeout.attach(&tx_packet, 0.4);
- dmp_load_motion_driver_firmware();
- dmp_set_orientation(
- inv_orientation_matrix_to_scalar(board_orientation));
- dmp_register_tap_cb(tap_cb);
- dmp_register_android_orient_cb(android_orient_cb);
- uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
- DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
- DMP_FEATURE_GYRO_CAL;
- dmp_enable_feature(dmp_features);
- dmp_set_fifo_rate(DEFAULT_MPU_HZ);
- mpu_set_dmp_state(1);
-
-// dmp_set_interrupt_mode(DMP_INT_GESTURE);
- dmp_set_tap_thresh(TAP_XYZ, 50);
-
while (true) {
- if (motion_event) {
- motion_event = 0;
-
- unsigned long sensor_timestamp;
- short gyro[3], accel[3], sensors;
- long quat[4];
- unsigned char more = 1;
-
- while (more) {
- /* This function gets new data from the FIFO when the DMP is in
- * use. The FIFO can contain any combination of gyro, accel,
- * quaternion, and gesture data. The sensors parameter tells the
- * caller which data fields were actually populated with new data.
- * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
- * the FIFO isn't being filled with accel data.
- * The driver parses the gesture data to determine if a gesture
- * event has occurred; on an event, the application will be notified
- * via a callback (assuming that a callback function was properly
- * registered). The more parameter is non-zero if there are
- * leftover packets in the FIFO.
- */
- dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
- &more);
- /* Gyro and accel data are written to the FIFO by the DMP in chip
- * frame and hardware units. This behavior is convenient because it
- * keeps the gyro and accel outputs of dmp_read_fifo and
- * mpu_read_fifo consistent.
- */
- if (sensors & INV_XYZ_GYRO) {
- // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
- }
- if (sensors & INV_XYZ_ACCEL) {
- // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
- }
-
- /* Unlike gyro and accel, quaternions are written to the FIFO in
- * the body frame, q30. The orientation is set by the scalar passed
- * to dmp_set_orientation during initialization.
- */
- if (sensors & INV_WXYZ_QUAT) {
- // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
- }
- }
- } else {
ble.waitForEvent();
- }
}
}
-
-void check_i2c_bus(void)
-{
-
- DigitalInOut scl(MPU6050_SCL);
- DigitalInOut sda(MPU6050_SDA);
-
- scl.input();
- sda.input();
- int scl_level = scl;
- int sda_level = sda;
- if (scl_level == 0 || sda_level == 0) {
- printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
-
- scl.output();
- for (int i = 0; i < 8; i++) {
- scl = 0;
- wait_us(10);
- scl = 1;
- wait_us(10);
- }
- }
-
- scl.input();
-
- scl_level = scl;
- sda_level = sda;
- if (scl_level == 0 || sda_level == 0) {
- printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
- }
-}
-
-/* These next two functions converts the orientation matrix (see
- * gyro_orientation) to a scalar representation for use by the DMP.
- * NOTE: These functions are borrowed from Invensense's MPL.
- */
-static inline unsigned short inv_row_2_scale(const signed char *row)
-{
- unsigned short b;
-
- if (row[0] > 0)
- b = 0;
- else if (row[0] < 0)
- b = 4;
- else if (row[1] > 0)
- b = 1;
- else if (row[1] < 0)
- b = 5;
- else if (row[2] > 0)
- b = 2;
- else if (row[2] < 0)
- b = 6;
- else
- b = 7; // error
- return b;
-}
-
-unsigned short inv_orientation_matrix_to_scalar(
- const signed char *mtx)
-{
- unsigned short scalar;
-
- /*
- XYZ 010_001_000 Identity Matrix
- XZY 001_010_000
- YXZ 010_000_001
- YZX 000_010_001
- ZXY 001_000_010
- ZYX 000_001_010
- */
-
- scalar = inv_row_2_scale(mtx);
- scalar |= inv_row_2_scale(mtx + 3) << 3;
- scalar |= inv_row_2_scale(mtx + 6) << 6;
-
-
- return scalar;
-}
-