From 8cc0486ed6bd78698dcadf628ff00cbff2049d77 Mon Sep 17 00:00:00 2001 From: yikestone Date: Fri, 31 Aug 2018 03:46:09 +0530 Subject: [PATCH] added BLE EEG data connection --- CMakeLists.txt | 20 +++++- msg/EEG_data.msg | 4 ++ src/Ble_connect.cpp | 136 +++++++++++++++++++++++++++++++++++++ src/evp-encrypt.cxx | 161 ++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 320 insertions(+), 1 deletion(-) create mode 100644 msg/EEG_data.msg create mode 100644 src/Ble_connect.cpp create mode 100644 src/evp-encrypt.cxx diff --git a/CMakeLists.txt b/CMakeLists.txt index e3f3bad..ecb1a64 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,9 +5,17 @@ find_package(catkin REQUIRED) catkin_python_setup() catkin_package(DEPENDS) -include_directories( +find_package(PkgConfig REQUIRED) + +find_path(Mcrypt_INCLUDE_DIR mcrypt.h PATHS + /usr/local/include ) +set(Mcrypt_LIB_PATHS /usr/local/lib) +find_library(Mcrypt_LIBS NAMES mcrypt rtfilter PATHS ${Mcrypt_LIB_PATHS}) + +include_directories(include) + install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) @@ -19,3 +27,13 @@ install(DIRECTORY launch install(DIRECTORY nodes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +pkg_search_module(GATTLIB REQUIRED gattlib) +pkg_search_module(GLIB REQUIRED glib-2.0) +include_directories(${GLIB_INCLUDE_DIRS}) + +include_directories(${Mcrypt_INCLUDE_DIRS}) +link_directories(${Mcrypt_LIBS}) + +add_executable(ble_connect src/Ble_connect.cpp) +target_link_libraries(ble_connect ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS}) diff --git a/msg/EEG_data.msg b/msg/EEG_data.msg new file mode 100644 index 0000000..60767b2 --- /dev/null +++ b/msg/EEG_data.msg @@ -0,0 +1,4 @@ +int channel[5] +float32 data[5] +int cq[5] +int batt_lvl diff --git a/src/Ble_connect.cpp b/src/Ble_connect.cpp new file mode 100644 index 0000000..995195c --- /dev/null +++ b/src/Ble_connect.cpp @@ -0,0 +1,136 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include "gattlib.h" + +static uuid_t g_uuid; +const uint8_t BT_key[4] = {0x5a, 0x68, 0x5b, 0xb6}; +const uint8_t key[16] = {BT_key[0], 00, BT_key[1], 21, BT_key[2], 00, BT_key[3], 12, BT_key[2], 00, BT_key[1], 68, BT_key[0], 00, BT_key[1], 88 }; +static uint8_t data[16]; +int data_length = 16; + +MCRYPT mcrpt; +double uV[5]; +uint8_t raw_data[72]; +enum Channels{ + AF3 = 0, + T7 = 1, + Pz = 2, + T8 = 3, + AF4 = 4 +}; + +int k = 0; + +double raw(uint8_t h, uint8_t l){ + return (((double)h - 128) * 32.82051289 + (double)l * -0.128205128205129 + 4201.02564096001); +} + +void notification_handler(const uuid_t* uuid, const uint8_t* val, size_t values_length, void* user_data) { + int i, j, k; + //for(i = 0; i < values_length; i++) + // std::cout<<(int)val[i]<<"\t"; + //std::cout<= 0; k--) + raw_data[i * 8 + (7 - k)] = (val[i] >> k) & 1; + } + + for(i = 0; i < 5; i++) + { + uint8_t h = 0, l = 0; + + j=-1; + while((++j) < 8) + h = (h << 1) | ((uint8_t)(raw_data[(14 * i ) + j] )); + j--; + while((++j) < 14) + l = (l << 1) | ((uint8_t)(raw_data[(14 * i ) + j])); + + uV[i] = raw(h,l); + } + + //for (i = 0; i < data_length; i++) { + //if(val[9] == 194 || val[9]==0)continue; + //std::cout<> 3); + for (c = 7; c >= 0; c--) + { + k = n >> c; + if (k & 1) + printf("1"); + else + printf("0"); + }*/ + printf("\n"); + //if(val[0]==0)printf("\n"); +} + +static void usage(char *argv[]) { + printf("%s \n", argv[0]); +} + +GMainLoop *loop; + +void run(){ + loop = g_main_loop_new(NULL, 0); + g_main_loop_run(loop); + pthread_exit(NULL); +} + +int main(int argc, char *argv[]) { + + mcrpt = mcrypt_module_open(MCRYPT_RIJNDAEL_128, NULL, MCRYPT_ECB, NULL); + mcrypt_generic_init(mcrpt, key, data_length, NULL); + int ret; + gatt_connection_t* connection; + + + if (argc != 3) { + usage(argv); + return 1; + } + + if (gattlib_string_to_uuid(argv[2], strlen(argv[2]) + 1, &g_uuid) < 0) { + usage(argv); + return 1; + } + connection = gattlib_connect(NULL, argv[1], BDADDR_LE_PUBLIC, BT_SEC_LOW, 0, 0); + if (connection == NULL) { + fprintf(stderr, "Fail to connect to the bluetooth device.\n"); + return 1; + } + gattlib_register_notification(connection, notification_handler, NULL); + ret = gattlib_notification_start(connection, &g_uuid); + if (ret) { + fprintf(stderr, "Fail to start notification\n."); + return 1; + } + pthread_t thread; + pthread_create(&thread, NULL,run,NULL); + getchar(); + g_main_loop_quit(loop); + g_main_loop_unref(loop); + gattlib_notification_stop(connection, &g_uuid); + gattlib_disconnect(connection); + mcrypt_generic_deinit (mcrpt); + mcrypt_module_close(mcrpt); + return 0; +} diff --git a/src/evp-encrypt.cxx b/src/evp-encrypt.cxx new file mode 100644 index 0000000..c60c6a6 --- /dev/null +++ b/src/evp-encrypt.cxx @@ -0,0 +1,161 @@ +#include +#include +#include +#include +#include + +#include +#include + +static const unsigned int KEY_SIZE = 32; +static const unsigned int BLOCK_SIZE = 16; + +template +struct zallocator +{ +public: + typedef T value_type; + typedef value_type* pointer; + typedef const value_type* const_pointer; + typedef value_type& reference; + typedef const value_type& const_reference; + typedef std::size_t size_type; + typedef std::ptrdiff_t difference_type; + + pointer address (reference v) const {return &v;} + const_pointer address (const_reference v) const {return &v;} + + pointer allocate (size_type n, const void* hint = 0) { + if (n > std::numeric_limits::max() / sizeof(T)) + throw std::bad_alloc(); + return static_cast (::operator new (n * sizeof (value_type))); + } + + void deallocate(pointer p, size_type n) { + OPENSSL_cleanse(p, n*sizeof(T)); + ::operator delete(p); + } + + size_type max_size() const { + return std::numeric_limits::max() / sizeof (T); + } + + template + struct rebind + { + typedef zallocator other; + }; + + void construct (pointer ptr, const T& val) { + new (static_cast(ptr) ) T (val); + } + + void destroy(pointer ptr) { + static_cast(ptr)->~T(); + } + +#if __cpluplus >= 201103L + template + void construct (U* ptr, Args&& ... args) { + ::new (static_cast (ptr) ) U (std::forward (args)...); + } + + template + void destroy(U* ptr) { + ptr->~U(); + } +#endif +}; + +typedef unsigned char byte; +typedef std::basic_string, zallocator > secure_string; +using EVP_CIPHER_CTX_free_ptr = std::unique_ptr; + +void gen_params(byte key[KEY_SIZE], byte iv[BLOCK_SIZE]); +void aes_encrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ptext, secure_string& ctext); +void aes_decrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ctext, secure_string& rtext); + +// g++ -Wall -std=c++11 evp-encrypt.cxx -o evp-encrypt.exe -lcrypto +int main(int argc, char* argv[]) +{ + // Load the necessary cipher + EVP_add_cipher(EVP_aes_256_cbc()); + + // plaintext, ciphertext, recovered text + secure_string ptext = "Now is the time for all good men to come to the aide of their country"; + secure_string ctext, rtext; + + byte key[KEY_SIZE], iv[BLOCK_SIZE]; + gen_params(key, iv); + + aes_encrypt(key, iv, ptext, ctext); + aes_decrypt(key, iv, ctext, rtext); + + OPENSSL_cleanse(key, KEY_SIZE); + OPENSSL_cleanse(iv, BLOCK_SIZE); + + std::cout << "Original message:\n" << ptext << std::endl; + std::cout << "Recovered message:\n" << rtext << std::endl; + + return 0; +} + +void gen_params(byte key[KEY_SIZE], byte iv[BLOCK_SIZE]) +{ + int rc = RAND_bytes(key, KEY_SIZE); + if (rc != 1) + throw std::runtime_error("RAND_bytes key failed"); + + rc = RAND_bytes(iv, BLOCK_SIZE); + if (rc != 1) + throw std::runtime_error("RAND_bytes for iv failed"); +} + +void aes_encrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ptext, secure_string& ctext) +{ + EVP_CIPHER_CTX_free_ptr ctx(EVP_CIPHER_CTX_new(), ::EVP_CIPHER_CTX_free); + int rc = EVP_EncryptInit_ex(ctx.get(), EVP_aes_256_cbc(), NULL, key, iv); + if (rc != 1) + throw std::runtime_error("EVP_EncryptInit_ex failed"); + + // Recovered text expands upto BLOCK_SIZE + ctext.resize(ptext.size()+BLOCK_SIZE); + int out_len1 = (int)ctext.size(); + + rc = EVP_EncryptUpdate(ctx.get(), (byte*)&ctext[0], &out_len1, (const byte*)&ptext[0], (int)ptext.size()); + if (rc != 1) + throw std::runtime_error("EVP_EncryptUpdate failed"); + + int out_len2 = (int)ctext.size() - out_len1; + rc = EVP_EncryptFinal_ex(ctx.get(), (byte*)&ctext[0]+out_len1, &out_len2); + if (rc != 1) + throw std::runtime_error("EVP_EncryptFinal_ex failed"); + + // Set cipher text size now that we know it + ctext.resize(out_len1 + out_len2); +} + +void aes_decrypt(const byte key[KEY_SIZE], const byte iv[BLOCK_SIZE], const secure_string& ctext, secure_string& rtext) +{ + EVP_CIPHER_CTX_free_ptr ctx(EVP_CIPHER_CTX_new(), ::EVP_CIPHER_CTX_free); + int rc = EVP_DecryptInit_ex(ctx.get(), EVP_aes_256_cbc(), NULL, key, iv); + if (rc != 1) + throw std::runtime_error("EVP_DecryptInit_ex failed"); + + // Recovered text contracts upto BLOCK_SIZE + rtext.resize(ctext.size()); + int out_len1 = (int)rtext.size(); + + rc = EVP_DecryptUpdate(ctx.get(), (byte*)&rtext[0], &out_len1, (const byte*)&ctext[0], (int)ctext.size()); + if (rc != 1) + throw std::runtime_error("EVP_DecryptUpdate failed"); + + int out_len2 = (int)rtext.size() - out_len1; + rc = EVP_DecryptFinal_ex(ctx.get(), (byte*)&rtext[0]+out_len1, &out_len2); + if (rc != 1) + throw std::runtime_error("EVP_DecryptFinal_ex failed"); + + // Set recovered text size now that we know it + rtext.resize(out_len1 + out_len2); +} +