added BLE EEG data connection

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})

msg/EEG_data.msg

@@ -0,0 +1,4 @@
+int channel[5]
+float32 data[5]
+int cq[5]
+int batt_lvl

src/Ble_connect.cpp

@@ -0,0 +1,136 @@
+#include <glib.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <iostream>
+#include <mcrypt.h>
+#include <pthread.h>
+
+#include <string.h>
+
+#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<<std::endl;
+    mdecrypt_generic (mcrpt, val, data_length);
+    for(i = 0; i < 9; i++)
+    {
+        for(k = 7; k >= 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<<std::dec<<(int)val[9]<<" ";
+		//std::cout<<std::dec<<(int)val[i]<<"\t";
+	//}
+
+    std::cout.precision(10);
+	std::cout<<k<<"\t";
+	for (i = 0; i < 5; i++) {
+		std::cout<<uV[i]<<"\t";
+	}
+
+    /*int n=((val[1] & 0b00000011) << 5 ) + (val[2] >> 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 <device_address> <UUID>\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;
+}

src/evp-encrypt.cxx

@@ -0,0 +1,161 @@
+#include <iostream>
+#include <string>
+#include <memory>
+#include <limits>
+#include <stdexcept>
+
+#include <openssl/evp.h>
+#include <openssl/rand.h>
+
+static const unsigned int KEY_SIZE = 32;
+static const unsigned int BLOCK_SIZE = 16;
+
+template <typename T>
+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<size_type>::max() / sizeof(T))
+            throw std::bad_alloc();
+        return static_cast<pointer> (::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<size_type>::max() / sizeof (T);
+    }
+    
+    template<typename U>
+    struct rebind
+    {
+        typedef zallocator<U> other;
+    };
+
+    void construct (pointer ptr, const T& val) {
+        new (static_cast<T*>(ptr) ) T (val);
+    }
+
+    void destroy(pointer ptr) {
+        static_cast<T*>(ptr)->~T();
+    }
+
+#if __cpluplus >= 201103L
+    template<typename U, typename... Args>
+    void construct (U* ptr, Args&&  ... args) {
+        ::new (static_cast<void*> (ptr) ) U (std::forward<Args> (args)...);
+    }
+
+    template<typename U>
+    void destroy(U* ptr) {
+        ptr->~U();
+    }
+#endif
+};
+
+typedef unsigned char byte;
+typedef std::basic_string<char, std::char_traits<char>, zallocator<char> > secure_string;
+using EVP_CIPHER_CTX_free_ptr = std::unique_ptr<EVP_CIPHER_CTX, decltype(&::EVP_CIPHER_CTX_free)>;
+
+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);
+}
+