diff --git a/README.md b/README.md
index a710c0f3..abbca1e8 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,8 @@
 # corelibs-arduino101
+For Intel Community Support, product questions, or help troubleshooting, visit
+ICS: [https://communities.intel.com/community/tech/intel-curie](https://communities.intel.com/community/tech/intel-curie)
+
+|
 
 The contents of this repo is distributed through releases in Arduino IDE.    
 `Tools > Board > Boards Manager > Intel Curie Boards by Intel`
@@ -17,6 +21,10 @@ If you wish to use the latest **untested** changes, follow these instructions.
 6. Delete the content of the directory from step 5, and replace it with the
    content of the "corelibs-arduino101-master" folder in the zip from step 2.
 
+Note: your [tools](https://github.com/01org/intel-arduino-tools), found in
+`Arduino15/packages/Intel/tools/arduino101load/<version>/`, may also need to
+be updated to the latest snapshot.
+
 Future upgrades may fail since the internal contents were modified by hand. In
 order to recover, shut down the IDE, delete the entire `Arduino15` directory,
 then restart the IDE.
diff --git a/cores/arduino/Arduino.h b/cores/arduino/Arduino.h
index cc6cb593..a4cce47d 100644
--- a/cores/arduino/Arduino.h
+++ b/cores/arduino/Arduino.h
@@ -26,6 +26,8 @@
 #include <stdbool.h>
 #include <math.h>
 
+#include "pins_arduino.h"
+
 #include "binary.h"
 //#include "itoa.h"
 
@@ -60,7 +62,7 @@ typedef void (*voidFuncPtr)( void ) ;
 
 /* Types used for the tables below */
 /* TODO - consider using smaller types to optimise storage space */
-typedef struct _PinDescription
+typedef const struct _PinDescription
 {
         uint32_t                ulGPIOId;               // GPIO port pin
         uint32_t                ulGPIOPort;             // GPIO port ID
@@ -95,6 +97,10 @@ typedef struct _PinDescription
 /* Pins table to be instanciated into variant.cpp */
 extern PinDescription g_APinDescription[] ;
 
+extern uint32_t pwmPeriod[4];
+
+extern uint8_t pinmuxMode[NUM_DIGITAL_PINS];
+
 #ifdef __cplusplus
 } // extern "C"
 
@@ -104,6 +110,7 @@ extern PinDescription g_APinDescription[] ;
 #include "WMath.h"
 #include "HardwareSerial.h"
 #include "wiring_pulse.h"
+#include "error.h"
 
 #endif // __cplusplus
 
@@ -115,5 +122,6 @@ extern PinDescription g_APinDescription[] ;
 #include "wiring_analog.h"
 #include "wiring_shift.h"
 #include "WInterrupts.h"
+#include "dccm/dccm_alloc.h"
 
 #endif // Arduino_h
diff --git a/cores/arduino/CDCSerialClass.cpp b/cores/arduino/CDCSerialClass.cpp
index 56c50e89..c059cea8 100644
--- a/cores/arduino/CDCSerialClass.cpp
+++ b/cores/arduino/CDCSerialClass.cpp
@@ -30,11 +30,35 @@
 #include "wiring_digital.h"
 #include "variant.h"
 
+#define CDC_MAILBOX_TX_CHANNEL  7
+#define CDC_MAILBOX_RX_CHANNEL  6
 
 extern void CDCSerial_Handler(void);
 extern void serialEventRun1(void) __attribute__((weak));
 extern void serialEvent1(void) __attribute__((weak));
 
+static void cdc_mbox_isr(CurieMailboxMsg msg)
+{
+    char *rxbuffptr = (char*)msg.data;
+    int new_head;
+    for(int i = 0; i < MBOX_BYTES; i++)
+    {
+        if((uint8_t)(*(rxbuffptr+i)) != '\0')
+        {
+            new_head = (Serial._rx_buffer->head +1) % CDCACM_BUFFER_SIZE;
+            if(new_head != Serial._rx_buffer->tail)
+            {
+                Serial._rx_buffer->data[Serial._rx_buffer->head] = *(rxbuffptr+i);
+                Serial._rx_buffer->head = new_head;
+            }
+        }
+        else
+        {
+            break;
+        }
+    }
+}
+
 // Constructors ////////////////////////////////////////////////////////////////
 
 CDCSerialClass::CDCSerialClass(uart_init_info *info)
@@ -46,9 +70,9 @@ CDCSerialClass::CDCSerialClass(uart_init_info *info)
 
 void CDCSerialClass::setSharedData(struct cdc_acm_shared_data *cdc_acm_shared_data)
 {
-    this->_shared_data = cdc_acm_shared_data;
-    this->_rx_buffer = cdc_acm_shared_data->rx_buffer;
-    this->_tx_buffer = cdc_acm_shared_data->tx_buffer;
+    _shared_data = cdc_acm_shared_data;
+    _rx_buffer = cdc_acm_shared_data->rx_buffer;
+    _tx_buffer = cdc_acm_shared_data->tx_buffer;
 }
 
 void CDCSerialClass::begin(const uint32_t dwBaudRate)
@@ -62,8 +86,13 @@ void CDCSerialClass::begin(const uint32_t dwBaudRate, const uint8_t config)
 }
 
 
-void CDCSerialClass::init(const uint32_t dwBaudRate, const uint8_t modeReg)
+void CDCSerialClass::init(uint32_t dwBaudRate, const uint8_t modeReg)
 {
+    /* Set a max internal baud rate due to the limitation of the 
+     * Inter Processor Mailbox */
+     if(dwBaudRate > 115200)
+         dwBaudRate = 115200;
+    
     /* Set a per-byte write delay approximately equal to the time it would
      * take to clock out a byte on a standard UART at this baud rate */
     _writeDelayUsec = 8000000 / dwBaudRate;
@@ -72,7 +101,9 @@ void CDCSerialClass::init(const uint32_t dwBaudRate, const uint8_t modeReg)
      * Empty the Rx buffer but don't touch Tx buffer: it is drained by the
      * LMT one way or another */
     _rx_buffer->tail = _rx_buffer->head;
-
+    
+    mailbox_register(CDC_MAILBOX_RX_CHANNEL, cdc_mbox_isr);
+    mailbox_enable_receive(CDC_MAILBOX_RX_CHANNEL);
     _shared_data->device_open = true;
 }
 
@@ -83,12 +114,10 @@ void CDCSerialClass::end( void )
 
 int CDCSerialClass::available( void )
 {
-#define SBS	SERIAL_BUFFER_SIZE
-
   if (!_shared_data->device_open)
     return (0);
   else
-    return (int)(SBS + _rx_buffer->head - _rx_buffer->tail) % SBS;
+    return (int)(CDCACM_BUFFER_SIZE + _rx_buffer->head - _rx_buffer->tail) % CDCACM_BUFFER_SIZE;
 }
 
 int CDCSerialClass::availableForWrite(void)
@@ -100,7 +129,7 @@ int CDCSerialClass::availableForWrite(void)
     int tail = _tx_buffer->tail;
 
     if (head >= tail)
-        return SERIAL_BUFFER_SIZE - head + tail - 1;
+        return CDCACM_BUFFER_SIZE - head + tail - 1;
     return tail - head - 1;
 }
 
@@ -118,7 +147,7 @@ int CDCSerialClass::read( void )
     return -1;
 
   uint8_t uc = _rx_buffer->data[_rx_buffer->tail];
-  _rx_buffer->tail = (_rx_buffer->tail + 1) % SERIAL_BUFFER_SIZE;
+  _rx_buffer->tail = (_rx_buffer->tail + 1) % CDCACM_BUFFER_SIZE;
   return uc;
 }
 
@@ -130,29 +159,61 @@ void CDCSerialClass::flush( void )
     }
 }
 
-size_t CDCSerialClass::write( const uint8_t uc_data )
+size_t CDCSerialClass::write(uint8_t uc_data )
 {
-    uint32_t retries = 1;
+    CurieMailboxMsg cdcacm_msg;
 
     if (!_shared_data->device_open || !_shared_data->host_open)
         return(0);
 
-    do {
-        int i = (uint32_t)(_tx_buffer->head + 1) % SERIAL_BUFFER_SIZE;
-        // if we should be storing the received character into the location
-        // just before the tail (meaning that the head would advance to the
-        // current location of the tail), we're about to overflow the buffer
-        // and so we don't write the character or advance the head.
-        if (i != _tx_buffer->tail) {
-            _tx_buffer->data[_tx_buffer->head] = uc_data;
-            _tx_buffer->head = i;
-
-	    // Mimick the throughput of a typical UART by throttling the data
-	    // flow according to the configured baud rate
-	    delayMicroseconds(_writeDelayUsec);
-            break;
+    cdcacm_msg.channel = CDC_MAILBOX_TX_CHANNEL;
+    cdcacm_msg.data[0] = uc_data;
+    mailbox_write(cdcacm_msg);
+    delayMicroseconds(_writeDelayUsec);
+    return 1;
+}
+
+size_t CDCSerialClass::write(const uint8_t *buffer, size_t size)
+{
+    CurieMailboxMsg cdcacm_msg;
+     cdcacm_msg.channel = CDC_MAILBOX_TX_CHANNEL;
+    if (!_shared_data->device_open || !_shared_data->host_open)
+        return(0);
+    
+    int msg_len = size;
+    
+    for(int i = 0;msg_len > 0; msg_len -= MBOX_BYTES, i += MBOX_BYTES)
+    {
+        /* Copy data into mailbox message */
+        memset(cdcacm_msg.data, 0, MBOX_BYTES);
+        if(msg_len >= MBOX_BYTES)
+        {
+            memcpy(cdcacm_msg.data, buffer+i, MBOX_BYTES);
         }
-    } while (retries--);
+        else
+        {
+            memcpy(cdcacm_msg.data, buffer+i, msg_len);
+        }
+        /* Write to mailbox*/
+        mailbox_write(cdcacm_msg);
+    }
 
-    return 1;
+    // Mimick the throughput of a typical UART by throttling the data
+    // flow according to the configured baud rate  
+    delayMicroseconds(_writeDelayUsec * size);
+    
+    return size;
+}
+
+size_t CDCSerialClass::write(const char *str)
+{
+    if (str == NULL) return 0;
+    CurieMailboxMsg cdcacm_msg;
+     cdcacm_msg.channel = CDC_MAILBOX_TX_CHANNEL;
+    if (!_shared_data->device_open || !_shared_data->host_open)
+        return(0);
+    
+    int msg_len = strlen(str);
+
+    return write((const uint8_t *)str, msg_len);
 }
diff --git a/cores/arduino/CDCSerialClass.h b/cores/arduino/CDCSerialClass.h
index f240ac38..74eb7b3b 100644
--- a/cores/arduino/CDCSerialClass.h
+++ b/cores/arduino/CDCSerialClass.h
@@ -26,6 +26,7 @@
 #include "HardwareSerial.h"
 #include "platform.h"
 #include "wiring.h"
+#include "mailbox.h"
 
 #include <board.h>
 #include <uart.h>
@@ -36,6 +37,10 @@ class CDCSerialClass : public HardwareSerial
     CDCSerialClass(uart_init_info *info);
 
     void setSharedData(struct cdc_acm_shared_data *cdc_acm_shared_data);
+    
+    struct cdc_acm_shared_data *_shared_data;
+    struct cdc_ring_buffer *_rx_buffer;
+    struct cdc_ring_buffer *_tx_buffer;
 
     void begin(const uint32_t dwBaudRate);
     void begin(const uint32_t dwBaudRate, const uint8_t config);
@@ -46,6 +51,8 @@ class CDCSerialClass : public HardwareSerial
     int read(void);
     void flush(void);
     size_t write(const uint8_t c);
+    size_t write(const char *str);
+    size_t write(const uint8_t *buffer, size_t size);
     using Print::write; // pull in write(str) and write(buf, size) from Print
 
     operator bool() {
@@ -56,11 +63,7 @@ class CDCSerialClass : public HardwareSerial
     };
 
   protected:
-    void init(const uint32_t dwBaudRate, const uint8_t config);
-
-    struct cdc_acm_shared_data *_shared_data;
-    struct cdc_ring_buffer *_rx_buffer;
-    struct cdc_ring_buffer *_tx_buffer;
+    void init(uint32_t dwBaudRate, const uint8_t config);
 
     uart_init_info *info;
     uint32_t _writeDelayUsec;
diff --git a/cores/arduino/Print.h b/cores/arduino/Print.h
index 16552079..96bd3022 100644
--- a/cores/arduino/Print.h
+++ b/cores/arduino/Print.h
@@ -47,7 +47,7 @@ class Print
     void clearWriteError() { setWriteError(0); }
 
     virtual size_t write(uint8_t) = 0;
-    size_t write(const char *str) {
+    virtual size_t write(const char *str) {
       if (str == NULL) return 0;
       return write((const uint8_t *)str, strlen(str));
     }
diff --git a/cores/arduino/RingBuffer.cpp b/cores/arduino/RingBuffer.cpp
index 14ecc1e8..c3ec50c4 100644
--- a/cores/arduino/RingBuffer.cpp
+++ b/cores/arduino/RingBuffer.cpp
@@ -23,14 +23,15 @@
 
 RingBuffer::RingBuffer( void )
 {
-    memset( _aucBuffer, 0, SERIAL_BUFFER_SIZE ) ;
+    _aucBuffer = (uint8_t*)dccm_malloc(UART_BUFFER_SIZE);
+    memset( _aucBuffer, 0, UART_BUFFER_SIZE ) ;
     _iHead=0 ;
     _iTail=0 ;
 }
 
 void RingBuffer::store_char( uint8_t c )
 {
-  int i = (uint32_t)(_iHead + 1) % SERIAL_BUFFER_SIZE ;
+  int i = (uint32_t)(_iHead + 1) % UART_BUFFER_SIZE ;
 
   // if we should be storing the received character into the location
   // just before the tail (meaning that the head would advance to the
diff --git a/cores/arduino/RingBuffer.h b/cores/arduino/RingBuffer.h
index e391ace4..04d41b5a 100644
--- a/cores/arduino/RingBuffer.h
+++ b/cores/arduino/RingBuffer.h
@@ -18,20 +18,21 @@
 #define _RING_BUFFER_
 
 #include <stdint.h>
+#include "dccm/dccm_alloc.h"
 
 // Define constants and variables for buffering incoming serial data.  We're
 // using a ring buffer (I think), in which head is the index of the location
 // to which to write the next incoming character and tail is the index of the
 // location from which to read.
-#define SERIAL_BUFFER_SIZE 256
+#define UART_BUFFER_SIZE 64
 
 class RingBuffer
 {
 public:
-	uint8_t _aucBuffer[SERIAL_BUFFER_SIZE] ;
-	int _iHead ;
-	int _iTail ;
-	bool _buffer_overflow ;
+	uint8_t *_aucBuffer;
+	volatile int _iHead ;
+	volatile int _iTail ;
+	volatile bool _buffer_overflow ;
 
 	RingBuffer( void ) ;
 	void store_char( uint8_t c ) ;
diff --git a/cores/arduino/UARTClass.cpp b/cores/arduino/UARTClass.cpp
index 922cf74d..b4c31004 100644
--- a/cores/arduino/UARTClass.cpp
+++ b/cores/arduino/UARTClass.cpp
@@ -134,7 +134,7 @@ uint32_t UARTClass::getInterruptPriority()
 
 int UARTClass::available( void )
 {
-  return (uint32_t)(SERIAL_BUFFER_SIZE + _rx_buffer->_iHead - _rx_buffer->_iTail) % SERIAL_BUFFER_SIZE;
+  return (uint32_t)(UART_BUFFER_SIZE + _rx_buffer->_iHead - _rx_buffer->_iTail) % UART_BUFFER_SIZE;
 }
 
 int UARTClass::availableForWrite(void)
@@ -143,7 +143,7 @@ int UARTClass::availableForWrite(void)
     return(0);
   int head = _tx_buffer->_iHead;
   int tail = _tx_buffer->_iTail;
-  if (head >= tail) return SERIAL_BUFFER_SIZE - 1 - head + tail;
+  if (head >= tail) return UART_BUFFER_SIZE - 1 - head + tail;
   return tail - head - 1;
 }
 
@@ -162,13 +162,13 @@ int UARTClass::read( void )
     return -1;
 
   uint8_t uc = _rx_buffer->_aucBuffer[_rx_buffer->_iTail];
-  _rx_buffer->_iTail = (unsigned int)(_rx_buffer->_iTail + 1) % SERIAL_BUFFER_SIZE;
+  _rx_buffer->_iTail = (unsigned int)(_rx_buffer->_iTail + 1) % UART_BUFFER_SIZE;
   return uc;
 }
 
 void UARTClass::flush( void )
 {
-  while (_tx_buffer->_iHead != _tx_buffer->_iTail); //wait for transmit data to be sent
+  while (_tx_buffer->_iHead != (_tx_buffer->_iTail)); //wait for transmit data to be sent
   // Wait for transmission to complete
   while(!uart_tx_complete(CONFIG_UART_CONSOLE_INDEX));
 }
@@ -182,9 +182,8 @@ size_t UARTClass::write( const uint8_t uc_data )
   if (_tx_buffer->_iTail != _tx_buffer->_iHead)
   {
     // If busy we buffer
-    int l = (_tx_buffer->_iHead + 1) % SERIAL_BUFFER_SIZE;
-    while (_tx_buffer->_iTail == l)
-      ; // Spin locks if we're about to overwrite the buffer. This continues once the data is sent
+    int l = (_tx_buffer->_iHead + 1) % UART_BUFFER_SIZE;
+    while (_tx_buffer->_iTail == l); // Spin locks if we're about to overwrite the buffer. This continues once the data is sent
 
     _tx_buffer->_aucBuffer[_tx_buffer->_iHead] = uc_data;
     _tx_buffer->_iHead = l;
@@ -201,21 +200,29 @@ size_t UARTClass::write( const uint8_t uc_data )
 
 void UARTClass::IrqHandler( void )
 {
-  uint8_t uc_data;
-  int ret;
-  ret = uart_poll_in(CONFIG_UART_CONSOLE_INDEX, &uc_data);
-  
-  while ( ret != -1 ) {
-    _rx_buffer->store_char(uc_data);
+  uart_irq_update(CONFIG_UART_CONSOLE_INDEX);
+  // if irq is Receiver Data Available
+  if(uart_irq_rx_ready(CONFIG_UART_CONSOLE_INDEX))
+  {
+    uint8_t uc_data;
+    int ret;
     ret = uart_poll_in(CONFIG_UART_CONSOLE_INDEX, &uc_data);
+    
+    while ( ret != -1 ) {
+      _rx_buffer->store_char(uc_data);
+      ret = uart_poll_in(CONFIG_UART_CONSOLE_INDEX, &uc_data);
+    }
   }
 
-  // Do we need to keep sending data?
-  if (!uart_irq_tx_ready(CONFIG_UART_CONSOLE_INDEX))
+  // if irq is Transmitter Holding Register
+  if(uart_irq_tx_ready(CONFIG_UART_CONSOLE_INDEX))
   {
-    if (_tx_buffer->_iTail != _tx_buffer->_iHead) {
-      uart_poll_out(CONFIG_UART_CONSOLE_INDEX, _tx_buffer->_aucBuffer[_tx_buffer->_iTail]);
-      _tx_buffer->_iTail = (unsigned int)(_tx_buffer->_iTail + 1) % SERIAL_BUFFER_SIZE;
+    if(_tx_buffer->_iTail != _tx_buffer->_iHead)
+    {
+      int end = (_tx_buffer->_iTail < _tx_buffer->_iHead) ? _tx_buffer->_iHead : UART_BUFFER_SIZE;
+      int l = min(end - _tx_buffer->_iTail, UART_FIFO_SIZE);
+      l = uart_fifo_fill(CONFIG_UART_CONSOLE_INDEX, _tx_buffer->_aucBuffer+_tx_buffer->_iTail, l);
+      _tx_buffer->_iTail = (_tx_buffer->_iTail+l)%UART_BUFFER_SIZE;
     }
     else
     {
diff --git a/cores/arduino/Udp.h b/cores/arduino/Udp.h
index dc5644b9..89f31c67 100644
--- a/cores/arduino/Udp.h
+++ b/cores/arduino/Udp.h
@@ -41,7 +41,8 @@
 class UDP : public Stream {
 
 public:
-  virtual uint8_t begin(uint16_t) =0;	// initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use
+  virtual uint8_t begin(uint16_t) =0;  // initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use
+  virtual uint8_t beginMulticast(IPAddress, uint16_t) { return 0; }  // initialize, start listening on specified multicast IP address and port. Returns 1 if successful, 0 on failure
   virtual void stop() =0;  // Finish with the UDP socket
 
   // Sending UDP packets
diff --git a/cores/arduino/WString.cpp b/cores/arduino/WString.cpp
index d577e002..3b20b5ad 100644
--- a/cores/arduino/WString.cpp
+++ b/cores/arduino/WString.cpp
@@ -138,7 +138,7 @@ String::String(float value, unsigned char decimalPlaces)
 
 	if(decimalPlaces)  totalSize += 1 + ((int)decimalPlaces & 0x0FF);
 
-	char buf[totalSize+1];
+	char buf[totalSize + 2];
 	*this = dtostrf(value, 0, decimalPlaces, buf);
 }
 
@@ -149,7 +149,7 @@ String::String(double value, unsigned char decimalPlaces)
 
 	if(decimalPlaces)  totalSize += 1 + ((int)decimalPlaces & 0x0FF);
 
-	char buf[totalSize+1];
+	char buf[totalSize + 2];
 	*this = dtostrf(value, 0, decimalPlaces, buf);
 }
 
diff --git a/cores/arduino/dccm/dccm_alloc.c b/cores/arduino/dccm/dccm_alloc.c
new file mode 100644
index 00000000..b34dd020
--- /dev/null
+++ b/cores/arduino/dccm/dccm_alloc.c
@@ -0,0 +1,51 @@
+/*
+Copyright (c) 2016 Intel Corporation.  All right reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+*/
+
+#include "dccm_alloc.h"
+
+uint16_t dccm_index = 0;
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+void* dccm_malloc(uint16_t size)
+{
+    if((size + dccm_index) > DCCM_SIZE)
+    {
+        return 0;
+    }
+    
+    void* addr = (void*)(DCCM_START + dccm_index);
+    dccm_index += size;
+    return addr;
+}
+
+void *dccm_memalign(uint16_t size)
+{
+  if ((dccm_index +3) > DCCM_SIZE)
+    return 0;
+
+  dccm_index = (dccm_index + 3) & ~((uint16_t)0x3);  /* 4 byte addr alignment */
+  return dccm_malloc(size);
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/cores/arduino/dccm/dccm_alloc.h b/cores/arduino/dccm/dccm_alloc.h
new file mode 100644
index 00000000..a25ca9c7
--- /dev/null
+++ b/cores/arduino/dccm/dccm_alloc.h
@@ -0,0 +1,41 @@
+/*
+Copyright (c) 2016 Intel Corporation.  All right reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+*/
+
+#include <stdint.h>
+
+#define DCCM_START  0x80000000
+#define DCCM_SIZE 8192
+
+#ifndef _DCCM_ALLOC_
+#define _DCCM_ALLOC_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+void* dccm_malloc(uint16_t size);
+
+void *dccm_memalign(uint16_t size);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif
diff --git a/cores/arduino/error.cpp b/cores/arduino/error.cpp
new file mode 100644
index 00000000..7371415e
--- /dev/null
+++ b/cores/arduino/error.cpp
@@ -0,0 +1,49 @@
+/*
+Copyright (c) 2017 Intel Corporation.  All right reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+*/
+
+#include "error.h"
+
+void error_continue()
+{
+    error_continue(1);
+}
+
+void error_continue(uint8_t error_code)
+{
+    uint32_t exc_addr = aux_reg_read(ARC_V2_EFA);
+    uint32_t ecr = aux_reg_read(ARC_V2_ECR);
+
+    pr_error(0, "Exception vector: 0x%x, cause code: 0x%x, parameter 0x%x\n",
+    ARC_V2_ECR_VECTOR(ecr),
+    ARC_V2_ECR_CODE(ecr),
+    ARC_V2_ECR_PARAMETER(ecr));
+    pr_error(0, "Address 0x%x\n", exc_addr);
+    shared_data->error_code = error_code;
+}
+
+void error_halt()
+{
+    error_halt(1);
+}
+
+void error_halt(uint8_t error_code)
+{
+    error_continue(error_code);
+    __asm__("flag 0x01") ; /* Halt the CPU */
+}
diff --git a/cores/arduino/error.h b/cores/arduino/error.h
new file mode 100644
index 00000000..9ca0be1b
--- /dev/null
+++ b/cores/arduino/error.h
@@ -0,0 +1,37 @@
+/*
+Copyright (c) 2017 Intel Corporation.  All right reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+*/
+
+#ifndef _Error_h
+#define _Error_h
+
+#include <stdint.h>
+#include "os/os.h"
+#include "infra/log.h"
+#include "aux_regs.h"
+#include "platform.h"
+
+extern void error_halt();
+
+extern void error_halt(uint8_t error_code);
+
+extern void error_continue();
+
+extern void error_continue(uint8_t error_code);
+
+#endif
diff --git a/cores/arduino/mailbox.cpp b/cores/arduino/mailbox.cpp
new file mode 100644
index 00000000..f8b54786
--- /dev/null
+++ b/cores/arduino/mailbox.cpp
@@ -0,0 +1,163 @@
+#include <string.h>
+#include "interrupt.h"
+#include "scss_registers.h"
+#include "mailbox.h"
+
+#define CHANNEL_STS_MASK            0x1
+#define CHANNEL_INT_MASK            0x2
+#define CTRL_WORD_MASK              0x7FFFFFFF
+#define CHALL_STATUS_MASK           0xFFFF
+#define CHANNEL_STS_BITS            (CHANNEL_STS_MASK | CHANNEL_INT_MASK)
+
+/* Mailbox channel status register */
+#define IO_REG_MAILBOX_CHALL_STS    (SCSS_REGISTER_BASE + 0xAC0)
+
+typedef struct mbox_channel mbox_channel_t;
+typedef struct mbox_intmask mbox_intmask_t;
+
+/* Represents the registers for a single mailbox channel */
+struct mbox_channel {
+    uint32_t ctrl;
+    uint32_t data[CHANNEL_DATA_WORDS];
+    uint32_t sts;
+};
+
+/* Mailbox interrupt mask; right now we only care about the
+ * second byte, for SS mailbox interrupts (one bit per mailbox channel) */
+struct mbox_intmask {
+    uint8_t lmt_intmask;
+    uint8_t ss_intmask;
+    uint8_t lmt_halt;
+    uint8_t ss_halt;
+};
+
+static volatile mbox_intmask_t *intmask;
+static volatile mbox_channel_t *mbox;
+static void (*callbacks[NUM_MAILBOX_CHANNELS])(CurieMailboxMsg);
+
+void mailbox_register (int channel, void(*callback)(CurieMailboxMsg))
+{
+    if (channel >= 0 && channel < NUM_MAILBOX_CHANNELS) {
+        callbacks[channel] = callback;
+    }
+}
+
+void mailbox_unregister (int channel)
+{
+    if (channel >= 0 && channel < NUM_MAILBOX_CHANNELS) {
+        callbacks[channel] = 0;
+    }
+}
+
+void mailbox_disable_receive (int channel)
+{
+    intmask->ss_intmask |= 1 << channel;
+}
+
+void mailbox_enable_receive (int channel)
+{
+    intmask->ss_intmask &= ~(1 << channel);
+}
+
+static void do_callback (int channel, CurieMailboxMsg& msg)
+{
+    void (*cb)(CurieMailboxMsg) = callbacks[channel];
+    if (cb) {
+        cb(msg);
+    }
+}
+
+static void mailbox_read (int channel, CurieMailboxMsg& msg)
+{
+    unsigned int i;
+
+    /* Copy channel data into CurieMailboxMsg object */
+    msg.id = mbox[channel].ctrl & CTRL_WORD_MASK;
+    msg.channel = channel;
+
+    for (i = 0; i < CHANNEL_DATA_WORDS; ++i) {
+        msg.data[i] = mbox[channel].data[i];
+    }
+
+    /* Clear channel status & interrupt flags */
+    mbox[channel].sts |= CHANNEL_STS_BITS;
+}
+
+void mailbox_write (CurieMailboxMsg& msg)
+{
+    int i;
+    uint32_t key;
+
+    /* Can't write if channel status flag is set */
+    while ((mbox[msg.channel].sts & CHANNEL_STS_MASK));
+    key = interrupt_lock();
+
+    /* Poplate channel payload */
+    mbox[msg.channel].ctrl |= (msg.id & CTRL_WORD_MASK);
+    for (i = 0; i < CHANNEL_DATA_WORDS; ++i) {
+        mbox[msg.channel].data[i] = msg.data[i];
+    }
+
+    /* Trigger interupt to host */
+    mbox[msg.channel].ctrl |= ~(CTRL_WORD_MASK);
+
+    /* Wait for HW to set the channel status bit */
+    while (!(mbox[msg.channel].sts & CHANNEL_STS_MASK));
+
+    /* Wait for destination processor to clear channel status bit */
+    while ((mbox[msg.channel].sts & CHANNEL_STS_MASK));
+    interrupt_unlock(key);
+}
+
+static uint16_t get_chall_sts (void)
+{
+    return MMIO_REG_VAL(IO_REG_MAILBOX_CHALL_STS) & CHALL_STATUS_MASK;
+}
+
+static void mailbox_isr (void)
+{
+    int i;
+    uint32_t sts;
+    CurieMailboxMsg msg;
+
+    sts = get_chall_sts();
+    /* Get channel number */
+    for (i = 0; i < NUM_MAILBOX_CHANNELS; ++i) {
+        if (sts & (1 << (i * 2 + 1))) {
+            break;
+        }
+    }
+
+    mailbox_read(i, msg);
+    do_callback(i, msg);
+}
+
+static void mailbox_hardware_init (void)
+{
+    int i;
+
+    for (i = 0; i < NUM_MAILBOX_CHANNELS; ++i) {
+        mbox[i].sts &= ~(CHANNEL_STS_BITS);
+    }
+}
+
+static void mailbox_interrupts_init (bool master)
+{
+    interrupt_disable(SOC_MBOX_INTERRUPT);
+
+   /* Mask SS mailbox interrupts for all channels;
+    * Unmasking is done by enableReceive */
+    intmask->ss_intmask = 0xFF;
+
+    if (master) mailbox_hardware_init();
+    interrupt_connect(SOC_MBOX_INTERRUPT, mailbox_isr);
+    interrupt_enable(SOC_MBOX_INTERRUPT);
+}
+
+void mailbox_init (bool master)
+{
+    intmask = (mbox_intmask_t *)IO_REG_MAILBOX_INT_MASK;
+    mbox = (mbox_channel_t *)IO_REG_MAILBOX_BASE;
+    memset(callbacks, 0, sizeof(callbacks));
+    mailbox_interrupts_init(master);
+}
diff --git a/cores/arduino/mailbox.h b/cores/arduino/mailbox.h
new file mode 100644
index 00000000..a72634eb
--- /dev/null
+++ b/cores/arduino/mailbox.h
@@ -0,0 +1,22 @@
+#ifndef _MAILBOX_H_
+#define _MAILBOX_H_
+
+#define NUM_MAILBOX_CHANNELS 8
+#define CHANNEL_DATA_WORDS   4
+#define MBOX_BYTES           16
+
+class CurieMailboxMsg {
+public:
+    uint32_t data[CHANNEL_DATA_WORDS];
+    uint32_t id;
+    int channel = 0;
+};
+
+void mailbox_init (bool master);
+void mailbox_register (int channel, void(*callback)(CurieMailboxMsg));
+void mailbox_unregister (int channel);
+void mailbox_enable_receive (int channel);
+void mailbox_disable_receive (int channel);
+void mailbox_write(CurieMailboxMsg& msg);
+
+#endif
diff --git a/system/libarc32_arduino101/framework/src/nordic_interface.h b/cores/arduino/printk.cpp
similarity index 79%
rename from system/libarc32_arduino101/framework/src/nordic_interface.h
rename to cores/arduino/printk.cpp
index 1b187b16..babfc0ed 100644
--- a/system/libarc32_arduino101/framework/src/nordic_interface.h
+++ b/cores/arduino/printk.cpp
@@ -28,13 +28,25 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef NORDIC_INTERFACE_H
-#define NORDIC_INTERFACE_H
-#include "infra/ipc_uart.h"
+#include <stdarg.h>
+#include <cstdio>
+#include "UARTClass.h"
 
-void uart_ipc_message_cback(uint8_t cpu_id, int channel, int len, void * p_data);
-int send_message_ipc_uart(struct message * message);
-void free_message_ipc_uart(struct message * message);
-int nordic_interface_init(T_QUEUE queue);
+extern "C" void printk(const char *fmt, va_list args);
+extern UARTClass Serial1;
+#define PRINTK_BUFSIZ 256
+
+void printk(const char *fmt, va_list args)
+{
+#ifdef CONFIGURE_DEBUG_CORELIB_ENABLED
+    int len = 0;
+
+    char tmp[PRINTK_BUFSIZ];
+
+    len = vsnprintf(tmp, PRINTK_BUFSIZ, fmt, args);
+    
+    tmp[len] = '\0';
+    Serial1.println(tmp);
+#endif
+}
 
-#endif // NORDIC_INTERFACE_H
diff --git a/cores/arduino/stdlib_noniso.cpp b/cores/arduino/stdlib_noniso.cpp
index 7b8a107c..5215a041 100644
--- a/cores/arduino/stdlib_noniso.cpp
+++ b/cores/arduino/stdlib_noniso.cpp
@@ -199,6 +199,16 @@ char *dtostrf(double number, signed char width, unsigned char prec, char *s)
         return s;
     }
 
+    // rounding up to the precision
+    rounding = 0.5;
+    for (i = 0; i < prec; ++i)
+        rounding /= 10.0;
+
+    if (number < 0.0)
+        number -= rounding;
+    else
+        number += rounding;
+
     out = s;
     before = digitsBe4Decimal(number);
 
@@ -229,23 +239,17 @@ char *dtostrf(double number, signed char width, unsigned char prec, char *s)
 
     out[i - 1] = ASCII_ZERO + integer;
     out += before;
-    if (!prec) goto end;
 
-    // rounding up to the precision
-    rounding = 0.5;
-    for (i = 0; i < prec; ++i)
-        rounding /= 10.0;
-    fraction += rounding;
-
-    // generate chars for each digit of the fractional part
-    *out++ = '.';
-    for (i = 0; i < prec; ++i) {
-        fraction *= 10.0;
-        digit = ((unsigned long long) fraction) % 10;
-        *out++ = (char) (ASCII_ZERO + digit);
+    if (prec) {
+        // generate chars for each digit of the fractional part
+        *out++ = '.';
+        for (i = 0; i < prec; ++i) {
+            fraction *= 10.0;
+            digit = ((unsigned long long) fraction) % 10;
+            *out++ = (char) (ASCII_ZERO + digit);
+        }
     }
 
-end:
     // check if padding is required
     if (width > 0) {
         delta = width - (before + prec + 1);
diff --git a/cores/arduino/wiring_analog.c b/cores/arduino/wiring_analog.c
index 6e0c84ae..c2917f89 100644
--- a/cores/arduino/wiring_analog.c
+++ b/cores/arduino/wiring_analog.c
@@ -27,11 +27,12 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 /* Standard Arduino PWM resolution */
 static int _writeResolution = 8;
 static int _readResolution = 10;
-
+uint32_t maxResolutionValue = 0xFF;
 
 void analogWriteResolution(int res)
 {
     _writeResolution = res;
+    maxResolutionValue = 0xFFFFFFFF >> (32-res);
 }
 
 void analogReadResolution(int res)
@@ -49,7 +50,7 @@ static inline uint32_t mapResolution(uint32_t value, uint32_t from, uint32_t to)
          return value << (to-from);
 }
 
-void analogWrite(uint8_t pin, int val)
+void analogWrite(uint8_t pin, uint32_t val)
 {
     if (! digitalPinHasPWM(pin))
     {
@@ -75,20 +76,12 @@ void analogWrite(uint8_t pin, int val)
     } else {
         /* PWM for everything in between */
         PinDescription *p = &g_APinDescription[pin];
-        uint32_t hcnt = mapResolution(val, _writeResolution, PWM_RESOLUTION);
-        uint32_t lcnt = PWM_MAX_DUTY_CYCLE - hcnt;
-        uint32_t offset;
-
-        /* For Arduino Uno compatibilty, we scale up frequency on certain pins */
-        hcnt >>= p->ulPwmScale;
-        lcnt >>= p->ulPwmScale;
-
-        /* Each count must be > 0 */
-        if (hcnt < PWM_MIN_DUTY_CYCLE)
-            hcnt = PWM_MIN_DUTY_CYCLE;
-        if (lcnt < PWM_MIN_DUTY_CYCLE)
-            lcnt = PWM_MIN_DUTY_CYCLE;
 
+        uint32_t offset;
+        
+        uint32_t hcnt = (val/(float)maxResolutionValue) * pwmPeriod[p->ulPwmChan];
+        uint32_t lcnt = pwmPeriod[p->ulPwmChan] - hcnt;
+        
         /* Set the high count period (duty cycle) */
         offset = ((p->ulPwmChan * QRK_PWM_N_LCNT2_LEN) + QRK_PWM_N_LOAD_COUNT2);
         MMIO_REG_VAL(QRK_PWM_BASE_ADDR + offset) = hcnt;
@@ -97,15 +90,16 @@ void analogWrite(uint8_t pin, int val)
         offset = ((p->ulPwmChan * QRK_PWM_N_REGS_LEN) + QRK_PWM_N_LOAD_COUNT1);
         MMIO_REG_VAL(QRK_PWM_BASE_ADDR + offset) = lcnt;
 
-        if (p->ulPinMode != PWM_MUX_MODE) {
-            /* start the PWM output */
-            offset = ((p->ulPwmChan * QRK_PWM_N_REGS_LEN) + QRK_PWM_N_CONTROL);
-            SET_MMIO_MASK(QRK_PWM_BASE_ADDR + offset, QRK_PWM_CONTROL_ENABLE);
-
+        /* start the PWM output */
+        offset = ((p->ulPwmChan * QRK_PWM_N_REGS_LEN) + QRK_PWM_N_CONTROL);
+        SET_MMIO_MASK(QRK_PWM_BASE_ADDR + offset, QRK_PWM_CONTROL_ENABLE);
+        
+        if(pinmuxMode[pin] != PWM_MUX_MODE)
+        {
             /* Disable pull-up and set pin mux for PWM output */
             SET_PIN_PULLUP(p->ulSocPin, 0);
             SET_PIN_MODE(p->ulSocPin, PWM_MUX_MODE);
-            p->ulPinMode = PWM_MUX_MODE;
+            pinmuxMode[pin] = PWM_MUX_MODE;
         }
     }
 }
@@ -120,11 +114,8 @@ uint32_t analogRead(uint32_t pin)
     PinDescription *p = &g_APinDescription[pin];
 
     /* Disable pull-up and set pin mux for ADC output */
-    if (p->ulPinMode != ADC_MUX_MODE) {
-       SET_PIN_MODE(p->ulSocPin, ADC_MUX_MODE);
-       p->ulPinMode = ADC_MUX_MODE;
-       SET_PIN_PULLUP(p->ulSocPin,0);
-    }
+    SET_PIN_MODE(p->ulSocPin, ADC_MUX_MODE);
+    SET_PIN_PULLUP(p->ulSocPin,0);
 
     /* Reset sequence pointer */
     SET_ARC_MASK(ADC_CTRL, ADC_SEQ_PTR_RST);
@@ -145,6 +136,13 @@ uint32_t analogRead(uint32_t pin)
 
 }
 
+void analogWriteFrequency(uint8_t pin, uint32_t freq)
+{
+    //convert frequency to period in clock ticks
+    PinDescription *p = &g_APinDescription[pin];
+    pwmPeriod[p->ulPwmChan] = F_CPU / freq;
+}
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/cores/arduino/wiring_analog.h b/cores/arduino/wiring_analog.h
index c30aa5ff..5777d534 100644
--- a/cores/arduino/wiring_analog.h
+++ b/cores/arduino/wiring_analog.h
@@ -33,6 +33,8 @@ typedef enum _eAnalogReference
   AR_DEFAULT,
 } eAnalogReference ;
 
+#define PWM_PERIOD 	65306	//490 Hz
+
 /*
  * \brief Configures the reference voltage used for analog input (i.e. the value used as the top of the input range).
  * This function is kept only for compatibility with existing AVR based API.
@@ -47,7 +49,7 @@ extern void analogReference( eAnalogReference ulMode ) ;
  * \param pin
  * \param val
  */
-extern void analogWrite( uint8_t pin, int val ) ;
+extern void analogWrite( uint8_t pin, uint32_t val ) ;
 
 /*
  * \brief Reads the value from the specified analog pin.
@@ -72,6 +74,13 @@ extern void analogReadResolution(int res);
  */
 extern void analogWriteResolution(int res);
 
+/*
+ * \brief Set the frequency of analogWrite parameters.
+ *
+ * \param res
+ */
+extern void analogWriteFrequency(uint8_t pin, uint32_t freq);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/cores/arduino/wiring_digital.c b/cores/arduino/wiring_digital.c
index 73c4fa76..e895a822 100644
--- a/cores/arduino/wiring_digital.c
+++ b/cores/arduino/wiring_digital.c
@@ -31,7 +31,6 @@ void pinMode( uint8_t pin, uint8_t mode )
     PinDescription *p = &g_APinDescription[pin];
 
     if (mode == OUTPUT) {
-        p->ulInputMode = OUTPUT_MODE;
         if (p->ulGPIOType == SS_GPIO) {
             uint32_t reg = p->ulGPIOBase + SS_GPIO_SWPORTA_DDR;
             SET_ARC_BIT(reg, p->ulGPIOId);
@@ -41,7 +40,6 @@ void pinMode( uint8_t pin, uint8_t mode )
             SET_MMIO_BIT(reg, p->ulGPIOId);
         }
     } else {
-        p->ulInputMode = INPUT_MODE;
         if (p->ulGPIOType == SS_GPIO) {
             uint32_t reg = p->ulGPIOBase + SS_GPIO_SWPORTA_DDR;
             CLEAR_ARC_BIT(reg, p->ulGPIOId);
@@ -54,9 +52,10 @@ void pinMode( uint8_t pin, uint8_t mode )
 
     /* Set SoC pin mux configuration */
     SET_PIN_PULLUP(p->ulSocPin, (mode == INPUT_PULLUP) ? 1 : 0);
-    if (p->ulPinMode != GPIO_MUX_MODE) {
-        SET_PIN_MODE(p->ulSocPin, GPIO_MUX_MODE);
-        p->ulPinMode = GPIO_MUX_MODE;
+    SET_PIN_MODE(p->ulSocPin, GPIO_MUX_MODE);
+    if(pinmuxMode[pin] != GPIO_MUX_MODE)
+    {
+        pinmuxMode[pin] = GPIO_MUX_MODE;
     }
 }
 
@@ -65,28 +64,32 @@ void digitalWrite( uint8_t pin, uint8_t val )
     if (pin >= NUM_DIGITAL_PINS) return;
 	
     PinDescription *p = &g_APinDescription[pin];
-
-    if (!p->ulInputMode) {
-        if (p->ulGPIOType == SS_GPIO) {
-            uint32_t reg = p->ulGPIOBase + SS_GPIO_SWPORTA_DR;
-            if (val)
-                SET_ARC_BIT(reg, p->ulGPIOId);
-            else
-                CLEAR_ARC_BIT(reg, p->ulGPIOId);
-        }
-        else if (p->ulGPIOType == SOC_GPIO) {
-            uint32_t reg = p->ulGPIOBase + SOC_GPIO_SWPORTA_DR;
-            if (val)
-                SET_MMIO_BIT(reg, p->ulGPIOId);
-            else
-                CLEAR_MMIO_BIT(reg, p->ulGPIOId);
-        }
-    } else {
-        if (val)
-            SET_PIN_PULLUP(p->ulSocPin,1);
-        else
-            SET_PIN_PULLUP(p->ulSocPin,0);
+    
+    if(pinmuxMode[pin] != GPIO_MUX_MODE)
+    {
+        pinmuxMode[pin] = GPIO_MUX_MODE;
+        SET_PIN_MODE(p->ulSocPin, GPIO_MUX_MODE);
     }
+    
+	if (p->ulGPIOType == SS_GPIO) {
+		uint32_t reg = p->ulGPIOBase + SS_GPIO_SWPORTA_DR;
+		if (val)
+			SET_ARC_BIT(reg, p->ulGPIOId);
+		else
+			CLEAR_ARC_BIT(reg, p->ulGPIOId);
+	}
+	else if (p->ulGPIOType == SOC_GPIO) {
+		uint32_t reg = p->ulGPIOBase + SOC_GPIO_SWPORTA_DR;
+		if (val)
+			SET_MMIO_BIT(reg, p->ulGPIOId);
+		else
+			CLEAR_MMIO_BIT(reg, p->ulGPIOId);
+	}
+
+	if (val)
+		SET_PIN_PULLUP(p->ulSocPin,1);
+	else
+		SET_PIN_PULLUP(p->ulSocPin,0);
 }
 
 int digitalRead( uint8_t pin )
diff --git a/drivers/amd64/WdfCoInstaller01009.dll b/drivers/amd64/WdfCoInstaller01009.dll
new file mode 100644
index 00000000..1731b962
Binary files /dev/null and b/drivers/amd64/WdfCoInstaller01009.dll differ
diff --git a/drivers/amd64/winusbcoinstaller2.dll b/drivers/amd64/winusbcoinstaller2.dll
new file mode 100644
index 00000000..30e55025
Binary files /dev/null and b/drivers/amd64/winusbcoinstaller2.dll differ
diff --git a/drivers/dpinst-amd64.exe b/drivers/dpinst-amd64.exe
new file mode 100644
index 00000000..0507e738
Binary files /dev/null and b/drivers/dpinst-amd64.exe differ
diff --git a/drivers/dpinst-x86.exe b/drivers/dpinst-x86.exe
new file mode 100644
index 00000000..41a890d1
Binary files /dev/null and b/drivers/dpinst-x86.exe differ
diff --git a/drivers/intc_composite.cat b/drivers/intc_composite.cat
new file mode 100644
index 00000000..7c55789c
Binary files /dev/null and b/drivers/intc_composite.cat differ
diff --git a/drivers/intc_composite.inf b/drivers/intc_composite.inf
new file mode 100644
index 00000000..2dc466b2
--- /dev/null
+++ b/drivers/intc_composite.inf
@@ -0,0 +1,46 @@
+;*********************************************************************************************
+; Windows USB Composite Setup File for Arduino 101
+;
+; Copyright (c) 2015 Intel Corporation
+;
+; For use only on Windows operating systems.
+;*********************************************************************************************
+[Version]
+Signature 			= "$WINDOWS NT$"
+Class 				= USB
+ClassGUID 			= {4D36E978-E325-11CE-BFC1-08002BE10318}
+Provider 			= %ProviderName%
+DriverVer			= 10/13/2015,1.1.0.0
+CatalogFile = intc_composite.cat
+
+[Manufacturer]
+%ProviderName% 	=	Generic,NTx86,NTamd64
+
+[Generic.NTx86]
+; Arduino 101
+%USBCompositeDevice% = Composite.Device,USB\VID_8087&PID_0AB6 ; ACM+ACM
+
+[Generic.NTamd64]
+; Arduino 101
+%USBCompositeDevice% = Composite.Device,USB\VID_8087&PID_0AB6 ; ACM+ACM
+
+[ControlFlags]
+ExcludeFromSelect=*
+
+; Common Class Generic Parent for root ID
+[Composite.Device]
+Include=usb.inf
+Needs=Composite.Dev.NT
+
+[Composite.Device.Services]
+Include=usb.inf
+Needs=Composite.Dev.NT.Services
+
+[SourceDisksNames]
+1=%SourceDisk%,,1
+
+[Strings]
+ProviderName 		= "Intel Corporation"
+ServiceDisplayName	= "Arduino 101 USB Composite Device Driver"
+USBCompositeDevice	= "Arduino 101 USB Composite Device"
+SourceDisk 		= "Arduino 101 USB Composite Device Install Disk"
diff --git a/drivers/intc_libusb.cat b/drivers/intc_libusb.cat
new file mode 100644
index 00000000..ea90e2ca
Binary files /dev/null and b/drivers/intc_libusb.cat differ
diff --git a/drivers/intc_libusb.inf b/drivers/intc_libusb.inf
new file mode 100644
index 00000000..cfc698d0
--- /dev/null
+++ b/drivers/intc_libusb.inf
@@ -0,0 +1,174 @@
+; ======== libusb 1.0 (WinUSB) device driver ==========
+;
+; To customize this inf file for your own device
+;
+; 1. Change "DeviceName" with the name you want your device to appear with 
+;    on your system.
+;
+; 2. Change "VendorID" and "ProductID" according to those of your device. 
+;    If your device is plugged in, you can retrieve these values through the 
+;    Device Manager (regardless of whether the driver is installed or not).
+;
+; 3. Change "DeviceGUID" to a value that is unique on your system. For more 
+;    information and tools to help you generate your own GUIDs, see 
+;    http://en.wikipedia.org/wiki/Universally_Unique_Identifier.
+;
+; 4. Change "DeviceClassGUID" to reflect your USB Device Class. 
+;    The following Device Classes are listed for reference:
+;    {745a17a0-74d3-11d0-b6fe-00a0c90f57da} : HID device
+;    {78a1c341-4539-11d3-b88d-00c04fad5171} : Generic WinUSB device
+;
+; 5. (Optional) Change the "Date" string.
+;
+; Note 1: if you need to create a matching cat file for this inf, you can use
+; the inf2cat utility from the WinDDK, with the the following command:
+; inf2cat /driver:"path_to_your inf" /os:7_X86,7_X64,Vista_X86,Vista_X64
+;
+; Note 2: The co-installers provided in these files are version 1.9.
+; Please refer to:
+; http://blogs.msdn.com/iliast/archive/2008/03/10/why-do-we-need-wdf-coinstallers.aspx and
+; http://blogs.msdn.com/iliast/archive/2009/08/13/wdf-logo-requirements-regarding-coinstallers.aspx
+; for more information about co-installers and their versioning
+
+; ===================== Strings =======================
+
+[Strings]
+
+; =====================================================
+; ========= START USER CONFIGURABLE SECTION ===========
+; =====================================================
+
+DeviceName = "Arduino 101 DFU Interface"
+VendorID = "VID_8087"
+ProductID = "PID_0ABA"
+DeviceGUID = "{d35924d6-3e16-4a9e-9782-5524a4b79bac}"
+DeviceClassGUID = "{78a1c341-4539-11d3-b88d-00c04fad5171}"
+; Date MUST be in MM/DD/YYYY format
+Date = "10/13/2015"
+
+; =====================================================
+; ========== END USER CONFIGURABLE SECTION ============
+; =====================================================
+
+ProviderName = "libusb 1.0"
+WinUSB_SvcDesc = "WinUSB Driver Service"
+DiskName = "libusb (WinUSB) Device Install Disk"
+ClassName = "libusb (WinUSB) devices"
+
+; ====================== Version ======================
+
+[Version]
+DriverVer = %Date%
+Signature = "$Windows NT$"
+Class = %ClassName%
+ClassGuid = %DeviceClassGUID%
+Provider = %ProviderName%
+CatalogFile = intc_libusb.cat
+
+; =================== Class section ===================
+
+; Since the device is not a standard USB device, we define a new class for it.
+[ClassInstall32]
+Addreg = WinUSBDeviceClassReg
+
+[WinUSBDeviceClassReg]
+HKR,,,0,%ClassName%
+; -20 is for the USB icon
+HKR,,Icon,,-20
+
+; =========== Manufacturer/Models sections ============
+
+[Manufacturer]
+%ProviderName% = libusbDevice_WinUSB,NTx86,NTamd64
+
+[libusbDevice_WinUSB.NTx86]
+%DeviceName% = USB_Install, USB\%VendorID%&%ProductID%
+
+[libusbDevice_WinUSB.NTamd64]
+%DeviceName% = USB_Install, USB\%VendorID%&%ProductID%
+
+; ==================== Installation ===================
+
+; The Include and Needs directives in the USB_Install section are required for 
+; installing WinUSB on Windows Vista systems. Windows XP systems ignore these 
+; directives. These directives should not be modified.
+[USB_Install]
+Include=winusb.inf
+Needs=WINUSB.NT
+
+; The Include directive in the USB_Install.Services section includes the system-
+; supplied INF for WinUSB. This INF is installed by the WinUSB co-installer if 
+; it is not already on the target system. The AddService directive specifies 
+; WinUsb.sys as the device’s function driver. These directives should not be 
+; modified.
+[USB_Install.Services]
+Include=winusb.inf
+AddService=WinUSB,0x00000002,WinUSB_ServiceInstall
+
+; The WinUSB_ServiceInstall section contains the data for installing WinUsb.sys 
+; as a service. This section should not be modified.
+[WinUSB_ServiceInstall]
+DisplayName     = %WinUSB_SvcDesc%
+ServiceType     = 1
+StartType       = 3
+ErrorControl    = 1
+ServiceBinary   = %12%\WinUSB.sys
+
+; The KmdfService directive installs WinUsb.sys as a kernel-mode service. The 
+; referenced WinUSB_Install section specifies the KMDF library version. 
+; Usually, the version can be derived from the WdfCoInstallerxxyyy.dll with 
+; xx = major, yyy = minor
+[USB_Install.Wdf]
+KmdfService=WINUSB, WinUsb_Install
+
+[WinUSB_Install]
+KmdfLibraryVersion=1.9
+
+; USB_Install.HW is the key section in the INF. It specifies the device 
+; interface globally unique identifier (GUID) for your device. The AddReg 
+; directive puts the interface GUID in a standard registry value. When 
+; WinUsb.sys is loaded as the device’s function driver, it reads the registry
+; value and uses the specified GUID to represent the device interface. You 
+; should replace the GUID in this example with one that you create specifically
+; for your device. If the protocols for the device change, you should create a
+; new device interface GUID.
+[USB_Install.HW]
+AddReg=Dev_AddReg
+
+[Dev_AddReg]
+HKR,,DeviceInterfaceGUIDs,0x10000,%DeviceGUID%
+
+; The USB_Install.CoInstallers section, including the referenced AddReg and 
+; CopyFiles sections, contains data and instructions to install the WinUSB and 
+; KMDF co installers and associate them with the device. Most USB devices can 
+; use these sections and directives without modification.
+[USB_Install.CoInstallers]
+AddReg=CoInstallers_AddReg
+CopyFiles=CoInstallers_CopyFiles
+
+[CoInstallers_AddReg]
+HKR,,CoInstallers32,0x00010000,"WdfCoInstaller01009.dll,WdfCoInstaller","WinUSBCoInstaller2.dll"
+
+[CoInstallers_CopyFiles]
+WinUSBCoInstaller2.dll
+WdfCoInstaller01009.dll
+
+[DestinationDirs]
+CoInstallers_CopyFiles=11
+
+; =============== Source Media Section ================
+ 
+; The x86 and x64 versions of Windows have separate co installers. This example 
+; stores them on the installation disk in folders that are named x86 and amd64
+[SourceDisksNames]
+1 = %DiskName%,,,\x86
+2 = %DiskName%,,,\amd64
+
+[SourceDisksFiles.x86]
+WinUSBCoInstaller2.dll=1
+WdfCoInstaller01009.dll=1
+
+[SourceDisksFiles.amd64]
+WinUSBCoInstaller2.dll=2
+WdfCoInstaller01009.dll=2
+
diff --git a/drivers/intc_serial.cat b/drivers/intc_serial.cat
new file mode 100644
index 00000000..ab02985d
Binary files /dev/null and b/drivers/intc_serial.cat differ
diff --git a/drivers/intc_serial.inf b/drivers/intc_serial.inf
new file mode 100644
index 00000000..e3e7439a
--- /dev/null
+++ b/drivers/intc_serial.inf
@@ -0,0 +1,73 @@
+;************************************************************
+; Windows USB CDC ACM Setup File
+; Copyright (c) 2000 Microsoft Corporation
+
+
+[Version]
+Signature="$Windows NT$"
+Class=Ports
+ClassGuid={4D36E978-E325-11CE-BFC1-08002BE10318}
+Provider=%MFGNAME%
+DriverVer=10/13/2015,5.1.2600.0
+CatalogFile = intc_serial.cat
+
+[Manufacturer]
+%MFGNAME%=DeviceList, NTamd64
+
+[DestinationDirs]
+DefaultDestDir=12
+
+
+;------------------------------------------------------------------------------
+;  Vendor and Product ID Definitions
+;------------------------------------------------------------------------------
+; When developing your USB device, the VID and PID used in the PC side
+; application program and the firmware on the microcontroller must match.
+; Modify the below line to use your VID and PID.  Use the format as shown below.
+; Note: One INF file can be used for multiple devices with different VID and PIDs.
+; For each supported device, append ",USB\VID_xxxx&PID_yyyy" to the end of the line.
+;------------------------------------------------------------------------------
+
+[DeviceList]
+%DESCRIPTION%=DriverInstall, USB\VID_8087&PID_0AB6
+
+[DeviceList.NTamd64]
+%DESCRIPTION%=DriverInstall, USB\VID_8087&PID_0AB6
+
+
+
+;------------------------------------------------------------------------------
+;  String Definitions
+;------------------------------------------------------------------------------
+;Modify these strings to customize your device
+;------------------------------------------------------------------------------
+[Strings]
+MFGFILENAME="CDC_vista"
+DRIVERFILENAME ="usbser"
+MFGNAME="http://www.intel.com"
+INSTDISK="Arduino 101 Driver Installer"
+DESCRIPTION="Arduino 101 Serial Monitor"
+SERVICE="USB RS-232 Emulation Driver"
+
+
+[DriverInstall]
+include=mdmcpq.inf,usb.inf
+CopyFiles = FakeModemCopyFileSection
+AddReg=DriverAddReg
+
+[DriverAddReg]
+HKR,,DevLoader,,*ntkern 
+HKR,,NTMPDriver,,usbser.sys 
+HKR,,EnumPropPages32,,"MsPorts.dll,SerialPortPropPageProvider"
+
+[DriverInstall.Services]
+include=mdmcpq.inf
+AddService=usbser, 0x00000002, DriverService
+
+[DriverService]
+DisplayName=%ServiceName%
+ServiceType=1
+StartType=3
+ErrorControl=1
+ServiceBinary=%12%\usbser.sys
+LoadOrderGroup=Base
diff --git a/drivers/x86/WdfCoInstaller01009.dll b/drivers/x86/WdfCoInstaller01009.dll
new file mode 100644
index 00000000..30e81af6
Binary files /dev/null and b/drivers/x86/WdfCoInstaller01009.dll differ
diff --git a/drivers/x86/winusbcoinstaller2.dll b/drivers/x86/winusbcoinstaller2.dll
new file mode 100644
index 00000000..fc450d2b
Binary files /dev/null and b/drivers/x86/winusbcoinstaller2.dll differ
diff --git a/libraries/CurieBLE/README.md b/libraries/CurieBLE/README.md
new file mode 100644
index 00000000..3642943d
--- /dev/null
+++ b/libraries/CurieBLE/README.md
@@ -0,0 +1,18 @@
+# Enable debug interface on Serail1
+
+* Default disable the debug interface.
+
+If you want to enable debug trace on Serial1 to debug corelib,  follow these instructions.
+
+1. Shut down the IDE
+2. Go to Arduino15 directory
+  * Windows: `C:\Users\<user>\AppData\Roaming\Arduino15`
+  * OS X: `~/Library/Arduino15`
+  * Linux: `~/.arduino15`
+3. Modify the platform.txt
+  * Find `compiler.c.flags` and add `-DCONFIGURE_DEBUG_CORELIB_ENABLED` at the end of this line
+  * Find `compiler.cpp.flags` and add `-DCONFIGURE_DEBUG_CORELIB_ENABLED` at the end of this line
+4. Initial Serial1 in your sketch
+  * Add `Serial1.begin(115200);` in your `setup()`
+5. Adjust the output level at log_init function in log.c
+
diff --git a/libraries/CurieBLE/examples/Central/LedControl/LedControl.ino b/libraries/CurieBLE/examples/Central/LedControl/LedControl.ino
new file mode 100644
index 00000000..2c1706a1
--- /dev/null
+++ b/libraries/CurieBLE/examples/Central/LedControl/LedControl.ino
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ * See the bottom of this file for the license terms.
+ */
+
+/*
+ * Sketch: LedControl.ino
+ *
+ * Description:
+ *   This is a Central sketch that looks for a particular Sevice with a
+ *   certain Characteristic from a Peripheral.  Upon succesful discovery,
+ *   it reads the state of a button and write that value to the
+ *   Peripheral Characteristic.
+ *
+ * Notes:
+ *
+ *  - Expected Peripheral Service: 19b10000-e8f2-537e-4f6c-d104768a1214
+ *  - Expected Peripheral Characteristic: 19b10001-e8f2-537e-4f6c-d104768a1214
+ *  - Expected Peripheral sketch:
+ *
+ */
+
+#include <CurieBLE.h>
+
+// variables for button
+const int buttonPin = 2;
+int oldButtonState = LOW;
+
+
+void setup() {
+  Serial.begin(9600);
+
+  // configure the button pin as input
+  pinMode(buttonPin, INPUT);
+
+  // initialize the BLE hardware
+  BLE.begin();
+
+  Serial.println("BLE Central - LED control");
+
+  // start scanning for peripherals
+  BLE.scanForUuid("19b10000-e8f2-537e-4f6c-d104768a1214");
+}
+
+void loop() {
+  // check if a peripheral has been discovered
+  BLEDevice peripheral = BLE.available();
+
+  if (peripheral) {
+    // discovered a peripheral, print out address, local name, and advertised service
+    Serial.print("Found ");
+    Serial.print(peripheral.address());
+    Serial.print(" '");
+    Serial.print(peripheral.localName());
+    Serial.print("' ");
+    Serial.print(peripheral.advertisedServiceUuid());
+    Serial.println();
+
+    // stop scanning
+    BLE.stopScan();
+
+    controlLed(peripheral);
+
+    // peripheral disconnected, start scanning again
+    BLE.scanForUuid("19b10000-e8f2-537e-4f6c-d104768a1214");
+  }
+}
+
+void controlLed(BLEDevice peripheral) {
+  // connect to the peripheral
+  Serial.println("Connecting ...");
+
+  if (peripheral.connect()) {
+    Serial.println("Connected");
+  } else {
+    Serial.println("Failed to connect!");
+    return;
+  }
+
+  // discover peripheral attributes
+  Serial.println("Discovering attributes ...");
+  if (peripheral.discoverAttributes()) {
+    Serial.println("Attributes discovered");
+  } else {
+    Serial.println("Attribute discovery failed!");
+    peripheral.disconnect();
+    return;
+  }
+
+  // retrieve the LED characteristic
+  BLECharacteristic ledCharacteristic = peripheral.characteristic("19b10001-e8f2-537e-4f6c-d104768a1214");
+
+  if (!ledCharacteristic) {
+    Serial.println("Peripheral does not have LED characteristic!");
+    peripheral.disconnect();
+    return;
+  } else if (!ledCharacteristic.canWrite()) {
+    Serial.println("Peripheral does not have a writable LED characteristic!");
+    peripheral.disconnect();
+    return;
+  }
+
+  while (peripheral.connected()) {
+    // while the peripheral is connection
+
+    // read the button pin
+    int buttonState = digitalRead(buttonPin);
+
+    if (oldButtonState != buttonState) {
+      // button changed
+      oldButtonState = buttonState;
+
+      if (buttonState) {
+        Serial.println("button pressed");
+
+        // button is pressed, write 0x01 to turn the LED on
+        ledCharacteristic.writeByte(0x01);
+      } else {
+        Serial.println("button released");
+
+        // button is released, write 0x00 to turn the LED of
+        ledCharacteristic.writeByte(0x00);
+      }
+    }
+  }
+
+  Serial.println("Peripheral disconnected");
+}
+
+/*
+  Arduino BLE Central LED Control example
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+
+
diff --git a/libraries/CurieBLE/examples/Central/PeripheralExplorer/PeripheralExplorer.ino b/libraries/CurieBLE/examples/Central/PeripheralExplorer/PeripheralExplorer.ino
new file mode 100644
index 00000000..b89492b1
--- /dev/null
+++ b/libraries/CurieBLE/examples/Central/PeripheralExplorer/PeripheralExplorer.ino
@@ -0,0 +1,193 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ * See the bottom of this file for the license terms.
+ */
+
+/*
+ * Sketch:PeripheralExplorer.ino
+ *
+ * Description:
+ *   This is a Central sketch demonstrating the discovery process
+ *   of a Peripheral.  The discovered Attributes are being
+ *   display onto the serial output.
+ *
+ * Notes:
+ *
+ *  - Expected Peripheral name: LED
+ *
+ */
+
+#include <CurieBLE.h>
+
+void setup() {
+  Serial.begin(9600);
+
+  // initialize the BLE hardware
+  BLE.begin();
+
+  Serial.println("BLE Central - Peripheral Explorer");
+
+  // start scanning for peripherals
+  BLE.scan();
+}
+
+void loop() {
+  // check if a peripheral has been discovered
+  BLEDevice peripheral = BLE.available();
+
+  if (peripheral) {
+    // discovered a peripheral, print out address, local name, and advertised service
+    Serial.print("Found ");
+    Serial.print(peripheral.address());
+    Serial.print(" '");
+    Serial.print(peripheral.localName());
+    Serial.print("' ");
+    Serial.print(peripheral.advertisedServiceUuid());
+    Serial.println();
+
+    // see if peripheral is a LED
+    if (peripheral.localName() == "LED") {
+      // stop scanning
+      BLE.stopScan();
+
+      explorerPeripheral(peripheral);
+
+      // peripheral disconnected, we are done
+      while (1) {
+        // do nothing
+      }
+    }
+  }
+}
+
+void explorerPeripheral(BLEDevice peripheral) {
+  // connect to the peripheral
+  Serial.println("Connecting ...");
+
+  if (peripheral.connect()) {
+    Serial.println("Connected");
+  } else {
+    Serial.println("Failed to connect!");
+    return;
+  }
+
+  // discover peripheral attributes
+  Serial.println("Discovering attributes ...");
+  if (peripheral.discoverAttributes()) {
+    Serial.println("Attributes discovered");
+  } else {
+    Serial.println("Attribute discovery failed!");
+    peripheral.disconnect();
+    return;
+  }
+
+  // read and print device name of peripheral
+  Serial.println();
+  Serial.print("Device name: ");
+  Serial.println(peripheral.deviceName());
+
+  // loop the services of the peripheral and explore each
+  for (int i = 0; i < peripheral.serviceCount(); i++) {
+    BLEService service = peripheral.service(i);
+
+    exploreService(service);
+  }
+
+  Serial.println();
+
+  // we are done exploring, disconnect
+  Serial.println("Disconnecting ...");
+  peripheral.disconnect();
+  Serial.println("Disconnected");
+}
+
+void exploreService(BLEService service) {
+  // print the UUID of the service
+  Serial.print("Service ");
+  Serial.println(service.uuid());
+
+  // loop the characteristics of the service and explore each
+  for (int i = 0; i < service.characteristicCount(); i++) {
+    BLECharacteristic characteristic = service.characteristic(i);
+
+    exploreCharacteristic(characteristic);
+  }
+}
+
+void exploreCharacteristic(BLECharacteristic characteristic) {
+  // print the UUID and properies of the characteristic
+  Serial.print("\tCharacteristic ");
+  Serial.print(characteristic.uuid());
+  Serial.print(", properties 0x");
+  Serial.print(characteristic.properties());
+
+  // check if the characteristic is readable
+  if (characteristic.canRead()) {
+    // read the characteristic value
+    characteristic.read();
+
+    if (characteristic.valueLength() > 0)
+    {
+      // print out the value of the characteristic
+      Serial.print(", value 0x");
+      printData(characteristic.value(), characteristic.valueLength());
+    }
+  }
+  Serial.println();
+
+  // loop the descriptors of the characteristic and explore each
+  for (int i = 0; i < characteristic.descriptorCount(); i++) {
+    BLEDescriptor descriptor = characteristic.descriptor(i);
+
+    exploreDescriptor(descriptor);
+  }
+}
+
+void exploreDescriptor(BLEDescriptor descriptor) {
+  // print the UUID of the descriptor
+  Serial.print("\t\tDescriptor ");
+  Serial.print(descriptor.uuid());
+
+  // read the descriptor value
+  descriptor.read();
+
+  // print out the value of the descriptor
+  Serial.print(", value 0x");
+  printData(descriptor.value(), descriptor.valueLength());
+
+  Serial.println();
+}
+
+void printData(const unsigned char data[], int length) {
+  for (int i = 0; i < length; i++) {
+    unsigned char b = data[i];
+
+    if (b < 16) {
+      Serial.print("0");
+    }
+
+    Serial.print(b, HEX);
+  }
+}
+
+
+/*
+  Arduino BLE Central peripheral explorer example
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+
diff --git a/libraries/CurieBLE/examples/Central/Scan/Scan.ino b/libraries/CurieBLE/examples/Central/Scan/Scan.ino
new file mode 100644
index 00000000..6af4b49f
--- /dev/null
+++ b/libraries/CurieBLE/examples/Central/Scan/Scan.ino
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ * See the bottom of this file for the license terms.
+ */
+
+/*
+ * Sketch: Scan.ino
+ *
+ * Description:
+ *   This is a Central sketch that performs scanning of Peripherals
+ *   only.  It does not perform connection.  The Advertized info
+ *   of a Peripheral are display.
+ *
+ * Notes:
+ *
+ */
+
+#include <CurieBLE.h>
+
+void setup() {
+  Serial.begin(9600);
+
+  // initialize the BLE hardware
+  BLE.begin();
+
+  Serial.println("BLE Central scan");
+
+  // start scanning for peripheral
+  BLE.scan();
+}
+
+void loop() {
+  // check if a peripheral has been discovered
+  BLEDevice peripheral = BLE.available();
+
+  if (peripheral) {
+    // discovered a peripheral
+    Serial.println("Discovered a peripheral");
+    Serial.println("-----------------------");
+
+    // print address
+    Serial.print("Address: ");
+    Serial.println(peripheral.address());
+
+    // print the local name, if present
+    if (peripheral.hasLocalName()) {
+      Serial.print("Local Name: ");
+      Serial.println(peripheral.localName());
+    }
+
+    // print the advertised service UUID's, if present
+    if (peripheral.hasAdvertisedServiceUuid()) {
+      Serial.print("Service UUID's: ");
+      for (int i = 0; i < peripheral.advertisedServiceUuidCount(); i++) {
+        Serial.print(peripheral.advertisedServiceUuid(i));
+        Serial.print(" ");
+      }
+      Serial.println();
+    }
+
+    // print the RSSI
+    Serial.print("RSSI: ");
+    Serial.println(peripheral.rssi());
+
+    Serial.println();
+  }
+}
+
+
+/*
+  Arduino BLE Central scan example
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
diff --git a/libraries/CurieBLE/examples/Central/ScanCallback/ScanCallback.ino b/libraries/CurieBLE/examples/Central/ScanCallback/ScanCallback.ino
new file mode 100644
index 00000000..4c47decd
--- /dev/null
+++ b/libraries/CurieBLE/examples/Central/ScanCallback/ScanCallback.ino
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ * See the bottom of this file for the license terms.
+ */
+
+/*
+ * Sketch: ScanCallback.ino
+ *
+ * Description:
+ *   This is a Central Sketch that scan for Peripherals without
+ *   performing connection.  It displays the Adv info for each
+ *   Peripheral scanned.  Notice that this sketch makes use of
+ *   the Event Handler, a call back routine, to display the
+ *   info onto the serial output.
+ *
+ * Notes:
+ *
+ *   - It is highly recommended not to use the Event Handler to
+ *     dump information to the serial monitor.  Please note
+ *     that Event Handler executes in interrupt context and it
+ *     is expected to perform short execution task.  It is due
+ *     to the fact that the entire system timing is impact by the
+ *     execution time of the Event Handler.  Accessing the serial
+ *     monitor is relatively time consuming, it is highly desirable
+ *     to perform that in the background.
+ *
+ */
+
+#include <CurieBLE.h>
+
+void setup() {
+  Serial.begin(9600);
+
+  // initialize the BLE hardware
+  BLE.begin();
+
+  Serial.println("BLE Central scan callback");
+
+  // set the discovered event handle
+  BLE.setEventHandler(BLEDiscovered, bleCentralDiscoverHandler);
+
+  // start scanning for peripherals with duplicates
+  BLE.scan(true);
+}
+
+void loop() {
+  // poll the central for events
+  BLE.poll();
+}
+
+void bleCentralDiscoverHandler(BLEDevice peripheral) {
+  // discovered a peripheral
+  Serial.println("Discovered a peripheral");
+  Serial.println("-----------------------");
+
+  // print address
+  Serial.print("Address: ");
+  Serial.println(peripheral.address());
+
+  // print the local name, if present
+  if (peripheral.hasLocalName()) {
+    Serial.print("Local Name: ");
+    Serial.println(peripheral.localName());
+  }
+
+  // print the advertised service UUID's, if present
+    if (peripheral.hasAdvertisedServiceUuid()) {
+    Serial.print("Service UUID's: ");
+    for (int i = 0; i < peripheral.advertisedServiceUuidCount(); i++) {
+      Serial.print(peripheral.advertisedServiceUuid(i));
+      Serial.print(" ");
+    }
+    Serial.println();
+  }
+
+  // print the RSSI
+  Serial.print("RSSI: ");
+  Serial.println(peripheral.rssi());
+
+  Serial.println();
+}
+
+
+
+/*
+  Arduino BLE Central scan callback example
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
diff --git a/libraries/CurieBLE/examples/Central/SensortagButton/SensortagButton.ino b/libraries/CurieBLE/examples/Central/SensortagButton/SensortagButton.ino
new file mode 100644
index 00000000..75de9267
--- /dev/null
+++ b/libraries/CurieBLE/examples/Central/SensortagButton/SensortagButton.ino
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ * See the bottom of this file for the license terms.
+ */
+
+/*
+ * Sketch: SensortagButton.ino
+ *
+ * Description:
+ *   This Central sketch scan for a Peripheral called the SensorTag.
+ *   It looks for particular Service, discovers all its attributes,
+ *   and them on the serial monitor.
+ *
+ */
+
+#include <CurieBLE.h>
+
+void setup() {
+  Serial.begin(9600);
+  while (!Serial);
+
+  // initialize the BLE hardware
+  BLE.begin();
+
+  Serial.println("BLE Central - SensorTag button");
+  Serial.println("Make sure to turn on the device.");
+
+  // start scanning for peripheral
+  BLE.scan();
+}
+
+void loop() {
+  // check if a peripheral has been discovered
+  BLEDevice peripheral = BLE.available();
+
+  if (peripheral) {
+    // discovered a peripheral, print out address, local name, and advertised service
+    Serial.print("Found ");
+    Serial.print(peripheral.address());
+    Serial.print(" '");
+    Serial.print(peripheral.localName());
+    Serial.print("' ");
+    Serial.print(peripheral.advertisedServiceUuid());
+    Serial.println();
+
+    /* see if peripheral is a SensorTag
+     * The localName, CC2650 SensorTag, is in the Scan Response Data packet.
+     * If this is not the expected name, please change the following
+     * if-statement accordingly.
+     */
+    if (peripheral.localName() == "CC2650 SensorTag") {
+      // stop scanning
+      BLE.stopScan();
+
+      monitorSensorTagButtons(peripheral);
+
+      // peripheral disconnected, start scanning again
+      BLE.scan();
+    }
+  }
+}
+
+void monitorSensorTagButtons(BLEDevice peripheral)
+{
+  // connect to the peripheral
+  Serial.println("Connecting ...");
+  if (peripheral.connect()) {
+    Serial.println("Connected");
+  } else {
+    Serial.println("Failed to connect!");
+    return;
+  }
+
+  // discover peripheral attributes
+  Serial.println("Discovering attributes of service 0xffe0 ...");
+  if (peripheral.discoverAttributesByService("ffe0")) {
+    Serial.println("Attributes discovered");
+  } else {
+    Serial.println("Attribute discovery failed.");
+    peripheral.disconnect();
+    return;
+  }
+
+  // retrieve the simple key characteristic
+  BLECharacteristic simpleKeyCharacteristic = peripheral.characteristic("ffe1");
+
+  // subscribe to the simple key characteristic
+  Serial.println("Subscribing to simple key characteristic ...");
+  if (!simpleKeyCharacteristic) {
+    Serial.println("no simple key characteristic found!");
+    peripheral.disconnect();
+    return;
+  } else if (!simpleKeyCharacteristic.canSubscribe()) {
+    Serial.println("simple key characteristic is not subscribable!");
+    peripheral.disconnect();
+    return;
+  } else if (!simpleKeyCharacteristic.subscribe()) {
+    Serial.println("subscription failed!");
+    peripheral.disconnect();
+    return;
+  } else {
+    Serial.println("Subscribed");
+    Serial.println("Press the right and left buttons on your Sensor Tag.");
+  }
+
+  while (peripheral.connected()) {
+    // while the peripheral is connected
+
+    // check if the value of the simple key characteristic has been updated
+    if (simpleKeyCharacteristic.valueUpdated()) {
+      // yes, get the value, characteristic is 1 byte so use char value
+      int value = simpleKeyCharacteristic.charValue();
+
+      if (value & 0x01) {
+        // first bit corresponds to the right button
+        Serial.println("Right button pressed");
+      }
+
+      if (value & 0x02) {
+        // second bit corresponds to the left button
+        Serial.println("Left button pressed");
+      }
+    }
+  }
+
+  Serial.println("SensorTag disconnected!");
+}
+
+
+
+/*
+  Arduino BLE Central SensorTag button example
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+
diff --git a/libraries/CurieBLE/examples/BatteryMonitor/BatteryMonitor.ino b/libraries/CurieBLE/examples/Peripheral/BatteryMonitor/BatteryMonitor.ino
similarity index 78%
rename from libraries/CurieBLE/examples/BatteryMonitor/BatteryMonitor.ino
rename to libraries/CurieBLE/examples/Peripheral/BatteryMonitor/BatteryMonitor.ino
index 688b147f..8535a697 100644
--- a/libraries/CurieBLE/examples/BatteryMonitor/BatteryMonitor.ino
+++ b/libraries/CurieBLE/examples/Peripheral/BatteryMonitor/BatteryMonitor.ino
@@ -1,17 +1,22 @@
 /*
- * Copyright (c) 2016 Intel Corporation.  All rights reserved.
- * See the bottom of this file for the license terms.
+   Copyright (c) 2016 Intel Corporation.  All rights reserved.
+   See the bottom of this file for the license terms.
+*/
+
+/*
+ * Sketch: BatteryMonitor.ino
+ *
+ * Description:
+ *     This sketch example partially implements the standard Bluetooth
+ *   Low-Energy Battery service and connection interval paramater update.
+ *
+ *   For more information:
+ *     https://developer.bluetooth.org/gatt/services/Pages/ServicesHome.aspx
+ *
  */
 
 #include <CurieBLE.h>
 
-/*
-   This sketch example partially implements the standard Bluetooth Low-Energy Battery service.
-   For more information: https://developer.bluetooth.org/gatt/services/Pages/ServicesHome.aspx
-*/
-
-/*  */
-BLEPeripheral blePeripheral;       // BLE Peripheral Device (the board you're programming)
 BLEService batteryService("180F"); // BLE Battery Service
 
 // BLE Battery Level Characteristic"
@@ -26,26 +31,33 @@ void setup() {
   Serial.begin(9600);    // initialize serial communication
   pinMode(13, OUTPUT);   // initialize the LED on pin 13 to indicate when a central is connected
 
+  // begin initialization
+  BLE.begin();
+
   /* Set a local name for the BLE device
      This name will appear in advertising packets
      and can be used by remote devices to identify this BLE device
-     The name can be changed but maybe be truncated based on space left in advertisement packet */
-  blePeripheral.setLocalName("BatteryMonitorSketch");
-  blePeripheral.setAdvertisedServiceUuid(batteryService.uuid());  // add the service UUID
-  blePeripheral.addAttribute(batteryService);   // Add the BLE Battery service
-  blePeripheral.addAttribute(batteryLevelChar); // add the battery level characteristic
+     The name can be changed but maybe be truncated based on space left in advertisement packet
+  */
+  BLE.setLocalName("BatteryMonitor");
+  BLE.setAdvertisedService(batteryService);  // add the service UUID
+  batteryService.addCharacteristic(batteryLevelChar); // add the battery level characteristic
+  BLE.addService(batteryService);   // Add the BLE Battery service
   batteryLevelChar.setValue(oldBatteryLevel);   // initial value for this characteristic
 
-  /* Now activate the BLE device.  It will start continuously transmitting BLE
+  /* Start advertising BLE.  It will start continuously transmitting BLE
      advertising packets and will be visible to remote BLE central devices
      until it receives a new connection */
-  blePeripheral.begin();
+
+  // start advertising
+  BLE.advertise();
+
   Serial.println("Bluetooth device active, waiting for connections...");
 }
 
 void loop() {
   // listen for BLE peripherals to connect:
-  BLECentral central = blePeripheral.central();
+  BLEDevice central = BLE.central();
 
   // if a central is connected to peripheral:
   if (central) {
diff --git a/libraries/CurieBLE/examples/ButtonLED/ButtonLED.ino b/libraries/CurieBLE/examples/Peripheral/ButtonLED/ButtonLED.ino
similarity index 83%
rename from libraries/CurieBLE/examples/ButtonLED/ButtonLED.ino
rename to libraries/CurieBLE/examples/Peripheral/ButtonLED/ButtonLED.ino
index b4cd873a..fb91c18e 100644
--- a/libraries/CurieBLE/examples/ButtonLED/ButtonLED.ino
+++ b/libraries/CurieBLE/examples/Peripheral/ButtonLED/ButtonLED.ino
@@ -8,7 +8,6 @@
 const int ledPin = 13; // set ledPin to on-board LED
 const int buttonPin = 4; // set buttonPin to digital pin 4
 
-BLEPeripheral blePeripheral; // create peripheral instance
 BLEService ledService("19B10010-E8F2-537E-4F6C-D104768A1214"); // create service
 
 
@@ -22,34 +21,39 @@ void setup() {
   pinMode(ledPin, OUTPUT); // use the LED on pin 13 as an output
   pinMode(buttonPin, INPUT); // use button pin 4 as an input
 
+  // begin initialization
+  BLE.begin();
+
   // set the local name peripheral advertises
-  blePeripheral.setLocalName("ButtonLED");
+  BLE.setLocalName("BtnLED");
   // set the UUID for the service this peripheral advertises:
-  blePeripheral.setAdvertisedServiceUuid(ledService.uuid());
+  BLE.setAdvertisedService(ledService);
+
+  // add the characteristics to the service
+  ledService.addCharacteristic(ledCharacteristic);
+  ledService.addCharacteristic(buttonCharacteristic);
 
-  // add service and characteristics
-  blePeripheral.addAttribute(ledService);
-  blePeripheral.addAttribute(ledCharacteristic);
-  blePeripheral.addAttribute(buttonCharacteristic);
+  // add the service
+  BLE.addService(ledService);
 
   ledCharacteristic.setValue(0);
   buttonCharacteristic.setValue(0);
 
-  // advertise the service
-  blePeripheral.begin();
+  // start advertising
+  BLE.advertise();
 
   Serial.println("Bluetooth device active, waiting for connections...");
 }
 
 void loop() {
-  // poll peripheral
-  blePeripheral.poll();
+  // poll for BLE events
+  BLE.poll();
 
   // read the current button pin state
   char buttonValue = digitalRead(buttonPin);
 
   // has the value changed since the last read
-  boolean buttonChanged = (buttonCharacteristic.value() != buttonValue);
+  bool buttonChanged = (buttonCharacteristic.value() != buttonValue);
 
   if (buttonChanged) {
     // button state changed, update characteristics
@@ -87,3 +91,4 @@ void loop() {
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-
   1301 USA
 */
+  
\ No newline at end of file
diff --git a/libraries/CurieBLE/examples/CallbackLED/CallbackLED.ino b/libraries/CurieBLE/examples/Peripheral/CallbackLED/CallbackLED.ino
similarity index 74%
rename from libraries/CurieBLE/examples/CallbackLED/CallbackLED.ino
rename to libraries/CurieBLE/examples/Peripheral/CallbackLED/CallbackLED.ino
index f8788731..c0d02e3e 100644
--- a/libraries/CurieBLE/examples/CallbackLED/CallbackLED.ino
+++ b/libraries/CurieBLE/examples/Peripheral/CallbackLED/CallbackLED.ino
@@ -6,7 +6,6 @@
 #include <CurieBLE.h>
 
 const int ledPin = 13; // set ledPin to use on-board LED
-BLEPeripheral blePeripheral; // create peripheral instance
 
 BLEService ledService("19B10000-E8F2-537E-4F6C-D104768A1214"); // create service
 
@@ -17,47 +16,53 @@ void setup() {
   Serial.begin(9600);
   pinMode(ledPin, OUTPUT); // use the LED on pin 13 as an output
 
+  // begin initialization
+  BLE.begin();
+
   // set the local name peripheral advertises
-  blePeripheral.setLocalName("LEDCB");
+  BLE.setLocalName("LEDCB");
   // set the UUID for the service this peripheral advertises
-  blePeripheral.setAdvertisedServiceUuid(ledService.uuid());
+  BLE.setAdvertisedService(ledService);
+
+  // add the characteristic to the service
+  ledService.addCharacteristic(switchChar);
 
-  // add service and characteristic
-  blePeripheral.addAttribute(ledService);
-  blePeripheral.addAttribute(switchChar);
+  // add service
+  BLE.addService(ledService);
 
   // assign event handlers for connected, disconnected to peripheral
-  blePeripheral.setEventHandler(BLEConnected, blePeripheralConnectHandler);
-  blePeripheral.setEventHandler(BLEDisconnected, blePeripheralDisconnectHandler);
+  BLE.setEventHandler(BLEConnected, blePeripheralConnectHandler);
+  BLE.setEventHandler(BLEDisconnected, blePeripheralDisconnectHandler);
 
   // assign event handlers for characteristic
   switchChar.setEventHandler(BLEWritten, switchCharacteristicWritten);
-// set an initial value for the characteristic
+  // set an initial value for the characteristic
   switchChar.setValue(0);
 
-  // advertise the service
-  blePeripheral.begin();
+  // start advertising
+  BLE.advertise();
+
   Serial.println(("Bluetooth device active, waiting for connections..."));
 }
 
 void loop() {
-  // poll peripheral
-  blePeripheral.poll();
+  // poll for BLE events
+  BLE.poll();
 }
 
-void blePeripheralConnectHandler(BLECentral& central) {
+void blePeripheralConnectHandler(BLEDevice central) {
   // central connected event handler
   Serial.print("Connected event, central: ");
   Serial.println(central.address());
 }
 
-void blePeripheralDisconnectHandler(BLECentral& central) {
+void blePeripheralDisconnectHandler(BLEDevice central) {
   // central disconnected event handler
   Serial.print("Disconnected event, central: ");
   Serial.println(central.address());
 }
 
-void switchCharacteristicWritten(BLECentral& central, BLECharacteristic& characteristic) {
+void switchCharacteristicWritten(BLEDevice central, BLECharacteristic characteristic) {
   // central wrote new value to characteristic, update LED
   Serial.print("Characteristic event, written: ");
 
diff --git a/libraries/CurieBLE/examples/LED/LED.ino b/libraries/CurieBLE/examples/Peripheral/LED/LED.ino
similarity index 82%
rename from libraries/CurieBLE/examples/LED/LED.ino
rename to libraries/CurieBLE/examples/Peripheral/LED/LED.ino
index a55501dc..258cb455 100644
--- a/libraries/CurieBLE/examples/LED/LED.ino
+++ b/libraries/CurieBLE/examples/Peripheral/LED/LED.ino
@@ -3,9 +3,17 @@
  * See the bottom of this file for the license terms.
  */
 
+/*
+ * Sketch: led.ino
+ *
+ * Description:
+ *   This is a Peripheral sketch that works with a connected Central.
+ *   It allows the Central to write a value and set/reset the led
+ *   accordingly.
+ */
+
 #include <CurieBLE.h>
 
-BLEPeripheral blePeripheral;  // BLE Peripheral Device (the board you're programming)
 BLEService ledService("19B10000-E8F2-537E-4F6C-D104768A1214"); // BLE LED Service
 
 // BLE LED Switch Characteristic - custom 128-bit UUID, read and writable by central
@@ -19,26 +27,31 @@ void setup() {
   // set LED pin to output mode
   pinMode(ledPin, OUTPUT);
 
+  // begin initialization
+  BLE.begin();
+
   // set advertised local name and service UUID:
-  blePeripheral.setLocalName("LED");
-  blePeripheral.setAdvertisedServiceUuid(ledService.uuid());
+  BLE.setLocalName("LED");
+  BLE.setAdvertisedService(ledService);
+
+  // add the characteristic to the service
+  ledService.addCharacteristic(switchCharacteristic);
 
-  // add service and characteristic:
-  blePeripheral.addAttribute(ledService);
-  blePeripheral.addAttribute(switchCharacteristic);
+  // add service
+  BLE.addService(ledService);
 
   // set the initial value for the characeristic:
   switchCharacteristic.setValue(0);
 
-  // begin advertising BLE service:
-  blePeripheral.begin();
+  // start advertising
+  BLE.advertise();
 
   Serial.println("BLE LED Peripheral");
 }
 
 void loop() {
   // listen for BLE peripherals to connect:
-  BLECentral central = blePeripheral.central();
+  BLEDevice central = BLE.central();
 
   // if a central is connected to peripheral:
   if (central) {
diff --git a/libraries/CurieBLE/examples/MIDIBLE/MIDIBLE.ino b/libraries/CurieBLE/examples/Peripheral/MIDIBLE/MIDIBLE.ino
similarity index 88%
rename from libraries/CurieBLE/examples/MIDIBLE/MIDIBLE.ino
rename to libraries/CurieBLE/examples/Peripheral/MIDIBLE/MIDIBLE.ino
index 2dbe9833..1ae7ebc8 100644
--- a/libraries/CurieBLE/examples/MIDIBLE/MIDIBLE.ino
+++ b/libraries/CurieBLE/examples/Peripheral/MIDIBLE/MIDIBLE.ino
@@ -84,44 +84,46 @@ void noteOff(char chan, char note) //channel 1
   midiData[4] = 0;
 }
 
-BLEPeripheral midiDevice; // create peripheral instance
+//BLEPeripheral midiDevice; // create peripheral instance
 
 BLEService midiSvc("03B80E5A-EDE8-4B33-A751-6CE34EC4C700"); // create service
 
 // create switch characteristic and allow remote device to read and write
-BLECharacteristic midiChar("7772E5DB-3868-4112-A1A9-F2669D106BF3", BLEWrite | BLEWriteWithoutResponse | BLENotify | BLERead, 5);
+BLECharacteristic midiChar("7772E5DB-3868-4112-A1A9-F2669D106BF3",  BLEWrite | BLEWriteWithoutResponse | BLENotify | BLERead,5);
 
 void setup() {
   Serial.begin(9600);
 
   BLESetup();
+    // advertise the service
+  BLE.advertise();
   Serial.println(("Bluetooth device active, waiting for connections..."));
 }
 
 void BLESetup()
 {
+  BLE.begin();
   // set the local name peripheral advertises
-  midiDevice.setLocalName("Auxren");
-  midiDevice.setDeviceName("Auxren");
+  BLE.setLocalName("Auxren");
 
   // set the UUID for the service this peripheral advertises
-  midiDevice.setAdvertisedServiceUuid(midiSvc.uuid());
+  BLE.setAdvertisedServiceUuid(midiSvc.uuid());
 
   // add service and characteristic
-  midiDevice.addAttribute(midiSvc);
-  midiDevice.addAttribute(midiChar);
+  
+  midiSvc.addCharacteristic(midiChar);
+  BLE.addService(midiSvc);
 
   // assign event handlers for connected, disconnected to peripheral
-  midiDevice.setEventHandler(BLEConnected, midiDeviceConnectHandler);
-  midiDevice.setEventHandler(BLEDisconnected, midiDeviceDisconnectHandler);
+  BLE.setEventHandler(BLEConnected, midiDeviceConnectHandler);
+  BLE.setEventHandler(BLEDisconnected, midiDeviceDisconnectHandler);
 
   // assign event handlers for characteristic
   midiChar.setEventHandler(BLEWritten, midiCharacteristicWritten);
   // set an initial value for the characteristic
   midiChar.setValue(midiData, 5);
 
-  // advertise the service
-  midiDevice.begin();
+
 }
 
 void loop() {
@@ -129,6 +131,7 @@ void loop() {
   /*Simple randome note player to test MIDI output
      Plays random note every 400ms
   */
+  BLE.poll();
   int note = random(0, 127);
   //readMIDI();
   noteOn(0, note, 127); //loads up midiData buffer
@@ -140,19 +143,19 @@ void loop() {
 }
 
 
-void midiDeviceConnectHandler(BLECentral& central) {
+void midiDeviceConnectHandler(BLEDevice central) {
   // central connected event handler
   Serial.print("Connected event, central: ");
   Serial.println(central.address());
 }
 
-void midiDeviceDisconnectHandler(BLECentral& central) {
+void midiDeviceDisconnectHandler(BLEDevice central) {
   // central disconnected event handler
   Serial.print("Disconnected event, central: ");
   Serial.println(central.address());
 }
 
-void midiCharacteristicWritten(BLECentral& central, BLECharacteristic& characteristic) {
+void midiCharacteristicWritten(BLEDevice central, BLECharacteristic characteristic) {
   // central wrote new value to characteristic, update LED
   Serial.print("Characteristic event, written: ");
 }
diff --git a/libraries/CurieBLE/keywords.txt b/libraries/CurieBLE/keywords.txt
index 85eccea2..91104ee0 100644
--- a/libraries/CurieBLE/keywords.txt
+++ b/libraries/CurieBLE/keywords.txt
@@ -5,77 +5,141 @@
 #######################################
 # Datatypes (KEYWORD1)
 #######################################
-
-BLEAttribute	KEYWORD1
+CurieBLE	KEYWORD1
+BLEAttributeWithValue	KEYWORD1
+BLEBoolCharacteristic	KEYWORD1
+BLEByteCharacteristic	KEYWORD1
 BLECentral	KEYWORD1
 BLECharacteristic	KEYWORD1
+BLECharCharacteristic	KEYWORD1
 BLEDescriptor	KEYWORD1
+BLEDevice	KEYWORD1
+BLEDoubleCharacteristic	KEYWORD1
+BLEFloatCharacteristic	KEYWORD1
+BLEIntCharacteristic	KEYWORD1
+BLELongCharacteristic	KEYWORD1
 BLEPeripheral	KEYWORD1
 BLEService	KEYWORD1
-BLETypedCharacteristic	KEYWORD1
-BLEUuid	KEYWORD1
-
-BLECharCharacteristic	KEYWORD1
-BLEUnsignedCharCharacteristic	KEYWORD1
 BLEShortCharacteristic	KEYWORD1
-BLEUnsignedShortCharacteristic	KEYWORD1
-BLEIntCharacteristic	KEYWORD1
+BLEUnsignedCharCharacteristic	KEYWORD1
 BLEUnsignedIntCharacteristic	KEYWORD1
-BLELongCharacteristic	KEYWORD1
 BLEUnsignedLongCharacteristic	KEYWORD1
-BLEFloatCharacteristic	KEYWORD1
-BLEDoubleCharacteristic	KEYWORD1
+BLEUnsignedShortCharacteristic	KEYWORD1
 
 #######################################
 # Methods and Functions (KEYWORD2)
 #######################################
+valueSize	KEYWORD2
+value	KEYWORD2
+writeValue	KEYWORD2
+stringValue	KEYWORD2
+charValue	KEYWORD2
+unsignedCharValue	KEYWORD2
+byteValue	KEYWORD2
+shortValue	KEYWORD2
+unsignedShortValue	KEYWORD2
+intValue	KEYWORD2
+unsignedIntValue	KEYWORD2
+longValue	KEYWORD2
+unsignedLongValue	KEYWORD2
+floatValue	KEYWORD2
+doubleValue	KEYWORD2
+writeString	KEYWORD2
+writeString	KEYWORD2
+writeChar	KEYWORD2
+writeUnsignedChar	KEYWORD2
+writeByte	KEYWORD2
+writeShort	KEYWORD2
+writeUnsignedShort	KEYWORD2
+writeInt	KEYWORD2
+writeUnsignedInt	KEYWORD2
+writeLong	KEYWORD2
+writeUnsignedLong	KEYWORD2
+writeFloat	KEYWORD2
+writeDouble	KEYWORD2
+setEventHandler	KEYWORD2
+descriptor	KEYWORD2
+hasDescriptor	KEYWORD2
+descriptorCount	KEYWORD2
+addDescriptor	KEYWORD2
+valueUpdated	KEYWORD2
+unsubscribe	KEYWORD2
+subscribe	KEYWORD2
+write	KEYWORD2
+read	KEYWORD2
 
 uuid	KEYWORD2
-type	KEYWORD2
-numAttributes	KEYWORD2
-
-connected	KEYWORD2
-address	KEYWORD2
-poll	KEYWORD2
-disconnect	KEYWORD2
-
 properties	KEYWORD2
-valueSize	KEYWORD2
-value	KEYWORD2
 valueLength	KEYWORD2
 setValue	KEYWORD2
-setEventHandler	KEYWORD2
+writeValue	KEYWORD2
+broadcast	KEYWORD2
 written	KEYWORD2
 subscribed	KEYWORD2
+canNotify	KEYWORD2
+canIndicate	KEYWORD2
+canRead	KEYWORD2
+canWrite	KEYWORD2
+canSubscribe	KEYWORD2
+canUnsubscribe	KEYWORD2
+
+connected	KEYWORD2
+address	KEYWORD2
+disconnect	KEYWORD2
+poll	KEYWORD2
 
 begin	KEYWORD2
-getAdvertisingLength	KEYWORD2
-getAdvertising	KEYWORD2
+poll	KEYWORD2
+end	KEYWORD2
+disconnect	KEYWORD2
+address	KEYWORD2
 setAdvertisedServiceUuid	KEYWORD2
-setAdvertisedServiceData	KEYWORD2
+setAdvertisedService	KEYWORD2
+setServiceSolicitationUuid	KEYWORD2
+setManufacturerData	KEYWORD2
 setLocalName	KEYWORD2
-setAppearance	KEYWORD2
+setAdvertisingInterval	KEYWORD2
 setConnectionInterval	KEYWORD2
-addAttribute	KEYWORD2
+setTxPower	KEYWORD2
+setConnectable	KEYWORD2
+setDeviceName	KEYWORD2
+addService	KEYWORD2
+addCharacteristic	KEYWORD2
+addDescriptor	KEYWORD2
+advertise	KEYWORD2
+stopAdvertise	KEYWORD2
 central	KEYWORD2
-
-setValueLE	KEYWORD2
-valueLE	KEYWORD2
-setValueBE	KEYWORD2
-valueBE	KEYWORD2
-
-str	KEYWORD2
-data	KEYWORD2
-length	KEYWORD2
-
+peripheral	KEYWORD2
+scan	KEYWORD2
+scanForName	KEYWORD2
+scanForUuid	KEYWORD2
+stopScan	KEYWORD2
+available	KEYWORD2
+hasLocalName	KEYWORD2
+hasAdvertisedServiceUuid	KEYWORD2
+advertisedServiceUuidCount	KEYWORD2
+localName	KEYWORD2
+advertisedServiceUuid	KEYWORD2
+rssi	KEYWORD2
+connect	KEYWORD2
+discoverAttributes	KEYWORD2
+discoverAttributesByService	KEYWORD2
+deviceName	KEYWORD2
+serviceCount	KEYWORD2
+hasService	KEYWORD2
+service	KEYWORD2
+characteristicCount	KEYWORD2
+hasCharacteristic	KEYWORD2
+characteristic	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)
 #######################################
 
-BLETypeService	LITERAL1
-BLETypeCharacteristic	LITERAL1
-BLETypeDescriptor	LITERAL1
+BLEWritten	LITERAL1
+BLESubscribed	LITERAL1
+BLEUnsubscribed	LITERAL1
+BLEValueUpdated	LITERAL1
 
 BLEBroadcast	LITERAL1
 BLERead	LITERAL1
@@ -84,9 +148,13 @@ BLEWrite	LITERAL1
 BLENotify	LITERAL1
 BLEIndicate	LITERAL1
 
+BLEDiscovered	LITERAL1
 BLEConnected	LITERAL1
 BLEDisconnected	LITERAL1
+BLEConParamUpdate	LITERAL1
 
-BLEWritten	LITERAL1
-BLESubscribed	LITERAL1
-BLEUnsubscribed	LITERAL1
+ble_conn_param_t	LITERAL1
+bt_uuid_t	LITERAL1
+bt_uuid_16_t	LITERAL1
+bt_uuid_128_t	LITERAL1
+bt_addr_le_t	LITERAL1
diff --git a/libraries/CurieBLE/library.properties b/libraries/CurieBLE/library.properties
index 4b17054d..c37a6a35 100644
--- a/libraries/CurieBLE/library.properties
+++ b/libraries/CurieBLE/library.properties
@@ -1,7 +1,7 @@
 name=CurieBLE
-version=1.0
-author=Emutex
-maintainer=Emutex <dan@emutex.com>
+version=2.0
+author=Lianggao
+maintainer=Lianggao <liang.gao@intel.com>
 sentence=Library to manage the Bluetooth Low Energy module with Curie Core boards.
 paragraph=Using this library, it is possible to use BLE features to communicate and interact with other devices like smartphones and tablets. This library enables multiple types of functionalities through a number of different classes.
 category=Communication
diff --git a/libraries/CurieBLE/src/BLEAttributeWithValue.cpp b/libraries/CurieBLE/src/BLEAttributeWithValue.cpp
new file mode 100644
index 00000000..88c629f1
--- /dev/null
+++ b/libraries/CurieBLE/src/BLEAttributeWithValue.cpp
@@ -0,0 +1,181 @@
+/*
+  BLE Attribute with value API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+#include "Arduino.h"
+#include "BLEAttributeWithValue.h"
+
+BLEAttributeWithValue::BLEAttributeWithValue()
+{
+
+}
+
+    // intepret the value of the attribute with the specified type
+String BLEAttributeWithValue::stringValue() const
+{
+    const char *retTemp = (const char *)this->value();
+    return retTemp;
+}
+
+char BLEAttributeWithValue::charValue() const
+{
+    char ret = this->operator[](0);
+    return ret;
+}
+
+unsigned char BLEAttributeWithValue::unsignedCharValue() const
+{
+    unsigned char ret = this->operator[](0);
+    return ret;
+}
+
+byte BLEAttributeWithValue::byteValue() const
+{
+    return this->operator[](0);
+}
+
+short BLEAttributeWithValue::shortValue() const
+{
+    short retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+unsigned short BLEAttributeWithValue::unsignedShortValue() const
+{
+    unsigned short retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+int BLEAttributeWithValue::intValue() const
+{
+    int retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+unsigned int BLEAttributeWithValue::unsignedIntValue() const
+{
+    unsigned int retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+long BLEAttributeWithValue::longValue() const
+{
+    long retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+unsigned long BLEAttributeWithValue::unsignedLongValue() const
+{
+    unsigned long retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+float BLEAttributeWithValue::floatValue() const
+{
+    float retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+double BLEAttributeWithValue::doubleValue() const
+{
+    double retTmp = 0;
+    memcpy(&retTmp, this->value(), sizeof(retTmp));
+    return retTmp;
+}
+
+// write the value of the attribute with the specified type
+bool BLEAttributeWithValue::writeString(const String& s)
+{
+    if (s.length() > (unsigned int)this->valueSize())
+    {
+        return false;
+    }
+    return this->writeValue((const byte*)s.c_str(), s.length());
+}
+
+bool BLEAttributeWithValue::writeString(const char* s)
+{
+    if (strlen(s) > (unsigned int)this->valueSize())
+    {
+        return false;
+    }
+    return this->writeValue((const byte*)s, strlen(s));
+}
+
+bool BLEAttributeWithValue::writeChar(char c)
+{
+    return this->writeValue((const byte*)&c, sizeof(c));
+}
+
+bool BLEAttributeWithValue::writeUnsignedChar(unsigned char c)
+{
+    return this->writeValue((const byte*)&c, sizeof(c));
+}
+
+bool BLEAttributeWithValue::writeByte(byte b)
+{
+    return this->writeValue((const byte*)&b, sizeof(b));
+}
+
+bool BLEAttributeWithValue::writeShort(short s)
+{
+    return this->writeValue((const byte*)&s, sizeof(s));
+}
+
+bool BLEAttributeWithValue::writeUnsignedShort(unsigned short s)
+{
+    return this->writeValue((const byte*)&s, sizeof(s));
+}
+
+bool BLEAttributeWithValue::writeInt(int i)
+{
+    return this->writeValue((const byte*)&i, sizeof(i));
+}
+
+bool BLEAttributeWithValue::writeUnsignedInt(unsigned int i)
+{
+    return this->writeValue((const byte*)&i, sizeof(i));
+}
+
+bool BLEAttributeWithValue::writeLong(long l)
+{
+    return this->writeValue((const byte*)&l, sizeof(l));
+}
+
+bool BLEAttributeWithValue::writeUnsignedLong(unsigned int l)
+{
+    return this->writeValue((const byte*)&l, sizeof(l));
+}
+
+bool BLEAttributeWithValue::writeFloat(float f)
+{
+    return this->writeValue((const byte*)&f, sizeof(f));
+}
+
+bool BLEAttributeWithValue::writeDouble(double d)
+{
+    return this->writeValue((const byte*)&d, sizeof(d));
+}
+
+
diff --git a/libraries/CurieBLE/src/BLEAttributeWithValue.h b/libraries/CurieBLE/src/BLEAttributeWithValue.h
new file mode 100644
index 00000000..96971d56
--- /dev/null
+++ b/libraries/CurieBLE/src/BLEAttributeWithValue.h
@@ -0,0 +1,63 @@
+/*
+  BLE Attribute with value API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+#ifndef ARDUINO_BLE_ATTRIBUTE_WITH_VALUE__H
+#define ARDUINO_BLE_ATTRIBUTE_WITH_VALUE__H
+
+class BLEAttributeWithValue
+{
+  public:
+    BLEAttributeWithValue();
+
+    virtual int valueSize() const = 0; // returns the length of the attribute value
+    virtual const byte* value() const = 0; // returns the value of the attribute array
+    virtual byte operator[] (int offset) const = 0; // access an attribute value at the specified offset
+    virtual bool writeValue(const byte value[], int length) = 0;
+
+    // intepret the value of the attribute with the specified type
+    String stringValue() const;
+    char charValue() const;
+    unsigned char unsignedCharValue() const;
+    byte byteValue() const;
+    short shortValue() const;
+    unsigned short unsignedShortValue() const;
+    int intValue() const;
+    unsigned int unsignedIntValue() const;
+    long longValue() const;
+    unsigned long unsignedLongValue() const;
+    float floatValue() const;
+    double doubleValue() const;
+
+    // write the value of the attribute with the specified type
+    bool writeString(const String& s);
+    bool writeString(const char* s);
+    bool writeChar(char c);
+    bool writeUnsignedChar(unsigned char c);
+    bool writeByte(byte b);
+    bool writeShort(short s);
+    bool writeUnsignedShort(unsigned short s);
+    bool writeInt(int i);
+    bool writeUnsignedInt(unsigned int i);
+    bool writeLong(long l);
+    bool writeUnsignedLong(unsigned int l);
+    bool writeFloat(float f);
+    bool writeDouble(double d);
+};
+
+#endif
diff --git a/libraries/CurieBLE/src/BLECentral.cpp b/libraries/CurieBLE/src/BLECentral.cpp
index 73d0326d..862dc272 100644
--- a/libraries/CurieBLE/src/BLECentral.cpp
+++ b/libraries/CurieBLE/src/BLECentral.cpp
@@ -1,103 +1,61 @@
-/*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-#include "BLECentral.h"
-
-#include "BLEPeripheral.h"
-
-
-BLECentral::BLECentral(BLEPeripheral* peripheral) :
-  _peripheral(peripheral)
-{
-    clearAddress();
-}
-
-BLECentral::operator bool() const {
-    ble_addr_t zero;
-
-    memset(&zero, 0, sizeof(zero));
-
-    return (memcmp(&_address, &zero, sizeof(_address)) != 0);
-}
-
-bool
-BLECentral::operator==(const BLECentral& rhs) const {
-    return (memcmp(&_address, &rhs._address, sizeof(_address)) == 0);
-}
-
-bool
-BLECentral::operator!=(const BLECentral& rhs) const {
-    return !(*this == rhs);
-}
-
-bool
-BLECentral::connected() {
-    poll();
-
-    return (*this && *this == _peripheral->central());
-}
-
-const char* 
-BLECentral::address() const {
-    static char address[18];
-
-    String addressStr = "";
-
-    for (int i = 5; i >= 0; i--) {
-        unsigned char a = _address.addr[i];
-
-        if (a < 0x10) {
-            addressStr += "0";
-        }
-
-        addressStr += String(a, 16);
-
-        if (i > 0) {
-            addressStr += ":";
-        }
-    }
-
-    strcpy(address, addressStr.c_str());
-
-    return address;
-}
-
-void
-BLECentral::poll() {
-    _peripheral->poll();
-}
-
-bool
-BLECentral::disconnect() {
-    if (connected()) {
-        return _peripheral->disconnect();
-    }
-
-    return false;
-}
-
-void
-BLECentral::setAddress(ble_addr_t address) {
-    _address = address;
-}
-
-void
-BLECentral::clearAddress() {
-    memset(&_address, 0x00, sizeof(_address));
-}
+/*
+  BLE Central API (deprecated)
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+#include "CurieBLE.h"
+
+BLECentral::BLECentral(BLEDevice& device) :
+  _device(&device)
+{
+
+}
+
+bool BLECentral::connected(void)
+{
+    return _device.connected();
+}
+
+const char* BLECentral::address(void) const
+{
+    return _device.address().c_str();
+}
+
+bool BLECentral::disconnect(void)
+{
+    return _device.disconnect();
+}
+
+void BLECentral::poll(void)
+{
+  _device.poll();
+}
+
+BLECentral::operator bool(void) const
+{
+  return _device;
+}
+
+bool BLECentral::operator==(const BLECentral& rhs) const
+{
+  return (_device == rhs._device);
+}
+
+bool BLECentral::operator!=(const BLECentral& rhs) const
+{
+  return (_device != rhs._device);
+}
diff --git a/libraries/CurieBLE/src/BLECentral.h b/libraries/CurieBLE/src/BLECentral.h
index c51250d9..47d98f77 100644
--- a/libraries/CurieBLE/src/BLECentral.h
+++ b/libraries/CurieBLE/src/BLECentral.h
@@ -1,70 +1,46 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-#ifndef _BLE_CENTRAL_H_INCLUDED
-#define _BLE_CENTRAL_H_INCLUDED
-
-#include "BLECommon.h"
-
-class BLEPeripheral;
+  BLE Central API (deprecated)
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+// The API in this file is in DEPRECATED MODE, please DO NOT use it for Sketch construction
+#ifndef ARDUINO_CENTRAL_H
+#define ARDUINO_CENTRAL_H
 
 class BLECentral {
+  public:
+    bool connected(void); // is the central connected
+    
+    const char* address(void) const; // address of the Central in string form
+
+    bool disconnect(void); // Disconnect the central if it is connected
+    void poll(void); // Poll the central for events
+
+    operator bool(void) const;
+    bool operator==(const BLECentral& rhs) const;
+    bool operator!=(const BLECentral& rhs) const;
+protected:
+    friend class BLECharacteristicImp;
+    friend void bleBackCompatiblePeripheralConnectHandler(BLEDevice central);
+    friend void bleBackCompatiblePeripheralDisconnectHandler(BLEDevice central);
     friend class BLEPeripheral;
+    BLECentral(BLEDevice& device);
+  private:
 
-    public:
-        /**
-         * Is the Central connected
-         *
-         * @return boolean_t true if the central is connected, otherwise false
-         */
-        bool connected(void);
-
-        /**
-         * Get the address of the Central in string form
-         *
-         * @return const char* address of the Central in string form
-         */
-        const char* address(void) const;
-        
-        /**
-         * Disconnect the central if it is connected
-         *
-         */
-        bool disconnect(void);
-
-        /**
-         * Poll the central for events
-         */
-        void poll(void);
-
-        operator bool(void) const;
-        bool operator==(const BLECentral& rhs) const;
-        bool operator!=(const BLECentral& rhs) const;
-
-    protected:
-        BLECentral(BLEPeripheral* peripheral);
-        void setAddress(ble_addr_t address);
-        void clearAddress();
-
-    private:
-        BLEPeripheral* _peripheral;
-        ble_addr_t     _address;
+    BLEDevice _device;
 };
 
 #endif
diff --git a/libraries/CurieBLE/src/BLECharacteristic.cpp b/libraries/CurieBLE/src/BLECharacteristic.cpp
index a6c46c07..1416cef3 100644
--- a/libraries/CurieBLE/src/BLECharacteristic.cpp
+++ b/libraries/CurieBLE/src/BLECharacteristic.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
@@ -17,249 +17,642 @@
  *
  */
 
+#include <errno.h>
+
+#include "CurieBLE.h"
+
+#include "./internal/BLEUtils.h"
+
 #include "BLECharacteristic.h"
-#include "internal/ble_client.h"
-
-#define BLE_CCCD_NOTIFY_EN_MASK   0x1
-#define BLE_CCCD_INDICATE_EN_MASK 0x2
-
-BLECharacteristic::BLECharacteristic(const char* uuid,
-                      const unsigned char properties,
-                      const unsigned short maxLength) :
-    BLEAttribute(uuid, BLETypeCharacteristic),
-    _properties(properties),
-    _value_length(0),
-    _written(false),
-    _cccd_value(0),
-    _value_handle(0),
-    _cccd_handle(0),
-    _user_description(NULL),
-    _presentation_format(NULL)
-{
-    _value_size = maxLength > BLE_MAX_ATTR_DATA_LEN ? BLE_MAX_ATTR_DATA_LEN : maxLength;
-    _value = (unsigned char*)malloc(_value_size);
+#include "./internal/BLEProfileManager.h"
+#include "./internal/BLEDeviceManager.h"
 
+#include "./internal/BLECharacteristicImp.h"
+
+BLECharacteristic::BLECharacteristic():
+    _bledev(), _internal(NULL), _chrc_local_imp(NULL), _broadcast(false),
+    _properties(0), _value_size(0), _value(NULL)//,
+    //_event_handlers(NULL)
+{
+    memset(_uuid_cstr, 0, sizeof(_uuid_cstr));
+    memset(_event_handlers, 0, sizeof(_event_handlers));
+    memset(_oldevent_handlers, 0, sizeof(_oldevent_handlers));
+}
+
+BLECharacteristic::BLECharacteristic(const char* uuid, 
+                                     unsigned char properties, 
+                                     unsigned short valueSize):
+    _bledev(), _internal(NULL), _chrc_local_imp(NULL), _broadcast(false),
+    _properties(properties), 
+    _value(NULL)//,
+    //_event_handlers(NULL)
+{
+    bt_uuid_128 bt_uuid_tmp;
+    _value_size = valueSize > BLE_MAX_ATTR_LONGDATA_LEN ? BLE_MAX_ATTR_LONGDATA_LEN : valueSize;
+    BLEUtils::uuidString2BT(uuid, (bt_uuid_t *)&bt_uuid_tmp);
+    BLEUtils::uuidBT2String((const bt_uuid_t *)&bt_uuid_tmp, _uuid_cstr);
+    _bledev.setAddress(*BLEUtils::bleGetLoalAddress());
     memset(_event_handlers, 0, sizeof(_event_handlers));
+    memset(_oldevent_handlers, 0, sizeof(_oldevent_handlers));
 }
 
-BLECharacteristic::BLECharacteristic(const char* uuid,
-                      const unsigned char properties,
-                      const char* value) :
+BLECharacteristic::BLECharacteristic(const char* uuid, 
+                                     unsigned char properties, 
+                                     const char* value):
     BLECharacteristic(uuid, properties, strlen(value))
 {
-    setValue((const uint8_t*)value, strlen(value));
+    _setValue((const uint8_t*)value, strlen(value));
+}
+
+BLECharacteristic::BLECharacteristic(BLECharacteristicImp *characteristicImp,
+                                     const BLEDevice *bleDev):
+    _bledev(bleDev), _internal(characteristicImp),  _chrc_local_imp(NULL), 
+    _broadcast(false), _value(NULL)//,_event_handlers(NULL)
+{
+    BLEUtils::uuidBT2String(characteristicImp->bt_uuid(), _uuid_cstr);
+    _properties = characteristicImp->properties();
+    _value_size = characteristicImp->valueSize();
+    memset(_event_handlers, 0, sizeof(_event_handlers));
+    memset(_oldevent_handlers, 0, sizeof(_oldevent_handlers));
+}
+
+BLECharacteristic::BLECharacteristic(const BLECharacteristic& rhs):
+    _value(NULL)//,
+    //_event_handlers(NULL)
+{
+    _chrc_local_imp = NULL; // Not copy
+    _value_size = rhs._value_size;
+    _internal = rhs._internal;
+    _bledev.setAddress(*rhs._bledev.bt_le_address());
+    memcpy(_uuid_cstr, rhs._uuid_cstr, sizeof(_uuid_cstr));
+    _properties = rhs._properties;
+    
+    if (rhs._internal == NULL)
+    {
+        if (rhs._value != NULL)
+        {
+            _value = (unsigned char*)malloc(rhs._value_size);
+            if (NULL != _value)
+            {
+                memcpy(_value, rhs._value, rhs._value_size);
+            }
+            else
+            {
+                errno = ENOMEM;
+            }
+            
+        }
+        
+        //if (rhs._event_handlers != NULL)
+        {
+            //_event_handlers = (BLECharacteristicEventHandler*)malloc(sizeof(BLECharacteristicEventHandler) * BLECharacteristicEventLast);
+            
+            //if (NULL != _event_handlers)
+                memcpy(_event_handlers, rhs._event_handlers, (sizeof(BLECharacteristicEventHandler) * BLECharacteristicEventLast));
+        }
+        memcpy(_oldevent_handlers, rhs._oldevent_handlers, (sizeof(BLECharacteristicEventHandler) * BLECharacteristicEventLast));
+    }
 }
 
 BLECharacteristic::~BLECharacteristic()
 {
-    if (_value) {
+    if (_value) 
+    {
         free(_value);
         _value = NULL;
     }
+    
+    if (_chrc_local_imp != NULL)
+    {
+        delete  _chrc_local_imp;
+        _chrc_local_imp = NULL;
+    }
 }
 
-unsigned char
-BLECharacteristic::properties() const
+const char* BLECharacteristic::uuid() const
 {
-    return _properties;
+    return _uuid_cstr;
 }
 
-bool
-BLECharacteristic::setValue(const unsigned char value[], uint16_t length)
+unsigned char BLECharacteristic::properties() const
 {
-    BleStatus status;
+    unsigned char property = 0;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    if (NULL != characteristicImp)
+    {
+        property = characteristicImp->properties();
+    }
+    return property;
+}
 
-     _setValue(value, length);
+int BLECharacteristic::valueSize() const
+{
+    int valuesize = 0;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    if (NULL != characteristicImp)
+    {
+        valuesize = characteristicImp->valueSize();
+    }
+    return valuesize;
+}
 
-    if (_value_handle) {
-        status = ble_client_gatts_set_attribute_value(_value_handle, _value_length, _value, 0);
-        if (BLE_STATUS_SUCCESS != status) {
-            return false;
-        }
+const byte* BLECharacteristic::value() const
+{
+    const byte* value_temp = NULL;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    if (NULL != characteristicImp)
+    {
+        value_temp = characteristicImp->value();
+    }
+    return value_temp;
+}
 
-        if (subscribed()) {
-            boolean_t indication = (_cccd_value & BLE_CCCD_INDICATE_EN_MASK);
+int BLECharacteristic::valueLength() const
+{
+    int valueLength = 0;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    if (NULL != characteristicImp)
+    {
+        valueLength = characteristicImp->valueLength();
+    }
+    return valueLength;
+}
 
-            status = ble_client_gatts_send_notif_ind(_value_handle, _value_length, _value, 0, indication);
-            if (BLE_STATUS_SUCCESS != status) {
-                return false;
+BLECharacteristic::operator bool() const
+{
+    return (strlen(_uuid_cstr) > 3);
+}
+
+BLECharacteristic& BLECharacteristic::operator= (const BLECharacteristic& chrc)
+{
+    if (this != &chrc)
+    {
+        memcpy(_uuid_cstr, chrc._uuid_cstr, sizeof(_uuid_cstr));
+        _bledev.setAddress(*chrc._bledev.bt_le_address());
+        _internal = chrc._internal;
+        _chrc_local_imp = NULL; // Not copy
+        _properties = chrc._properties;
+        
+        if (_value_size < chrc._value_size)
+        {
+            _value_size = chrc._value_size;
+            if (NULL != _value)
+            {
+                free(_value);
+                _value = NULL;
             }
         }
+        
+        if (_internal == NULL)
+        {
+            if (chrc._value != NULL)
+            {
+                if (NULL == _value)
+                    _value = (unsigned char*) malloc(_value_size);
+                
+                if (NULL != _value)
+                    memcpy(_value, chrc._value, chrc._value_size);
+                else {
+	                _value_size = 0;
+	            }
+            }
+            
+            //if (chrc._event_handlers != NULL)
+            {
+                //if (NULL == _event_handlers)
+                //    _event_handlers = (BLECharacteristicEventHandler*)malloc(sizeof(BLECharacteristicEventHandler) * BLECharacteristicEventLast);
+                
+                //if (NULL != _event_handlers)
+                    memcpy(_event_handlers, chrc._event_handlers, (sizeof(BLECharacteristicEventHandler) * BLECharacteristicEventLast));
+            }
+            memcpy(_oldevent_handlers, chrc._oldevent_handlers, (sizeof(BLECharacteristicEventHandler) * BLECharacteristicEventLast));
+        }
     }
-
-    return true;
+    return *this;
 }
 
-void
-BLECharacteristic::setValue(BLECentral& central, const unsigned char* value, unsigned short length)
+byte BLECharacteristic::operator[] (int offset) const
 {
-    _setValue(value, length);
-
-    _written = true;
-
-    if (_event_handlers[BLEWritten]) {
-        _event_handlers[BLEWritten](central, *this);
+    byte data = 0;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        data = (*characteristicImp)[offset];
     }
+    return data;
 }
 
-unsigned short
-BLECharacteristic::valueSize() const
+bool BLECharacteristic::setValue(const unsigned char value[], unsigned short length)
 {
-    return _value_size;
+    return writeValue(value, (int)length);
 }
 
-const unsigned char*
-BLECharacteristic::value() const
+bool BLECharacteristic::setValue(const char* value)
 {
-    return _value;
+    return this->setValue((const unsigned char *)value, strlen(value));
 }
 
-unsigned short
-BLECharacteristic::valueLength() const
+bool BLECharacteristic::writeValue(const byte value[], int length)
 {
-    return _value_length;
+    return writeValue(value, length, 0);
 }
 
-unsigned char
-BLECharacteristic::operator[] (int offset) const
+bool BLECharacteristic::writeValue(const byte value[], int length, int offset)
 {
-    return _value[offset];
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        if (BLEUtils::isLocalBLE(_bledev) == true)
+        {
+            retVar = characteristicImp->writeValue(value, length, offset);
+        }
+        else
+        {
+            retVar = characteristicImp->write(value, (uint16_t)length);
+        }
+        
+        if (true == _broadcast && 
+            true == BLEDeviceManager::instance()->advertising())
+        {
+            BLEDeviceManager::instance()->stopAdvertising();
+            BLEDeviceManager::instance()->setAdvertisedServiceData(characteristicImp->bt_uuid(),
+                                                                   characteristicImp->value(), 
+                                                                   characteristicImp->valueLength());
+            BLEDeviceManager::instance()->startAdvertising();
+        }
+    } else {
+        // not associated with a service yet
+        _setValue(value, length);
+        retVar = true;
+    }
+    return retVar;
 }
 
-bool
-BLECharacteristic::written()
+bool BLECharacteristic::writeValue(const char* value)
 {
-    boolean_t written = _written;
-
-    _written = false;
+    return writeValue((const byte*)value, strlen(value));
+}
 
-    return written;
+bool BLECharacteristic::broadcast()
+{
+    _broadcast = true;
+    BLEDeviceManager::instance()->setConnectable(false);
+    if (BLEDeviceManager::instance()->advertising())
+    {
+        BLEDeviceManager::instance()->stopAdvertising();
+    }
+    BLEDeviceManager::instance()->startAdvertising();
+    return _broadcast;
 }
 
-bool
-BLECharacteristic::subscribed()
+bool BLECharacteristic::written()
 {
-    return (_cccd_value & (BLE_CCCD_NOTIFY_EN_MASK | BLE_CCCD_INDICATE_EN_MASK));
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->written();
+    }
+    return retVar;
 }
 
-void
-BLECharacteristic::setEventHandler(BLECharacteristicEvent event, BLECharacteristicEventHandler callback)
+bool BLECharacteristic::subscribed()
 {
-    noInterrupts();
-    if (event < sizeof(_event_handlers)) {
-        _event_handlers[event] = callback;
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->subscribed();
     }
-    interrupts();
+    return retVar;
 }
 
-bool
-BLECharacteristic::add(uint16_t serviceHandle)
+bool BLECharacteristic::canNotify()
 {
-    bt_uuid uuid = btUuid();
+    return (_properties & BLENotify);
+}
 
-    struct ble_gatts_characteristic char_data;
-    struct ble_gatts_char_handles handles;
-    struct ble_gatt_char_user_desc user_desc;
-    struct ble_gatt_pf_desc pf_desc;
+bool BLECharacteristic::canIndicate()
+{
+    return (_properties & BLEIndicate);
+}
 
-    memset(&char_data, 0, sizeof(char_data));
+bool BLECharacteristic::canRead()
+{
+    return (_properties & BLERead);
+}
 
-    char_data.p_uuid = &uuid;
-    char_data.props.props = _properties;
+bool BLECharacteristic::canWrite()
+{
+    return (_properties & BLEWrite);
+}
 
-    if (_properties & (BLERead | BLENotify | BLEIndicate)) {
-        char_data.perms.rd = GAP_SEC_MODE_1 | GAP_SEC_LEVEL_1;
-    } else {
-        char_data.perms.rd = GAP_SEC_NO_PERMISSION;
+bool BLECharacteristic::canSubscribe()
+{
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    if (_properties & (BLENotify | BLEIndicate) && 
+        (NULL != characteristicImp))
+    {
+        retVar = !characteristicImp->subscribed();
     }
+    return retVar;
+}
 
-    if (_properties & (BLEWriteWithoutResponse | BLEWrite)) {
-        char_data.perms.wr = GAP_SEC_MODE_1 | GAP_SEC_LEVEL_1;
-    } else {
-        char_data.perms.wr = GAP_SEC_NO_PERMISSION;
+bool BLECharacteristic::canUnsubscribe()
+{
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->subscribed();
     }
+    return retVar;
+}
 
-    char_data.init_len = _value_length;
-    char_data.max_len = _value_size;
-    char_data.p_value = _value;
-
-    if (_user_description) {
-        user_desc.buffer = (uint8_t*)_user_description->value();
-        user_desc.len = _user_description->valueLength();
+bool BLECharacteristic::read(bool blocked)
+{
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->read(blocked);
+    }
+    return retVar;
+}
 
-        char_data.p_user_desc = &user_desc;
+bool BLECharacteristic::write(const unsigned char* value, int length)
+{
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->write(value, (uint16_t)length);
     }
+    return retVar;
+}
 
-    if (_presentation_format) {
-        const uint8_t* pfValue = _presentation_format->value();
+bool BLECharacteristic::subscribe()
+{
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->subscribe();
+    }
+    return retVar;
+}
 
-        pf_desc.format = pfValue[0];
-        pf_desc.exp = pfValue[1];
-        pf_desc.unit = (pfValue[3] << 8) | pfValue[2];
-        pf_desc.name_spc = pfValue[4];
-        pf_desc.descr = (pfValue[6] << 8) | pfValue[5];
+bool BLECharacteristic::unsubscribe()
+{
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->unsubscribe();
+    }
+    return retVar;
+}
 
-        char_data.p_char_pf_desc = &pf_desc;
+bool BLECharacteristic::valueUpdated()
+{
+    bool retVar = false;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->valueUpdated();
     }
+    return retVar;
+}
 
-    BleStatus status = ble_client_gatts_add_characteristic(serviceHandle, &char_data, &handles);
-    if (BLE_STATUS_SUCCESS == status) {
-        _value_handle = handles.value_handle;
-        _cccd_handle = handles.cccd_handle;
+int BLECharacteristic::addDescriptor(BLEDescriptor& descriptor)
+{
+    int retVar = BLE_STATUS_ERROR;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        retVar = characteristicImp->addDescriptor(descriptor);
     }
+    else if (BLEUtils::isLocalBLE(_bledev) == true)
+    {
+        // Only support the GATT server that create the service in local device.
+        //  Consider to add multi-descriptor
+        if (NULL == _chrc_local_imp)
+        {
+            _chrc_local_imp = new BLECharacteristicImp(*this, _bledev);
+        }
+        
+        if (NULL != _chrc_local_imp)
+        {
+            retVar = _chrc_local_imp->addDescriptor(descriptor);
+        }
+        else
+        {
+            retVar = BLE_STATUS_NO_MEMORY;
+        }
+    }
+    return retVar;
+}
 
-    return (BLE_STATUS_SUCCESS == status);
+BLECharacteristicImp* BLECharacteristic::fetchCharacteristicImp()
+{
+    BLECharacteristicImp* temp = _chrc_local_imp;
+    _chrc_local_imp = NULL;
+    return temp;
 }
 
-uint16_t
-BLECharacteristic::valueHandle()
+int BLECharacteristic::descriptorCount() const
 {
-    return _value_handle;
+    int count = 0;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        count = characteristicImp->descriptorCount();
+    }
+    return count;
 }
 
-uint16_t
-BLECharacteristic::cccdHandle()
+bool BLECharacteristic::hasDescriptor(const char* uuid) const
 {
-    return _cccd_handle;
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        descriptorImp = characteristicImp->descrptor(uuid);
+    }
+    
+    return (descriptorImp != NULL);
 }
 
-void
-BLECharacteristic::setCccdValue(BLECentral& central, uint16_t value)
+bool BLECharacteristic::hasDescriptor(const char* uuid, int index) const
 {
-    if (_cccd_value != value) {
-        _cccd_value = value;
+    bool retVal = false;
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        descriptorImp = characteristicImp->descrptor(index);
+        if (NULL != descriptorImp)
+        {
+            retVal = descriptorImp->compareUuid(uuid);
+        }
+    }
+    
+    return retVal;
+}
 
-        if (subscribed()) {
-            if (_event_handlers[BLESubscribed]) {
-                _event_handlers[BLESubscribed](central, *this);
-            }
-        } else {
-            if (_event_handlers[BLEUnsubscribed]) {
-                _event_handlers[BLEUnsubscribed](central, *this);
-            }
+BLEDescriptor BLECharacteristic::descriptor(int index) const
+{
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        descriptorImp = characteristicImp->descrptor(index);
+    }
+    
+    if (descriptorImp != NULL)
+    {
+        return BLEDescriptor(descriptorImp, &_bledev);
+    }
+    else
+    {
+        return BLEDescriptor();
+    }
+}
+BLEDescriptor BLECharacteristic::descriptor(const char * uuid) const
+{
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        descriptorImp = characteristicImp->descrptor(uuid);
+    }
+    
+    if (descriptorImp != NULL)
+    {
+        return BLEDescriptor(descriptorImp, &_bledev);
+    }
+    else
+    {
+        return BLEDescriptor();
+    }
+}
+
+BLEDescriptor BLECharacteristic::descriptor(const char * uuid, int index) const
+{
+    bool retVal = false;
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    
+    if (NULL != characteristicImp)
+    {
+        descriptorImp = characteristicImp->descrptor(index);
+        if (NULL != descriptorImp)
+        {
+            retVal = descriptorImp->compareUuid(uuid);
         }
     }
+    
+    if (descriptorImp != NULL && true == retVal)
+    {
+        return BLEDescriptor(descriptorImp, &_bledev);
+    }
+    else
+    {
+        return BLEDescriptor();
+    }
 }
 
-void
-BLECharacteristic::setUserDescription(BLEDescriptor *descriptor)
+void BLECharacteristic::setEventHandler(BLECharacteristicEvent event, 
+                                        BLECharacteristicEventHandler eventHandler)
 {
-    _user_description = descriptor;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    if (event >= BLECharacteristicEventLast)
+    {
+        return;
+    }
+    
+    if (NULL != characteristicImp)
+    {
+        characteristicImp->setEventHandler(event, eventHandler);
+    }
+    else
+    {
+        _event_handlers[event] = eventHandler;
+    }
 }
 
-void
-BLECharacteristic::setPresentationFormat(BLEDescriptor *descriptor)
+void BLECharacteristic::setEventHandler(BLECharacteristicEvent event, 
+                                        BLECharacteristicEventHandlerOld eventHandler)
 {
-    _presentation_format = descriptor;
+    BLECharacteristicImp *characteristicImp = getImplementation();
+    if (event >= BLECharacteristicEventLast)
+    {
+        return;
+    }
+    
+    if (NULL != characteristicImp)
+    {
+        characteristicImp->setEventHandler(event, eventHandler);
+    }
+    else
+    {
+        _oldevent_handlers[event] = eventHandler;
+    }
 }
 
+
 void
 BLECharacteristic::_setValue(const uint8_t value[], uint16_t length)
 {
     if (length > _value_size) {
         length = _value_size;
     }
-
+    
+    if (NULL == _value)
+    {
+        // Allocate the buffer for characteristic
+        _value = (unsigned char*)malloc(_value_size);
+    }
+    if (NULL == _value)
+    {
+        errno = ENOMEM;
+        return;
+    }
     memcpy(_value, value, length);
-    _value_length = length;
 }
+
+BLECharacteristicImp* BLECharacteristic::getImplementation() const
+{
+    BLECharacteristicImp* tmp = NULL;
+    tmp = _internal;
+    if (NULL == tmp)
+    {
+        tmp = BLEProfileManager::instance()->characteristic(_bledev, (const char*)_uuid_cstr);
+    }
+    return tmp;
+}
+
+void BLECharacteristic::setBLECharacteristicImp(BLECharacteristicImp *characteristicImp)
+{
+    _internal = characteristicImp;
+}
+
+
diff --git a/libraries/CurieBLE/src/BLECharacteristic.h b/libraries/CurieBLE/src/BLECharacteristic.h
index a5afaa36..79fbfed3 100644
--- a/libraries/CurieBLE/src/BLECharacteristic.h
+++ b/libraries/CurieBLE/src/BLECharacteristic.h
@@ -1,184 +1,547 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-#ifndef _BLE_CHARACTERISTIC_H_INCLUDED
-#define _BLE_CHARACTERISTIC_H_INCLUDED
-
-#include "BLEAttribute.h"
-#include "BLECentral.h"
-#include "BLEDescriptor.h"
-
-/**
- * BLE Characteristic Events
- */
-enum BLECharacteristicEvent {
-    BLEWritten = 0,
-    BLESubscribed = 1,
-    BLEUnsubscribed = 2,
+  BLE Characteristic API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
 
-    BLECharacteristicEventLast = 3
-};
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
 
-/* Forward declaration needed for callback function prototype below */
-class BLECharacteristic;
-class BLEPeripheral;
+#ifndef ARDUINO_BLE_CHARACTERISTIC_H
+#define ARDUINO_BLE_CHARACTERISTIC_H
 
-/** Function prototype for BLE Characteristic event callback */
-typedef void (*BLECharacteristicEventHandler)(BLECentral &central, BLECharacteristic &characteristic);
+#include "CurieBLE.h"
+
+#include "BLEDevice.h"
+
+enum BLECharacteristicEvent {
+  BLEWritten = 0,
+  BLESubscribed = 1,
+  BLEUnsubscribed = 2,
+  BLEValueUpdated = 3,
+  BLECharacteristicEventLast
+};
 
-/**
- * BLE Characteristic Property types
- */
 enum BLEProperty {
-    // broadcast (0x01) not supported
-    BLERead                 = 0x02,
-    BLEWriteWithoutResponse = 0x04,
-    BLEWrite                = 0x08,
-    BLENotify               = 0x10,
-    BLEIndicate             = 0x20
+  BLEBroadcast            = 0x01,
+  BLERead                 = 0x02,
+  BLEWriteWithoutResponse = 0x04,
+  BLEWrite                = 0x08,
+  BLENotify               = 0x10,
+  BLEIndicate             = 0x20
 };
 
-/**
- * BLE GATT Characteristic
- */
-class BLECharacteristic : public BLEAttribute {
+typedef void (*BLECharacteristicEventHandler)(BLEDevice bledev, BLECharacteristic characteristic);
+
+typedef void (*BLECharacteristicEventHandlerOld)(BLECentral &central, BLECharacteristic &characteristic);
+
+//#include "BLECharacteristicImp.h"
+
+class BLECharacteristic: public BLEAttributeWithValue
+{
 public:
+    BLECharacteristic();
     /**
-     * Constructor for BLE Characteristic
+     * @brief   Create a characteristic with specified value size
+     *
+     * @param   uuid        The UUID of the characteristic
+     *
+     * @param   properties  The properties of the characteristic
+     *
+     * @param   valueSize   The size of the characteristic data
+     *
+     * @return  none
      *
-     * @param uuid         16-bit or 128-bit UUID (in string form) defined by BLE standard
-     * @param properties   Characteristic property mask
-     * @param maxLength    Maximum data length required for characteristic value (<= BLE_MAX_ATTR_DATA_LEN)
+     * @note  none
      */
-    BLECharacteristic(const char* uuid,
-                      const unsigned char properties,
-                      const unsigned short maxLength);
-
+    BLECharacteristic(const char* uuid, 
+                      unsigned char properties, 
+                      unsigned short valueSize);
+    
     /**
-     * Constructor for BLE Characteristic
+     * @brief   Create a characteristic with string value
+     *
+     * @param   uuid        The UUID of the characteristic
+     *
+     * @param   properties  The properties of the characteristic
      *
-     * @param uuid         16-bit or 128-bit UUID (in string form) defined by BLE standard
-     * @param properties   Characteristic property mask
-     * @param value        String value for characteristic (string length (<= BLE_MAX_ATTR_DATA_LEN))
+     * @param   value       The string of the characteristic data
+     *
+     * @return  none
+     *
+     * @note  The data length is string's size. Can't set a string that is longger
+     *          than the this input
      */
-    BLECharacteristic(const char* uuid,
-                      const unsigned char properties,
+    BLECharacteristic(const char* uuid, 
+                      unsigned char properties, 
                       const char* value);
 
+    BLECharacteristic(const BLECharacteristic&);
+
     virtual ~BLECharacteristic();
 
+    /**
+     * @brief   Is the characteristic valid
+     *
+     * @param   none
+     *
+     * @return  bool    true/false
+     *
+     * @note  Invalid characteristic is NULL pointer or all zero with UUID
+     */
+    virtual operator bool() const; // 
+
+    /**
+     * @brief   Get the characteristic's UUID string
+     *
+     * @param   none
+     *
+     * @return  const char*     The UUID string
+     *
+     * @note  none
+     */
+    const char* uuid() const;
+
+    /**
+     * @brief   Get the property mask of the characteristic
+     *
+     * @param   none
+     *
+     * @return  unsigned char       The property mask of the characteristic
+     *
+     * @note  none
+     */
+    unsigned char properties() const;
+
+    /**
+     * @brief   Get the maximum size of the value
+     *
+     * @param   none
+     *
+     * @return  int     The maximum size of the value
+     *
+     * @note  none
+     */
+    int valueSize() const;
+    
+    /**
+     * @brief   Get the value buffer
+     *
+     * @param   none
+     *
+     * @return  const byte*     The value buffer
+     *
+     * @note  none
+     */
+    virtual const byte* value() const;
+    
+    /**
+     * @brief   Get the current length of the value
+     *
+     * @param   none
+     *
+     * @return  int     The current length of the value string
+     *
+     * @note  TODO: How to handle if the data is RAW data? This API is danger
+     */
+    virtual int valueLength() const;
+    
+    /**
+     * @brief   Get a byte of the value at the specified offset
+     *
+     * @param   none
+     *
+     * @return  byte    A byte of the value at the specified offset
+     *
+     * @note  none
+     */
+    virtual byte operator[] (int offset) const;
+    BLECharacteristic& operator= (const BLECharacteristic& chrc);
+
     /**
      * Set the current value of the Characteristic
      *
-     * @param value  New value to set, as a byte array.  Data is stored in internal copy.
-     * @param length Length, in bytes, of valid data in the array to write.
+     * @param[in] value  New value to set, as a byte array.  Data is stored in internal copy.
+     * @param[in] length Length, in bytes, of valid data in the array to write.
      *               Must not exceed maxLength set for this characteristic.
      *
      * @return bool true set value success, false on error
+     * @note    GATT Server only
      */
     bool setValue(const unsigned char value[], unsigned short length);
 
     /**
-     * Get the property mask of the Characteristic
+     * Set the current value of the Characteristic with a String
+     *
+     * @param value New string value to set, strings exceeding maxLength will be truncated
      *
-     * @return unsigned char property mask of the Characteristic
+     * @return bool true set value success, false on error
      */
-    unsigned char properties(void) const;
+     bool setValue(const char* value);
 
     /**
-     * Get the (maximum) size of the Characteristic
+     * @brief   Write the value of the characteristic
+     *
+     * @param   value   The value buffer that want to write to characteristic
+     *
+     * @param   length  The value buffer's length
+     *
+     * @return  bool    true - Success, false - Failed
      *
-     * @return unsigned size of characateristic in bytes
+     * @note  none
      */
-    unsigned short valueSize(void) const;
+    virtual bool writeValue(const byte value[], int length);
+    
+    /**
+     * @brief   Write the value of the characteristic
+     *
+     * @param   value   The value buffer that want to write to characteristic
+     *
+     * @param   length  The value buffer's length
+     *
+     * @param   offset  The offset in the characteristic's data
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  none
+     */
+    bool writeValue(const byte value[], int length, int offset);
+    
+    /**
+     * @brief   Write the value of the characteristic
+     *
+     * @param   value   The value string that want to write to characteristic
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  none
+     */
+    bool writeValue(const char* value);
 
+    // peripheral mode
+    bool broadcast(); // broadcast the characteristic value in the advertisement data
+    
+    // GATT server
+    /**
+     * @brief   Has the GATT client written a new value
+     *
+     * @param   none
+     *
+     * @return  bool    true - Written, false - Not changed
+     *
+     * @note  GATT server only. GATT client always return false.
+     */
+    bool written();
+    
+    /**
+     * @brief   Is the GATT client subscribed
+     *
+     * @param   none
+     *
+     * @return  bool    true - Subscribed, false - Not subscribed
+     *
+     * @note  GATT server and client
+     */
+    bool subscribed();
+    
+    /**
+     * @brief   Can a notification be sent to the GATT client
+     *
+     * @param   none
+     *
+     * @return  true - Yes, false - No
+     *
+     * @note  GATT server only
+     */
+    bool canNotify();
+    
+    /**
+     * @brief   Can a indication be sent to the GATT client
+     *
+     * @param   none
+     *
+     * @return  true - Yes, false - No
+     *
+     * @note  GATT server only
+     */
+    bool canIndicate();
+    
+    // GATT
+    /**
+     * @brief   Can the characteristic be read (based on properties)
+     *
+     * @param   none
+     *
+     * @return  true - readable, false - None
+     *
+     * @note  none
+     */
+    bool canRead();
+    
     /**
-     * Get data pointer to the value of the Characteristic
+     * @brief   Can the characteristic be written (based on properties)
      *
-     * @return const unsigned char* pointer to the value of the Characteristic
+     * @param   none
+     *
+     * @return  true - writable, false - None
+     *
+     * @note  none
+     */
+    bool canWrite();
+    
+    /**
+     * @brief   Can the characteristic be subscribed to (based on properties)
+     *
+     * @param   none
+     *
+     * @return  true - Can be subscribed, false - No
+     *
+     * @note  What different with canUnsubscribe?
      */
-    const unsigned char* value(void) const;
+    bool canSubscribe();
+    
+    /**
+     * @brief   Can the characteristic be unsubscribed to (based on properties)
+     *
+     * @param   none
+     *
+     * @return  true - Can be unsubscribed, false - No
+     *
+     * @note  none
+     */
+    bool canUnsubscribe();
 
     /**
-     * Get the current length of the value of the Characteristic
+     * @brief   Read the characteristic value
+     *
+     * @param   none
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  Only for GATT client. Schedule read request to the GATT server
+     *        Arduino requests to have read, by default, be blocking.
+     */
+    virtual bool read(bool blocked = true);
+    
+    /**
+     * @brief   Write the charcteristic value
+     *
+     * @param   value   The value buffer that want to write to characteristic
+     *
+     * @param   length  The value buffer's length
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  Only for GATT client. Schedule write request to the GATT server
+     */
+    virtual bool write(const unsigned char* value, int length);
+    
+    /**
+     * @brief   Subscribe to the characteristic
+     *
+     * @param   none
+     *
+     * @return  bool    true - Success, false - Failed
      *
-     * @return unsigned short size of characateristic value in bytes
+     * @note  Only for GATT client. Schedule CCCD to the GATT server
      */
-    unsigned short valueLength() const;
+    bool subscribe();
+    
+    /**
+     * @brief   Unsubscribe to the characteristic
+     *
+     * @param   none
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  Only for GATT client. Schedule CCCD to the GATT server
+     */
+    bool unsubscribe();
 
-    unsigned char operator[] (int offset) const;
 
     /**
-     * Has the value of the Characteristic been written by a central
+     * @brief   Read response or notification updated the characteristic
+     *
+     * @param   none
      *
-     * @return bool true is central has updated characteristic value, otherwise false
+     * @return  bool    true - Written, false - Not changed
+     *
+     * @note  GATT client only. GATT server always return false.
      */
-    bool written(void);
-
+    bool valueUpdated();
+    
     /**
-     * Is a central listening for notifications or indications of the Characteristic
+     * @brief   Add the characteristic's descriptor
+     *
+     * @param   descriptor  The descriptor for characteristic
      *
-     * @return bool true is central is subscribed, otherwise false
+     * @return  none
+     *
+     * @note  none
      */
-    bool subscribed(void);
-
+    int addDescriptor(BLEDescriptor& descriptor);
+    
+    /**
+     * @brief   Get the number of descriptors the characteristic has
+     *
+     * @param   none
+     *
+     * @return  int     the number of descriptors the characteristic has
+     *
+     * @note  none
+     */
+    int descriptorCount() const;
+    
+    /**
+     * @brief   Does the characteristic have a descriptor with the specified UUID
+     *
+     * @param   uuid        The descriptor's UUID
+     *
+     * @return  bool        true - Yes.     false - No
+     *
+     * @note  none
+     */
+    bool hasDescriptor(const char* uuid) const;
+    
+    /**
+     * @brief   Does the characteristic have an nth descriptor with the specified UUID
+     *
+     * @param   uuid        The descriptor's UUID
+     *
+     * @param   index       The index of descriptor
+     *
+     * @return  bool        true - Yes.     false - No
+     *
+     * @note  none
+     */
+    bool hasDescriptor(const char* uuid, int index) const;
+    
     /**
-     * Provide a function to be called when events related to this Characteristic are raised
+     * @brief   Get the nth descriptor of the characteristic
+     *
+     * @param   index   The index of descriptor
      *
-     * @param event Event type to set event handler for
-     * @param callback  Pointer to callback function to invoke when the event occurs.
+     * @return  BLEDescriptor   The descriptor
+     *
+     * @note  none
      */
-    void setEventHandler(BLECharacteristicEvent event, BLECharacteristicEventHandler callback);
+    BLEDescriptor descriptor(int index) const;
+    
+    /**
+     * @brief   Get the descriptor with the specified UUID
+     *
+     * @param   uuid        The descriptor's UUID
+     *
+     * @return  BLEDescriptor   The descriptor
+     *
+     * @note  none
+     */
+    BLEDescriptor descriptor(const char * uuid) const;
+    
+    /**
+     * @brief   Get the nth descriptor with the specified UUID
+     *
+     * @param   uuid        The descriptor's UUID
+     *
+     * @param   index       The index of descriptor
+     *
+     * @return  BLEDescriptor   The descriptor
+     *
+     * @note  none
+     */
+    BLEDescriptor descriptor(const char * uuid, int index) const;
 
+    /**
+     * @brief   Set an event handler (callback)
+     *
+     * @param   event           Characteristic event
+     *
+     * @param   eventHandler    The handler of characteristic
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void setEventHandler(BLECharacteristicEvent event, 
+                         BLECharacteristicEventHandler eventHandler);
+    void setEventHandler(BLECharacteristicEvent event, 
+                         BLECharacteristicEventHandlerOld eventHandler);
+    
 protected:
-    bool add(uint16_t serviceHandle);
-
-    uint16_t valueHandle(void);
-
-    uint16_t cccdHandle(void);
-
-    void setValue(BLECentral& central, const uint8_t value[], uint16_t length);
-    void setCccdValue(BLECentral& central, uint16_t value);
-
-    void setUserDescription(BLEDescriptor *descriptor);
-    void setPresentationFormat(BLEDescriptor *descriptor);
-
-    friend class BLEPeripheral;
-
+    friend class BLEDevice;
+    friend class BLEService;
+    friend class BLEServiceImp;
+    /**
+     * @brief   Create a characteristic with specified value size
+     *
+     * @param   characteristicImp  The implementation of the characteristic
+     *
+     * @param   bleDev      The peer BLE device
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLECharacteristic(BLECharacteristicImp *characteristicImp,
+                      const BLEDevice *bleDev);
+    
+    /**
+     * @brief   Create a characteristic with string value
+     *
+     * @param   uuid        The UUID of the characteristic
+     *
+     * @param   properties  The properties of the characteristic
+     *
+     * @param   value       The string of the characteristic data
+     *
+     * @param   bleDev      The peer BLE device
+     *
+     * @return  none
+     *
+     * @note  The data length is string's size. Can't set a string that is longger
+     *          than the this input
+     */
+    //BLECharacteristic(const char* uuid, 
+    //                  unsigned char properties, 
+    //                  const char* value,
+    //                  BLEDevice *bleDev);
+    
+    // For GATT
+    void setBLECharacteristicImp(BLECharacteristicImp *characteristicImp);
+    BLECharacteristicImp* fetchCharacteristicImp();
 private:
     void _setValue(const uint8_t value[], uint16_t length);
-
+    BLECharacteristicImp *getImplementation() const;
+    
 private:
-    unsigned char _properties;
-    unsigned short _value_size;
-    unsigned short _value_length;
-    unsigned char* _value;
-    bool _written;
-
-    uint16_t _cccd_value;
-    uint16_t _value_handle;
-    uint16_t _cccd_handle;
-
-    BLEDescriptor* _user_description;
-    BLEDescriptor* _presentation_format;
+    char    _uuid_cstr[37];  // The characteristic UUID
+    BLEDevice _bledev;  // The GATT server BLE object. Only for GATT client to read/write
+                        //  NULL - GATT server
+                        //  None-NULL - GATT client
+    BLECharacteristicImp *_internal;    // The real implementation of characteristic.
+    BLECharacteristicImp *_chrc_local_imp;
+    bool _broadcast;
+protected:
+    friend class BLECharacteristicImp;
+    unsigned char _properties;      // The characteristic property
     
-    BLECharacteristicEventHandler _event_handlers[BLECharacteristicEventLast];
+    unsigned short _value_size;       // The value size
+    unsigned char* _value;          // The value. Will delete after create the _internal
+    
+    BLECharacteristicEventHandler _event_handlers[BLECharacteristicEventLast];  // Sid. Define the arr as in BLECharacteristicImp.h
+    
+    BLECharacteristicEventHandlerOld _oldevent_handlers[BLECharacteristicEventLast];
 };
 
-#endif // _BLE_CHARACTERISTIC_H_INCLUDED
+#endif
+
diff --git a/libraries/CurieBLE/src/BLECommon.h b/libraries/CurieBLE/src/BLECommon.h
index 20bf0e23..37ac49df 100644
--- a/libraries/CurieBLE/src/BLECommon.h
+++ b/libraries/CurieBLE/src/BLECommon.h
@@ -21,16 +21,32 @@
 #define _BLE_COMMON_H_INCLUDED
 
 #include "Arduino.h"
+//#include "CurieBLE.h"
 
-#include "../src/services/ble/ble_protocol.h"
-#include "services/ble/ble_service_gatt.h"
-#include "services/ble/ble_service_gatts_api.h"
+#include "../src/services/ble_service/ble_protocol.h"
+
+
+#include "infra/log.h"
+
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/gatt.h>
+#include <bluetooth/conn.h>
+#include <bluetooth/hci.h>
+//#include <bluetooth/uuid.h>
+
+#define BLE_ADDR_LEN 6
+
+#define UUID_SIZE_128   16
+#define UUID_SIZE_16    2
+#define MAX_UUID_SIZE   UUID_SIZE_128
 
 /* Theoretically we should be able to support attribute lengths up to 512 bytes
  * but this involves splitting it across multiple packets.  For simplicity,
  * we will just limit this to 20 bytes for now, which will fit in a single packet
  */
-#define BLE_MAX_ATTR_DATA_LEN 20
+#define BLE_MAX_ATTR_DATA_LEN       20
+#define BLE_MAX_ATTR_LONGDATA_LEN   512
 
 /* Default device name prefix, applied only if user does not provide a name
  * If a factory-configured MAC address is defined, the last 2 bytes of the
@@ -40,6 +56,90 @@
 /* Invalid BLE Address type */
 #define BLE_DEVICE_ADDR_INVALID 0xFF
 
+#if 0
+/** BLE response/event status codes. */
+enum BLE_STATUS {
+    BLE_STATUS_SUCCESS = 0, /**< General BLE Success code */
+    BLE_STATUS_PENDING, /**< Request received and execution started, response pending */
+    BLE_STATUS_TIMEOUT, /**< Request timed out */
+    BLE_STATUS_NOT_SUPPORTED, /**< Request/feature/parameter not supported */
+    BLE_STATUS_NOT_ALLOWED, /**< Request not allowed */
+    BLE_STATUS_LINK_TIMEOUT, /**< Link timeout (link loss) */
+    BLE_STATUS_NOT_ENABLED, /**< BLE not enabled, @ref ble_enable */
+    BLE_STATUS_ERROR,   /**< Generic Error */
+    BLE_STATUS_ALREADY_REGISTERED, /**< BLE service already registered */
+    BLE_STATUS_WRONG_STATE, /**< Wrong state for request */
+    BLE_STATUS_ERROR_PARAMETER, /**< Parameter in request is wrong */
+    BLE_STATUS_NO_MEMORY, /**< System doesn't have memory */
+    BLE_STATUS_GAP_BASE = 0x100, /**< GAP specific error base */
+    BLE_STATUS_GATT_BASE = 0x200, /**< GATT specific Error base */
+};
+#endif
+
+typedef enum 
+{
+    BLE_STATUS_SUCCESS = 0,
+    BLE_STATUS_FORBIDDEN, /**< The operation is forbidden. Central mode call peripheral API and vice versa */
+    BLE_STATUS_PENDING, /**< Request received and execution started, response pending */
+    BLE_STATUS_TIMEOUT, /**< Request timed out */
+    BLE_STATUS_NOT_SUPPORTED, /**< Request/feature/parameter not supported */
+    BLE_STATUS_NOT_FOUND,
+    BLE_STATUS_NOT_ALLOWED, /**< Request not allowed */
+    BLE_STATUS_LINK_TIMEOUT, /**< Link timeout (link loss) */
+    BLE_STATUS_NOT_ENABLED, /**< BLE not enabled, @ref ble_enable */
+    BLE_STATUS_ERROR,   /**< Generic Error */
+    BLE_STATUS_ALREADY_REGISTERED, /**< BLE service already registered */
+    BLE_STATUS_WRONG_STATE, /**< Wrong state for request */
+    BLE_STATUS_ERROR_PARAMETER, /**< Parameter in request is wrong */
+    BLE_STATUS_NO_MEMORY, /**< System doesn't have memory */
+    BLE_STATUS_NO_SERVICE, /**< System doesn't have service */
+}BLE_STATUS_T;
+
+typedef uint16_t ble_status_t; /**< Response and event BLE service status type @ref BLE_STATUS */
+
 typedef ble_status_t BleStatus;
 
+#define BLE_LIB_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+
+#define BLE_MAX_CONN_CFG            2
+#define BLE_MAX_ADV_BUFFER_CFG      3
+#define BLE_MAX_ADV_FILTER_SIZE_CFG 20
+
+typedef bool (*ble_advertise_handle_cb_t)(uint8_t type, const uint8_t *dataPtr,
+                                          uint8_t data_len, const bt_addr_le_t *addrPtr);
+
+
+typedef struct ble_conn_param {
+    float interval_min; // millisecond 7.5 - 4000ms
+    float interval_max; // millisecond 7.5 - 4000ms
+    uint16_t latency;   // 0x0000 - 0x01F4
+    uint16_t timeout;   // millisecond 100 - 32000ms
+}ble_conn_param_t;
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "os/os.h"
+
+extern void __assert_fail(void);
+
+/// Define the structure for app
+typedef struct bt_uuid      bt_uuid_t;
+typedef struct bt_uuid_16   bt_uuid_16_t;
+typedef struct bt_uuid_128  bt_uuid_128_t;
+typedef struct bt_conn  bt_conn_t;
+typedef struct bt_gatt_attr  bt_gatt_attr_t;
+typedef struct bt_gatt_discover_params  bt_gatt_discover_params_t;
+typedef struct bt_le_scan_param  bt_le_scan_param_t;
+typedef struct bt_le_conn_param bt_le_conn_param_t;
+typedef struct bt_gatt_subscribe_params bt_gatt_subscribe_params_t;
+typedef struct bt_gatt_read_params bt_gatt_read_params_t;
+typedef struct _bt_gatt_ccc _bt_gatt_ccc_t;
+typedef struct bt_gatt_chrc bt_gatt_chrc_t;
+typedef struct bt_gatt_ccc_cfg bt_gatt_ccc_cfg_t;
+typedef struct bt_data bt_data_t;
+
+#ifdef __cplusplus
+}
+#endif
 #endif // _BLE_COMMON_H_INCLUDED
diff --git a/libraries/CurieBLE/src/BLEDescriptor.cpp b/libraries/CurieBLE/src/BLEDescriptor.cpp
index 8e50120b..eb624532 100644
--- a/libraries/CurieBLE/src/BLEDescriptor.cpp
+++ b/libraries/CurieBLE/src/BLEDescriptor.cpp
@@ -1,85 +1,198 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
+  BLE Descriptor API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+#include <errno.h>
+
+#include "./internal/BLEAttribute.h"
 #include "BLEDescriptor.h"
+#include "./internal/BLEUtils.h"
+#include "./internal/BLEDescriptorImp.h"
 
-#include "internal/ble_client.h"
+BLEDescriptor::BLEDescriptor():
+    _properties(0),
+    _value_size(0),
+    _value(NULL)
+{
+    memset(_uuid_cstr, 0, sizeof (_uuid_cstr));
+}
+    
+BLEDescriptor::BLEDescriptor(BLEDescriptorImp* descriptorImp, 
+                             const BLEDevice *bleDev):
+    _bledev(bleDev),
+    _value_size(0),
+    _value(NULL),
+    _internal(descriptorImp)
+{
+    _properties = descriptorImp->properties();
+    memset(_uuid_cstr, 0, sizeof (_uuid_cstr));
+    BLEUtils::uuidBT2String(descriptorImp->bt_uuid(), _uuid_cstr);
+}
 
-BLEDescriptor::BLEDescriptor(const char* uuid, const unsigned char value[], unsigned short valueLength) :
-    BLEAttribute(uuid, BLETypeDescriptor)
+BLEDescriptor::BLEDescriptor(const char* uuid, 
+                             const unsigned char value[], 
+                             unsigned short valueLength):
+    _bledev(),
+    _internal(NULL)
 {
-    if (valueLength > BLE_MAX_ATTR_DATA_LEN) {
-        valueLength = BLE_MAX_ATTR_DATA_LEN;
+    bt_uuid_128_t uuid_tmp;
+    memset(_uuid_cstr, 0, sizeof (_uuid_cstr));
+    BLEUtils::uuidString2BT(uuid, (bt_uuid_t *)&uuid_tmp);
+    BLEUtils::uuidBT2String((const bt_uuid_t *)&uuid_tmp, _uuid_cstr);
+    
+    _bledev.setAddress(*BLEUtils::bleGetLoalAddress());
+    
+    _value_size = (valueLength > BLE_MAX_ATTR_LONGDATA_LEN) ? BLE_MAX_ATTR_LONGDATA_LEN : valueLength;
+    _value = (unsigned char*)malloc(_value_size);
+    if (NULL != _value)
+    {
+        memcpy(_value, value, _value_size);
+    }
+    else
+    {
+        errno = ENOMEM;
+        _value_size = 0;
     }
-    _value_length = valueLength;
-    _value = (unsigned char*)malloc(_value_length);
+}
 
-    memcpy(_value, value, _value_length);
+BLEDescriptor::BLEDescriptor(const char* uuid, 
+                             const char* value):
+     BLEDescriptor(uuid, (const unsigned char*)value, strlen(value))
+{}
+
+BLEDescriptor::BLEDescriptor(const BLEDescriptor& rhs):
+    _bledev(&rhs._bledev),
+    _properties(rhs._properties),
+    _value_size(0),
+    _value(NULL),
+    _internal(rhs._internal)
+{
+    memcpy(_uuid_cstr, rhs._uuid_cstr, sizeof(_uuid_cstr));
+    if (NULL == _internal && rhs._value_size > 0)
+    {
+        _value = (unsigned char*)malloc(rhs._value_size);  // Sid. KW: allocate memory for _value, not local
+        if (_value) 
+        {
+            memcpy(_value, rhs._value, rhs._value_size);
+            _value_size = rhs._value_size;
+        }
+        else
+        {
+            errno = ENOMEM;
+        }
+    }
+}
+
+BLEDescriptor& BLEDescriptor::operator= (const BLEDescriptor& rhs)
+{
+    if (this != &rhs)
+    {
+        memcpy(_uuid_cstr, rhs._uuid_cstr, sizeof(_uuid_cstr));
+        _properties = rhs._properties;
+        _bledev = BLEDevice(&rhs._bledev);
+        _internal = rhs._internal;
+        if (NULL == _internal && rhs._value_size > 0)
+        {
+            if (_value_size < rhs._value_size)
+            {
+                _value_size = rhs._value_size;
+                
+                if (NULL != _value)
+                    free(_value);
+                _value = (unsigned char*)malloc(_value_size);
+            }
+            
+            if (NULL != _value)
+            {
+                memcpy(_value, rhs._value, rhs._value_size);
+            }
+            else
+            {
+                _value_size = 0;
+                errno = ENOMEM;
+            }
+        }
+    }
+    return *this;
 }
 
-BLEDescriptor::~BLEDescriptor() {
-    if (_value) {
+BLEDescriptor::~BLEDescriptor()
+{
+    if (_value)
+    {
         free(_value);
         _value = NULL;
     }
 }
 
-BLEDescriptor::BLEDescriptor(const char* uuid, const char* value) :
-    BLEDescriptor(uuid, (const uint8_t*)value, strlen(value))
+const char* BLEDescriptor::uuid() const
 {
+    return _uuid_cstr;
 }
 
-const unsigned char*
-BLEDescriptor::BLEDescriptor::value() const
+const byte* BLEDescriptor::value() const
 {
-    return _value;
+    const byte* ret = _value;
+    if (NULL != _internal)
+    {
+        ret = _internal->value();
+    }
+    return ret;
 }
 
-unsigned short
-BLEDescriptor::valueLength() const
+int BLEDescriptor::valueLength() const
 {
-    return _value_length;
+    int ret = _value_size;
+    if (NULL != _internal)
+    {
+        ret = _internal->valueLength();
+    }
+    return ret;
 }
 
-unsigned char
-BLEDescriptor::operator[] (int offset) const
+BLEDescriptor::operator bool() const
 {
-    return _value[offset];
+    return (strlen(_uuid_cstr) > 3);
 }
 
-bool
-BLEDescriptor::add(uint16_t serviceHandle)
+unsigned char BLEDescriptor::properties() const
 {
-    bt_uuid uuid = btUuid();
-    struct ble_gatts_descriptor desc;
-    uint16_t handle = 0;
-
-    memset(&desc, 0, sizeof(desc));
-
-    desc.p_uuid = &uuid;
+    return _properties;
+}
 
-    desc.p_value = _value;
-    desc.length = _value_length;
 
-    // this class only supports read-only descriptors
-    desc.perms.rd = GAP_SEC_MODE_1 | GAP_SEC_LEVEL_1;
-    desc.perms.wr = GAP_SEC_NO_PERMISSION;
+int BLEDescriptor::valueSize() const
+{
+    int ret = _value_size;
+    if (NULL != _internal)
+    {
+        ret = _internal->valueSize();
+    }
+    return ret;
+}
 
-    return (ble_client_gatts_add_descriptor(serviceHandle, &desc, &handle) == BLE_STATUS_SUCCESS); 
+bool BLEDescriptor::read()
+{
+    bool retVar = false;
+    
+    if (NULL != _internal)
+    {
+        retVar = _internal->read();
+    }
+    return retVar;
 }
+
diff --git a/libraries/CurieBLE/src/BLEDescriptor.h b/libraries/CurieBLE/src/BLEDescriptor.h
index 08e53f13..9e95de7d 100644
--- a/libraries/CurieBLE/src/BLEDescriptor.h
+++ b/libraries/CurieBLE/src/BLEDescriptor.h
@@ -1,77 +1,128 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
+  BLE Descriptor API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
 
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
 
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
 
-#ifndef _BLE_DESCRIPTOR_H_INCLUDED
-#define _BLE_DESCRIPTOR_H_INCLUDED
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
 
-#include "BLEAttribute.h"
+#ifndef ARDUINO_BLE_DESCRIPTOR_H
+#define ARDUINO_BLE_DESCRIPTOR_H
 
-/**
- * BLE GATT Descriptor class
- */
-class BLEDescriptor : public BLEAttribute {
-public:
-    /**
-     * Constructor for BLE Descriptor
-     *
-     * @param uuid        16-bit UUID (in string form) defined by BLE standard
-     * @param value       Value of descriptor, as a byte array.  Data is stored in internal copy.
-     * @param valueLength Data length required for descriptor value (<= BLE_MAX_ATTR_DATA_LEN)
-     */
-    BLEDescriptor(const char* uuid, const unsigned char value[], unsigned short valueLength);
+#include "CurieBLE.h"
+
+#include "BLEDevice.h"
+
+class BLEDescriptor
+{
+  public:
+    BLEDescriptor();
+    BLEDescriptor(const char* uuid, const unsigned char value[], unsigned short valueLength); // create a descriptor the specified uuid and value
+    BLEDescriptor(const char* uuid, const char* value); // create a descriptor the specified uuid and string value
+
+    BLEDescriptor(BLEDescriptorImp* descriptorImp, const BLEDevice *bleDev);
+    BLEDescriptor(const BLEDescriptor&);
+    BLEDescriptor& operator=(const BLEDescriptor&);
 
     virtual ~BLEDescriptor();
 
     /**
-     * Constructor for BLE Descriptor
+     * @brief   Get the descriptor's UUID string
+     *
+     * @param   none
+     *
+     * @return  const char*     The UUID string
      *
-     * @param uuid        16-bit UUID (in string form) defined by BLE standard
-     * @param value       String value of descriptor.  Data is stored in internal copy. 
-     *                    (String length <= BLE_MAX_ATTR_DATA_LEN)
+     * @note  none
      */
-    BLEDescriptor(const char* uuid, const char* value);
+    const char* uuid() const;
 
+    
     /**
-     * Get data pointer to the value of the Descriptor
+     * @brief   Get the value of descriptor
+     *
+     * @param   none
+     *
+     * @return  const byte*     The value buffer
      *
-     * @return const unsigned char* pointer to the value of the Descriptor
+     * @note  none
      */
-    const unsigned char* value(void) const;
-
+    virtual const byte* value() const;
+    
     /**
-     * Get the length of the value of the Descriptor
+     * @brief   Get the current length of the value
+     *
+     * @param   none
+     *
+     * @return  int     The current length of the value string
      *
-     * @return unsigned short size of Descriptor value in bytes
+     * @note  none
      */
-    unsigned short valueLength(void) const;
-
-
-    unsigned char operator[] (int offset) const;
-
-protected:
-    bool add(uint16_t serviceHandle);
-
-    friend BLEPeripheral;
+    virtual int valueLength() const;
+    
+    /**
+     * @brief   Is the descriptor valid
+     *
+     * @param   none
+     *
+     * @return  bool    true/false
+     *
+     * @note  none
+     */
+    virtual operator bool() const;
+    
+    /**
+     * @brief   Read the descriptor value
+     *
+     * @param   none
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  Only for GATT client. Schedule read request to the GATT server
+     */
+    bool read();
 
+    /**
+     * @brief   Get the property mask of the descriptor
+     *
+     * @param   none
+     *
+     * @return  unsigned char       The property mask of the descriptor
+     *
+     * @note  none
+     */
+    unsigned char properties() const;
+    
+    /**
+     * @brief   Get the maximum size of the value
+     *
+     * @param   none
+     *
+     * @return  int     The maximum size of the value
+     *
+     * @note  none
+     */
+    int valueSize() const;
 private:
-    unsigned short _value_length;
-    unsigned char* _value;
+    char    _uuid_cstr[37];  // The characteristic UUID
+    BLEDevice _bledev; 
+    
+    unsigned char _properties;      // The characteristic property
+    
+    unsigned short _value_size;     // The value size
+    unsigned char* _value;          // The value. Will delete after create the _internal
+    BLEDescriptorImp *_internal;    // The real implementation of Descriptor
 };
 
-#endif // _BLE_DESCRIPTOR_H_INCLUDED
+#endif
diff --git a/libraries/CurieBLE/src/BLEDevice.cpp b/libraries/CurieBLE/src/BLEDevice.cpp
new file mode 100644
index 00000000..f4713cf8
--- /dev/null
+++ b/libraries/CurieBLE/src/BLEDevice.cpp
@@ -0,0 +1,544 @@
+/*
+  BLE Device API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+#include "CurieBLE.h"
+#include "BLEDevice.h"
+
+#include "./internal/ble_client.h"
+
+#include "./internal/BLEUtils.h"
+
+#include "./internal/BLEProfileManager.h"
+#include "./internal/BLEDeviceManager.h"
+#include "./internal/BLECharacteristicImp.h"
+
+BLEDevice::BLEDevice()
+{
+    memset(&_bt_addr, 0, sizeof(_bt_addr));
+    _conn_param.interval_max = BT_GAP_INIT_CONN_INT_MAX;
+    _conn_param.interval_min = BT_GAP_INIT_CONN_INT_MIN;
+    _conn_param.latency = 0;
+    _conn_param.timeout = 400;
+}
+
+/*
+BLEDevice::BLEDevice(String bleaddress)
+{
+    BLEUtils::macAddressString2BT(bleaddress.c_str(), _bt_addr);
+}
+
+BLEDevice::BLEDevice(const char* bleaddress)
+{
+    BLEUtils::macAddressString2BT(bleaddress, _bt_addr);
+}
+
+*/
+
+BLEDevice::BLEDevice(const bt_addr_le_t* bleaddress):
+    BLEDevice()
+{
+    memcpy(&_bt_addr, bleaddress, sizeof(_bt_addr));
+    BLEDeviceManager::instance()->getConnectionInterval(this, &_conn_param);
+}
+
+BLEDevice::BLEDevice(const BLEDevice* bledevice)
+{
+    memcpy(&_bt_addr, bledevice->bt_le_address(), sizeof(_bt_addr));
+    memcpy(&_conn_param, &bledevice->_conn_param, sizeof (_conn_param));
+}
+
+BLEDevice::BLEDevice(const BLEDevice& bledevice)
+{
+    memcpy(&_bt_addr, bledevice.bt_le_address(), sizeof(_bt_addr));
+    memcpy(&_conn_param, &bledevice._conn_param, sizeof (_conn_param));
+}
+
+BLEDevice::~BLEDevice()
+{
+    //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+}
+
+bool BLEDevice::begin()
+{
+    return BLEDeviceManager::instance()->begin(this);
+}
+
+void BLEDevice::poll()
+{
+    BLEProfileManager::instance()->handleDisconnectedPutOffEvent();
+    BLEDeviceManager::instance()->poll();
+}
+
+void BLEDevice::end()
+{
+    if (BLEUtils::isLocalBLE(*this))
+    {
+        BLEDeviceManager::instance()->end();
+    }
+}
+
+bool BLEDevice::connected() const
+{
+    bool link_exist = BLEDeviceManager::instance()->connected(this);
+    {
+    // If release the discoverd attributes, 
+    //  the GATT client may has crash issue due to used release pointer
+    BLEProfileManager::instance()->handleDisconnectedPutOffEvent();
+    }
+    return link_exist;
+}
+
+bool BLEDevice::disconnect()
+{
+    bool retval = BLEDeviceManager::instance()->disconnect(this);
+    BLEProfileManager::instance()->handleDisconnectedPutOffEvent();
+    return retval;
+}
+
+String BLEDevice::address() const
+{
+    return BLEUtils::macAddressBT2String(_bt_addr);
+}
+
+void BLEDevice::setAddress(const bt_addr_le_t& addr)
+{
+    memcpy(&_bt_addr, &addr, sizeof(_bt_addr));
+}
+
+void BLEDevice::setAdvertisedServiceUuid(const char* advertisedServiceUuid)
+{
+    BLEDeviceManager::instance()->setAdvertisedServiceUuid(advertisedServiceUuid);
+}
+
+void BLEDevice::setAdvertisedService(const BLEService& service)
+{
+    setAdvertisedServiceUuid(service.uuid());
+}
+
+void BLEDevice::setServiceSolicitationUuid(const char* serviceSolicitationUuid)
+{
+    BLEDeviceManager::instance()->setServiceSolicitationUuid(serviceSolicitationUuid);
+}
+
+void BLEDevice::setManufacturerData(const unsigned char manufacturerData[], 
+                                    unsigned char manufacturerDataLength)
+{
+    BLEDeviceManager::instance()->setManufacturerData(manufacturerData, manufacturerDataLength);
+}
+
+bool BLEDevice::getManufacturerData (unsigned char* manu_data, 
+                                     unsigned char& manu_data_len) const
+{
+    return BLEDeviceManager::instance()->getManufacturerData(this, manu_data, manu_data_len);
+}
+
+bool BLEDevice::hasManufacturerData() const
+{
+    return BLEDeviceManager::instance()->hasManufacturerData(this);
+}
+
+void BLEDevice::setLocalName(const char *localName)
+{
+    BLEDeviceManager::instance()->setLocalName(localName);
+}
+
+void BLEDevice::setAdvertisingInterval(float advertisingInterval)
+{
+    BLEDeviceManager::instance()->setAdvertisingInterval(advertisingInterval);
+}
+
+void BLEDevice::setConnectionInterval(int minInterval, 
+                                      int maxInterval,
+                                      uint16_t latency,
+                                      uint16_t timeout)
+{
+    uint16_t minVal = (uint16_t)MSEC_TO_UNITS(minInterval, UNIT_1_25_MS);
+    uint16_t maxVal = (uint16_t)MSEC_TO_UNITS(maxInterval, UNIT_1_25_MS);
+    uint16_t timeoutVal = MSEC_TO_UNITS(timeout, UNIT_10_MS);
+    _conn_param.interval_min = minVal;
+    _conn_param.interval_max = maxVal;
+    _conn_param.timeout = timeoutVal;
+    _conn_param.latency = latency;
+    BLEDeviceManager::instance()->setConnectionInterval(this);
+}
+
+void BLEDevice::setConnectionInterval(int minimumConnectionInterval, 
+                                      int maximumConnectionInterval)
+{
+    uint16_t minVal = (uint16_t)MSEC_TO_UNITS(minimumConnectionInterval, UNIT_1_25_MS);
+    uint16_t maxVal = (uint16_t)MSEC_TO_UNITS(maximumConnectionInterval, UNIT_1_25_MS);
+    _conn_param.interval_min = minVal;
+    _conn_param.interval_max = maxVal;
+    
+    BLEDeviceManager::instance()->setConnectionInterval(this);
+}
+
+int BLEDevice::getConnectionInterval()
+{
+    bt_le_conn_param_t conn_param;
+    
+    BLEDeviceManager::instance()->getConnectionInterval(this, &conn_param);
+    return UNITS_TO_MSEC((int)conn_param.interval_max, UNIT_1_25_MS);
+}
+
+int BLEDevice::getConnectionTimeout()
+{
+    bt_le_conn_param_t conn_param;
+    
+    BLEDeviceManager::instance()->getConnectionInterval(this, &conn_param);
+    return UNITS_TO_MSEC(conn_param.timeout, UNIT_10_MS);;
+}
+
+int BLEDevice::getConnectionLatency()
+{
+    bt_le_conn_param_t conn_param;
+    
+    BLEDeviceManager::instance()->getConnectionInterval(this, &conn_param);
+    return conn_param.latency;
+}
+
+bool BLEDevice::setTxPower(int txPower)
+{
+    return BLEDeviceManager::instance()->setTxPower(txPower);
+}
+
+void BLEDevice::setConnectable(bool connectable)
+{
+    BLEDeviceManager::instance()->setConnectable(connectable);
+}
+
+void BLEDevice::setDeviceName(const char* deviceName)
+{
+    BLEDeviceManager::instance()->setDeviceName(deviceName);
+}
+
+void BLEDevice::setAppearance(unsigned short appearance)
+{
+    BLEDeviceManager::instance()->setAppearance(appearance);
+}
+
+int BLEDevice::addService(BLEService& attribute)
+{
+    BLEServiceImp *service_imp = BLEProfileManager::instance()->addService(*this, attribute);
+    if (NULL == service_imp)
+    {
+        return BLE_STATUS_NO_MEMORY;
+    }
+    return BLE_STATUS_SUCCESS;
+}
+
+int BLEDevice::advertise()
+{
+    preCheckProfile();
+    return BLEDeviceManager::instance()->startAdvertising();
+}
+
+void BLEDevice::stopAdvertise()
+{
+    BLEDeviceManager::instance()->stopAdvertising();
+}
+
+BLEDevice BLEDevice::central()
+{
+    return BLEDeviceManager::instance()->central();
+}
+
+BLEDevice BLEDevice::peripheral()
+{
+    return BLEDeviceManager::instance()->peripheral();
+}
+
+BLEDevice::operator bool() const
+{
+    return BLEUtils::macAddressValid(_bt_addr);
+}
+
+BLEDevice& BLEDevice::operator=(const BLEDevice& device)
+{
+    if (this != &device)
+    {
+        memcpy(&(this->_bt_addr), &(device._bt_addr), sizeof (this->_bt_addr));
+        memcpy(&this->_conn_param, &device._conn_param, sizeof (this->_conn_param));
+    }
+    return *this;
+}
+
+bool BLEDevice::operator==(const BLEDevice& device) const
+{
+    return (memcmp(this->_bt_addr.val, device._bt_addr.val, 6) == 0);
+}
+
+bool BLEDevice::operator!=(const BLEDevice& device) const
+{
+    return (memcmp(this->_bt_addr.val, device._bt_addr.val, 6) != 0);
+}
+
+
+bool BLEDevice::startScan(bool withDuplicates)
+{
+    preCheckProfile();
+    if (withDuplicates)
+    {
+        return BLEDeviceManager::instance()->startScanningWithDuplicates();
+    }
+    else
+    {
+        return BLEDeviceManager::instance()->startScanningNewPeripherals();
+    }
+}
+
+void BLEDevice::scan(bool withDuplicates)
+{
+    BLEDeviceManager::instance()->clearAdvertiseCritical();
+    startScan(withDuplicates);
+}
+
+void BLEDevice::scanForName(String name, bool withDuplicates)
+{
+    BLEDeviceManager::instance()->setAdvertiseCritical(name);
+    startScan(withDuplicates);
+}
+
+void BLEDevice::scanForUuid(String uuid, bool withDuplicates)
+{
+    BLEService service_temp(uuid.c_str());
+    BLEDeviceManager::instance()->setAdvertiseCritical(service_temp);
+    startScan(withDuplicates);
+}
+
+void BLEDevice::scanForAddress(String macaddr, bool withDuplicates)
+{
+    BLEDeviceManager::instance()->setAdvertiseCritical(macaddr.c_str());
+    startScan(withDuplicates);
+}
+
+void BLEDevice::stopScan()
+{
+    BLEDeviceManager::instance()->stopScanning();
+}
+
+BLEDevice BLEDevice::available()
+{
+    BLEProfileManager::instance()->handleDisconnectedPutOffEvent();
+    return BLEDeviceManager::instance()->available();
+}
+
+bool BLEDevice::hasLocalName() const
+{
+    return BLEDeviceManager::instance()->hasLocalName(this);
+}
+
+bool BLEDevice::hasAdvertisedServiceUuid() const
+{
+    return BLEDeviceManager::instance()->hasAdvertisedServiceUuid(this);
+}
+
+bool BLEDevice::hasAdvertisedServiceUuid(int index) const
+{
+    return BLEDeviceManager::instance()->hasAdvertisedServiceUuid(this, index);
+}
+
+int BLEDevice::advertisedServiceUuidCount() const
+{
+    return BLEDeviceManager::instance()->advertisedServiceUuidCount(this);
+}
+
+String BLEDevice::localName() const
+{
+    return BLEDeviceManager::instance()->localName(this);
+}
+
+String BLEDevice::advertisedServiceUuid() const
+{
+    return BLEDeviceManager::instance()->advertisedServiceUuid(this);
+}
+
+String BLEDevice::advertisedServiceUuid(int index) const
+{
+    return BLEDeviceManager::instance()->advertisedServiceUuid(this, index);
+}
+
+int BLEDevice::rssi() const
+{
+    return BLEDeviceManager::instance()->rssi(this);
+}
+
+bool BLEDevice::connect()
+{
+    return BLEDeviceManager::instance()->connect(*this);
+}
+
+bool BLEDevice::discoverAttributes()
+{
+    return BLEProfileManager::instance()->discoverAttributes(this);
+}
+
+bool BLEDevice::discoverAttributesByService(const char* svc_uuid)
+{
+    bt_uuid_128_t uuid;
+    BLEUtils::uuidString2BT(svc_uuid, (bt_uuid_t *)&uuid);
+    return BLEProfileManager::instance()->discoverAttributesByService(this, (const bt_uuid_t *)&uuid);
+}
+
+
+String BLEDevice::deviceName()
+{
+    return BLEDeviceManager::instance()->deviceName(this);
+}
+
+// For GATT
+int BLEDevice::serviceCount() const
+{
+    return BLEProfileManager::instance()->serviceCount(*this);
+}
+
+bool BLEDevice::hasService(const char* uuid) const
+{
+    BLEServiceImp* serviceImp = BLEProfileManager::instance()->service(*this, uuid);
+    return (NULL != serviceImp);
+}
+
+bool BLEDevice::hasService(const char* uuid, int index) const
+{
+    BLEServiceImp* serviceImp = BLEProfileManager::instance()->service(*this, index);
+    return serviceImp->compareUuid(uuid);
+}
+
+BLEService BLEDevice::service(int index) const
+{
+    BLEServiceImp* serviceImp = BLEProfileManager::instance()->service(*this, index);
+    if (serviceImp != NULL)
+    {
+        BLEService temp(serviceImp, this);
+        return temp;
+    }
+    BLEService temp;
+    return temp;
+}
+
+BLEService BLEDevice::service(const char * uuid) const
+{
+    BLEServiceImp* serviceImp = BLEProfileManager::instance()->service(*this, uuid);
+    if (serviceImp != NULL)
+    {
+        BLEService temp(serviceImp, this);
+        return temp;
+    }
+    BLEService temp;
+    return temp;
+}
+
+BLEService BLEDevice::service(const char * uuid, int index) const
+{
+    BLEServiceImp* serviceImp = BLEProfileManager::instance()->service(*this, index);
+    if (serviceImp != NULL && serviceImp->compareUuid(uuid))
+    {
+        BLEService temp(serviceImp, this);
+        return temp;
+    }
+    BLEService temp;
+    return temp;
+}
+
+int BLEDevice::characteristicCount() const
+{
+    return BLEProfileManager::instance()->characteristicCount(*this);
+}
+
+bool BLEDevice::hasCharacteristic(const char* uuid) const
+{
+    BLECharacteristicImp* characteristicImp = BLEProfileManager::instance()->characteristic(*this, uuid);
+    return (NULL != characteristicImp);
+}
+
+bool BLEDevice::hasCharacteristic(const char* uuid, int index) const
+{
+    BLECharacteristicImp* characteristicImp = BLEProfileManager::instance()->characteristic(*this, uuid, index);
+    return (NULL != characteristicImp);
+}
+
+BLECharacteristic BLEDevice::characteristic(int index) const
+{
+    BLECharacteristicImp* characteristicImp = BLEProfileManager::instance()->characteristic(*this, index);
+    
+    if (NULL == characteristicImp)
+    {
+        BLECharacteristic temp;
+        return temp;
+    }
+    BLECharacteristic temp(characteristicImp, this);
+    return temp;
+}
+
+BLECharacteristic BLEDevice::characteristic(const char * uuid) const
+{
+    BLECharacteristicImp* characteristicImp = BLEProfileManager::instance()->characteristic(*this, uuid);
+    
+    if (NULL == characteristicImp)
+    {
+        BLECharacteristic temp;
+        return temp;
+    }
+    BLECharacteristic temp(characteristicImp, this);
+    return temp;
+}
+
+BLECharacteristic BLEDevice::characteristic(const char * uuid, int index) const
+{
+    BLECharacteristicImp* characteristicImp = BLEProfileManager::instance()->characteristic(*this, index);
+    if (false == characteristicImp->compareUuid(uuid))
+    {
+        // UUID not matching
+        characteristicImp = NULL;
+    }
+    
+    if (NULL == characteristicImp)
+    {
+        BLECharacteristic temp;
+        return temp;
+    }
+    BLECharacteristic temp(characteristicImp, this);
+    return temp;
+}
+
+// event handler
+void BLEDevice::setEventHandler(BLEDeviceEvent event, 
+                                BLEDeviceEventHandler eventHandler)
+{
+    BLEDeviceManager::instance()->setEventHandler(event, eventHandler);
+}
+
+const bt_addr_le_t* BLEDevice::bt_le_address() const
+{
+    return &_bt_addr;
+}
+const bt_le_conn_param* BLEDevice::bt_conn_param() const
+{
+    return &_conn_param;
+}
+
+void BLEDevice::preCheckProfile()
+{
+    if (false == BLEProfileManager::instance()->hasRegisterProfile() &&
+        BLEProfileManager::instance()->serviceCount(*this) > 0)
+    {
+        BLEProfileManager::instance()->registerProfile(*this);
+        delay(8); 
+    }
+}
+    
diff --git a/libraries/CurieBLE/src/BLEDevice.h b/libraries/CurieBLE/src/BLEDevice.h
new file mode 100644
index 00000000..23279446
--- /dev/null
+++ b/libraries/CurieBLE/src/BLEDevice.h
@@ -0,0 +1,699 @@
+/*
+  BLE Device API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+#ifndef ARDUINO_BLE_DEVICE_H
+#define ARDUINO_BLE_DEVICE_H
+
+#include <Arduino.h>
+
+enum BLEDeviceEvent {
+  BLEConnected = 0,         // BLE device connected
+  BLEDisconnected = 1,      // BLE device disconnected 
+  BLEConParamUpdate = 2,    // Update the connection parameter 
+                            //  Connection update request in central
+                            //  Connection parameter updated in peripheral
+  BLEDiscovered,            // The scanned BLE device
+  BLEDeviceLastEvent
+};
+
+typedef void (*BLEDeviceEventHandler)(BLEDevice device);
+
+class BLEDevice
+{
+  public:
+    /**
+     * @brief   The BLE device constructure
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLEDevice();
+    
+    /**
+     * @brief   The BLE device constructure
+     *
+     * @param[in]   bledevice  BLE device
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLEDevice(const BLEDevice* bledevice);
+    BLEDevice(const BLEDevice& bledevice);
+    /**
+     * @brief   The BLE device constructure
+     *
+     * @param[in]   bleaddress  BLE device address
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLEDevice(const bt_addr_le_t* bleaddress);
+    virtual ~BLEDevice();
+
+    
+    /**
+     * @brief   Initiliaze the BLE hardware
+     *
+     * @return  bool indicating success or error
+     *
+     * @note  This method are for real BLE device. 
+     *          Not for peer BLE device.
+     */
+    bool begin();
+    
+    /**
+     * @brief   Poll for events
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  This method are for real BLE device. 
+     *          Not for peer BLE device.
+     */
+    void poll(); // Do we need add the return value or 
+                 //  input parameter to get the events?
+                 // Events may inlcue:
+                 //  GAP : Connected, Disconnected, Update connetion parameter
+                 //  GATT: Discovered
+    
+    /**
+     * @brief   Deinitiliaze the BLE hardware
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  This method are for real BLE device. 
+     *          Not for peer BLE device.
+     */
+    void end(); 
+
+    /**
+     * @brief   Is the device connected with another BLE device.
+     *
+     * @param   none
+     *
+     * @return   bool indicating success or error
+     *
+     * @note  none
+     */
+    bool connected() const;
+    
+    /**
+     * @brief   Disconnect the connected device/s.
+     *
+     * @param   none
+     *
+     * @return   bool indicating success or error    
+     *
+     * @note  The BLE may connected multiple devices.
+     *        This call will disconnect all conected devices.
+     */
+    bool disconnect();
+    
+    
+    /**
+     * @brief   Get the BLE address of the BLE in string format
+     *
+     * @param   none
+     *
+     * @return String   The address of the BLE in string format
+     *
+     * @note  none
+     */
+    String address() const;
+
+    /**
+     * @brief   Set the service UUID that the BLE Peripheral Device advertises
+     *
+     * @param[in] advertisedServiceUuid  16-bit or 128-bit UUID to advertis
+     *                               (in string form)
+     *
+     * @note This method must be called before the begin method
+     *        Only for peripheral mode.
+     */
+    void setAdvertisedServiceUuid(const char* advertisedServiceUuid);
+    
+    /**
+     * @brief   Set the service that the BLE Peripheral Device will advertise this UUID
+     *
+     * @param[in] service  The service the will in advertise data.
+     *
+     * @note This method must be called before the begin method
+     *        Only for peripheral mode.
+     */
+    void setAdvertisedService(const BLEService& service);
+    
+    /**
+     * @brief   Set the service UUID that is solicited in the BLE Peripheral 
+     *           Device advertises
+     *
+     * @param[in] advertisedServiceUuid  16-bit or 128-bit UUID to advertis
+     *                               (in string form)
+     *
+     * @note This method must be called before the begin method
+     *        Only for peripheral mode.
+     */
+    void setServiceSolicitationUuid(const char* serviceSolicitationUuid);
+
+    /**
+     * @brief   Set the manufacturer data in the BLE Peripheral Device advertises
+     *
+     * @param[in] manufacturerData          The data about manufacturer will 
+     *                                       be set in advertisement
+     * @param[in] manufacturerDataLength    The length of the manufacturer data
+     *
+     * @note This method must be called before the begin method
+     *        Only for peripheral mode.
+     */
+    void setManufacturerData(const unsigned char manufacturerData[], 
+                             unsigned char manufacturerDataLength);
+    
+    bool getManufacturerData (unsigned char* manu_data, 
+                              unsigned char& manu_data_len) const;
+    
+    bool hasManufacturerData() const;
+    
+    /**
+     * Set the local name that the BLE Peripheral Device advertises
+     *
+     * @param[in] localName  local name to advertise
+     *
+     * @note This method must be called before the begin method
+     */
+    void setLocalName(const char *localName);
+
+    /**
+     * @brief   Set advertising interval
+     *
+     * @param[in]   advertisingInterval    Advertising Interval in ms
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void setAdvertisingInterval(float advertisingInterval);
+    
+    /**
+     * @brief   Set the connection parameters and send connection 
+     *           update request in both BLE peripheral and central
+     *
+     * @param[in]   intervalmin     Minimum Connection Interval (ms)
+     *
+     * @param[in]   intervalmax     Maximum Connection Interval (ms)
+     *
+     * @param[in]   latency         Connection Latency
+     *
+     * @param[in]   timeout         Supervision Timeout (ms)
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+     void setConnectionInterval(int minimumConnectionInterval, 
+                                int maximumConnectionInterval,
+                                uint16_t latency, 
+                                uint16_t timeout);
+    
+    /**
+     * @brief   Set the min and max connection interval and send connection 
+     *           update request in both BLE peripheral and central
+     *
+     * @param[in]   intervalmin     Minimum Connection Interval (ms)
+     *
+     * @param[in]   intervalmax     Maximum Connection Interval (ms)
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void setConnectionInterval(int minimumConnectionInterval, 
+                               int maximumConnectionInterval);
+    
+    int getConnectionInterval();
+    int getConnectionTimeout();
+    int getConnectionLatency();
+    
+    /**
+     * @brief   Set TX power of the radio in dBM
+     *
+     * @param[in]   tx_power    The antenna TX power
+     *
+     * @return boolean_t true if established connection, otherwise false
+     */
+    bool setTxPower(int txPower);
+    
+    /**
+     * @brief   Set advertising type as connectable/non-connectable
+     *
+     * @param[in]   connectable     true  - The device connectable
+     *                              false - The device non-connectable
+     *
+     * @return  none
+     *
+     * @note  Only for peripheral mode.
+     *         Default value is connectable
+     */
+    void setConnectable(bool connectable);
+
+    /**
+     * @brief   Set the value of the device name characteristic 
+     *
+     * @param[in] device  User-defined name string for this device.  Truncated if
+     *                   more than maximum allowed string length (20 bytes).
+     *
+     * @note This method must be called before the begin method
+     *       If device name is not set, a default name will be used
+     */
+    void setDeviceName(const char* deviceName);
+    
+    /**
+     * @brief   Set the appearance type for the BLE Peripheral Device
+     *
+     * See https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.gap.appearance.xml
+     * for available options.
+     *
+     * @param[in] appearance Appearance category identifier as defined by BLE Standard
+     *
+     * @return BleStatus indicating success or error
+     *
+     * @note This method must be called before the begin method
+     */
+    void setAppearance(unsigned short appearance);
+
+    /**
+     * @brief   Add a Service to the BLE Peripheral Device
+     *
+     * @param[in] attribute     The service that will add to Peripheral
+     *
+     * @return int     Indicating success or error type @enum BLE_STATUS_T
+     *
+     * @note This method must be called before the begin method
+     */
+    int addService(BLEService& attribute);
+    
+    /**
+     * @brief   Construct the ADV data and start send advertisement
+     *
+     * @param   none
+     *
+     * @return  int       0 - Success. Others - error code @enum BLE_STATUS_T
+     *
+     * @note  none
+     */
+    int advertise();
+    
+    /**
+     * @brief   Stop send advertisement
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void stopAdvertise();
+
+    /**
+     * @brief   Get currently connected central 
+     *
+     * @return BLEDevice Connected central device
+     *
+     * @note  Peripheral mode only
+     */
+    BLEDevice central();
+    
+    /**
+     * @brief   Get currently connected peripheral 
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  Central mode only. How to distinguish the peripheral?
+     */
+    BLEDevice peripheral();
+    
+    operator bool() const;
+    bool operator==(const BLEDevice& device) const;
+    bool operator!=(const BLEDevice& device) const;
+    BLEDevice& operator=(const BLEDevice& device);
+    // central mode
+
+    //void scanForAddress(String address); // Not include in baseline. Add here as feature for feature release.
+    
+    /**
+     * @brief   Start scanning for peripherals with the option of accepting all detectable
+     *          Peripherals or just the newly detected.
+     *
+     * @param[in] withDuplicates        true - return all detectable Peripherals.
+     *                                  false- return a detected Peripheral only once.
+     *
+     * @return  none
+     *
+     * @note  When, withDuplicates = true, accept all detectable Peripherals.
+     *        No Peripheral filtering process applied to the scan result.
+     *        By default, withDuplicates = false, a detected Peripheral is
+     *        reported once.
+     */
+    void scan(bool withDuplicates = false);
+    
+    /**
+     * @brief   Start scanning for peripherals and filter by device name in ADV and
+     *          the option of accepting all detectable Peripherals or just the
+     *          newly detected.
+     *
+     * @param[in]   name    The device's local name.
+     *
+     * @param[in]   withDuplicates      true - return all detectable Peripherals.
+     *                                  false- return a detected Peripheral only once.
+     *
+     * @return  none
+     *
+     * @note  When, withDuplicates = true, accept all detectable Peripherals.
+     *        No Peripheral filtering process applied to the scan result.
+     *        By default, withDuplicates = false, a detected Peripheral is
+     *        reported once.
+     */
+    void scanForName(String name, bool withDuplicates = false);
+    
+    /**
+     * @brief   Start scanning for peripherals and filter by service in ADV and
+     *          the option of accepting all detectable Peripherals or just the
+     *          newly detected.
+     *
+     * @param[in]   service    The service
+     *
+     * @param[in]   withDuplicates      true - return all detectable Peripherals.
+     *                                  false- return a detected Peripheral only once.
+     *
+     * @return  none
+     *
+     * @note   When, withDuplicates = true, accept all detectable Peripherals.
+     *         No Peripheral filtering process applied to the scan result.
+     *         By default, withDuplicates = false, a detected Peripheral is
+     *         reported once.
+     */
+    void scanForUuid(String uuid, bool withDuplicates = false);
+    
+    /**
+     * @brief   Start scanning for peripherals and filter by MAC address and
+     *          the option of accepting all detectable Peripherals or just the
+     *          newly detected.
+     *
+     * @param[in]   macaddr     The Peripheral MAC address
+     *
+     * @param[in]   withDuplicates      true - return all detectable Peripherals.
+     *                                  false- return a detected Peripheral only once.
+     *
+     * @return  none
+     *
+     * @note   When, withDuplicates = true, accept all detectable Peripherals.
+     *         No Peripheral filtering process applied to the scan result.
+     *         By default, withDuplicates = false, a detected Peripheral is
+     *         reported once.
+     */
+    void scanForAddress(String macaddr, bool withDuplicates = false);
+
+    /**
+     * @brief   Stop scanning for peripherals
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void stopScan();
+    
+    /**
+     * @brief   Retrieve a discovered peripheral
+     *
+     * @param   none
+     *
+     * @return  BLEDevice   The BLE device that central scanned
+     *
+     * @note  none
+     */
+    BLEDevice available();
+
+    /**
+     * @brief   Does the peripheral advertise a local name
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none //TODO: The implementation doesn't save the ADV's local name.
+     */
+    bool hasLocalName() const;
+    
+    bool hasAdvertisedServiceUuid() const; // does the peripheral advertise a service
+    bool hasAdvertisedServiceUuid(int index) const; // does the peripheral advertise a service n
+    int advertisedServiceUuidCount() const; // number of services the peripheral is advertising
+
+    String localName() const; // returns the advertised local name as a String
+    String advertisedServiceUuid() const; // returns the advertised service as a UUID String
+    String advertisedServiceUuid(int index) const; // returns the nth advertised service as a UUID String
+
+    int rssi() const; // returns the RSSI of the peripheral at discovery
+
+    bool connect(); // connect to the peripheral
+    bool discoverAttributes(); // discover the peripheral's attributes
+    bool discoverAttributesByService(const char* svc_uuid);
+
+    String deviceName(); // read the device name attribute of the peripheral, and return String value
+    //int appearance(); // read the appearance attribute of the peripheral and return value as int
+
+    // For GATT
+    /**
+     * @brief   returns the number of services the BLE device has
+     *
+     * @param   none
+     *
+     * @return  int     The number of services
+     *
+     * @note  none
+     */
+    int serviceCount() const; 
+    
+    /**
+     * @brief   Does the peripheral have a service with the specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @return  bool    true - Found
+     *                  false- Not found
+     *
+     * @note  none
+     */
+    bool hasService(const char* uuid) const;
+    
+    /**
+     * @brief   Does the peripheral have an nth service with the specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @param   index   The index
+     *
+     * @return  bool    true - Found
+     *                  false- Not found
+     *
+     * @note  none
+     */
+    bool hasService(const char* uuid, int index) const;
+    
+    /**
+     * @brief   Return the nth service of the peripheral
+     *
+     * @param   index   The index
+     *
+     * @return  BLEService    The BLE service
+     *
+     * @note  none
+     */
+    BLEService service(int index) const;
+    
+    /**
+     * @brief   Return the service with the specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @return  BLEService    The BLE service
+     *
+     * @note  none
+     */
+    BLEService service(const char * uuid) const;
+    
+    /**
+     * @brief   Return the nth service with the specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @param   index   The index
+     *
+     * @return  BLEService    The BLE service
+     *
+     * @note  none
+     */
+    BLEService service(const char * uuid, int index) const;
+
+    /**
+     * @brief   Returns the number of characteristics the BLE device has
+     *
+     * @param   none
+     *
+     * @return  int     The number of characteristics
+     *
+     * @note  none
+     */
+    int characteristicCount() const;
+    
+    /**
+     * @brief   Does the device have a characteristic with the specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @return  bool    true - Found
+     *                  false- Not found
+     *
+     * @note  none
+     */
+    bool hasCharacteristic(const char* uuid) const;
+    
+    /**
+     * @brief   Does the device have an nth characteristic with the 
+     *           specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @param   index   The index
+     *
+     * @return  bool    true - Found
+     *                  false- Not found
+     *
+     * @note  none
+     */
+    bool hasCharacteristic(const char* uuid, int index) const;
+    
+    /**
+     * @brief   Return the nth characteristic of the BLE device
+     *
+     * @param   index   The index
+     *
+     * @return  BLECharacteristic   The BLE characteristic
+     *
+     * @note  none
+     */
+    BLECharacteristic characteristic(int index) const;
+    
+    /**
+     * @brief   Return the characteristic with the specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @return  BLECharacteristic   The BLE characteristic
+     *
+     * @note  none
+     */
+    BLECharacteristic characteristic(const char * uuid) const;
+    
+    /**
+     * @brief   Return the nth characteristic with the specified UUID
+     *
+     * @param   uuid    The 128/16 bits UUID
+     *
+     * @param   index   The index
+     *
+     * @return  BLECharacteristic   The BLE characteristic
+     *
+     * @note  none
+     */
+    BLECharacteristic characteristic(const char * uuid, int index) const;
+
+    // event handler
+    /**
+     * @brief   Set the event callbacks
+     *
+     * @param   event           The BLE device event
+     *
+     * @param   eventHandler    The BLE device event handler
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void setEventHandler(BLEDeviceEvent event, BLEDeviceEventHandler eventHandler); // set an event handler (callback)
+    
+protected:
+    friend class BLEDescriptorImp;
+    friend class BLECharacteristicImp;
+    friend class BLEServiceImp;
+    friend class BLEDeviceManager;
+    friend class BLEProfileManager;
+    friend class BLECharacteristic;
+    friend class BLEDescriptor;
+    friend class BLEService;
+    friend uint8_t profile_notify_process (bt_conn_t *conn,
+                                            bt_gatt_subscribe_params_t *params,
+                                            const void *data, uint16_t length);
+    friend uint8_t profile_read_rsp_process(bt_conn_t *conn, 
+                                             int err,
+                                             bt_gatt_read_params_t *params,
+                                             const void *data, 
+                                             uint16_t length);
+    friend uint8_t profile_descriptor_read_rsp_process(bt_conn_t *conn, 
+                                            int err,
+                                            bt_gatt_read_params_t *params,
+                                            const void *data, 
+                                            uint16_t length);
+    const bt_addr_le_t* bt_le_address() const;
+    const bt_le_conn_param* bt_conn_param() const;
+    void setAddress(const bt_addr_le_t& addr);
+    
+    void setAdvertiseData(const uint8_t* adv_data, uint8_t len);
+private:
+    void preCheckProfile();
+    
+    /**
+     * @brief   Start scanning for peripherals with the option of accepting all
+     *          detectable Peripherals or just the newly detected.
+     *
+     * @param[in] withDuplicates        true - return all detectable Peripherals.
+     *                                  false- return a detected Peripheral only once.
+     *
+     * @return  none
+     *
+     * @note  When, withDuplicates = true, accept all detectable Peripherals.
+     */
+    bool startScan(bool withDuplicates);
+    
+private:
+    bt_addr_le_t _bt_addr;
+    
+    bt_le_conn_param_t _conn_param;
+};
+
+#endif
diff --git a/libraries/CurieBLE/src/BLEPeripheral.cpp b/libraries/CurieBLE/src/BLEPeripheral.cpp
index f1218bcd..2bcee466 100644
--- a/libraries/CurieBLE/src/BLEPeripheral.cpp
+++ b/libraries/CurieBLE/src/BLEPeripheral.cpp
@@ -1,477 +1,161 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
+  BLE Peripheral API (deprecated)
+  Copyright (c) 2016 Arduino LLC. All right reserved.
 
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
 
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
 
-#include "BLEPeripheral.h"
-
-#include "BLECharacteristic.h"
-#include "BLEDescriptor.h"
-#include "BLEService.h"
-#include "BLEUuid.h"
-
-
-#define BLE_DISCONNECT_REASON_LOCAL_TERMINATION 0x16
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
 
-void
-blePeripheralGapEventHandler(ble_client_gap_event_t event, struct ble_gap_event *event_data, void *param)
-{
-    BLEPeripheral* p = (BLEPeripheral*)param;
-
-    p->handleGapEvent(event, event_data);
-}
+#include "CurieBLE.h"
 
-void
-blePeripheralGattsEventHandler(ble_client_gatts_event_t event, struct ble_gatts_evt_msg *event_data, void *param)
-{
-    BLEPeripheral* p = (BLEPeripheral*)param;
-
-    p->handleGattsEvent(event, event_data);
-}
-
-BLEPeripheral::BLEPeripheral(void) :
-    _state(BLE_PERIPH_STATE_NOT_READY),
-    _advertise_service_uuid(NULL),
-    _local_name(NULL),
-    _service_data_uuid(NULL),
-    _service_data(NULL),
-    _service_data_length(0),
-    _appearance(0),
-    _min_conn_interval(DEFAULT_MIN_CONN_INTERVAL),
-    _max_conn_interval(DEFAULT_MAX_CONN_INTERVAL),
-    _central(this),
-    _attributes(NULL),
-    _num_attributes(0),
-    _last_added_characteritic(NULL)
-{
-    memset(_event_handlers, 0x00, sizeof(_event_handlers));
+#include "BLEPeripheral.h"
 
-    ble_client_get_factory_config(&_local_bda, _device_name);
-}
+static BLEPeripheralEventHandler m_eventHandlers[BLEDeviceLastEvent];
 
-BLEPeripheral::~BLEPeripheral(void)
+void bleBackCompatiblePeripheralConnectHandler(BLEDevice central) 
 {
-    if (this->_attributes) {
-        free(this->_attributes);
+    if (m_eventHandlers[BLEConnected])
+    {
+        BLECentral temp(central);
+        m_eventHandlers[BLEConnected](temp);
     }
 }
 
-bool BLEPeripheral::begin()
+void bleBackCompatiblePeripheralDisconnectHandler(BLEDevice central) 
 {
-    BleStatus status;
-
-    status = _init();
-    if (status != BLE_STATUS_SUCCESS) {
-        return false;
-    }
-
-    /* Populate advertising data
-     */
-    _advDataInit();
-
-    status = ble_client_gap_wr_adv_data(_adv_data, _adv_data_len);
-    if (BLE_STATUS_SUCCESS != status) {
-        return false;
+    if (m_eventHandlers[BLEDisconnected])
+    {
+        BLECentral temp(central);
+        m_eventHandlers[BLEDisconnected](temp);
     }
-
-    uint16_t lastServiceHandle = 0;
-
-    for (int i = 0; i < _num_attributes; i++) {
-        BLEAttribute* attribute = _attributes[i];
-        BLEAttributeType type = attribute->type();
-        bool addResult = false;
-
-        if (BLETypeService == type) {
-            BLEService* service = (BLEService*)attribute;
-
-            addResult = service->add();
-
-            lastServiceHandle = service->handle();
-        } else if (BLETypeCharacteristic == type) {
-            BLECharacteristic* characteristic = (BLECharacteristic*)attribute;
-
-            addResult = characteristic->add(lastServiceHandle);
-        } else if (BLETypeDescriptor == type) {
-            BLEDescriptor *descriptor = (BLEDescriptor*)attribute;
-
-            if (strcmp(descriptor->uuid(), "2901") == 0 ||
-                strcmp(descriptor->uuid(), "2902") == 0 ||
-                strcmp(descriptor->uuid(), "2903") == 0 ||
-                strcmp(descriptor->uuid(), "2904") == 0) {
-                continue; // skip
-            }
-
-            addResult = descriptor->add(lastServiceHandle);
-        }
-
-        if (!addResult) {
-            return false;
-        }
-    }
-
-    return (_startAdvertising() == BLE_STATUS_SUCCESS);
 }
 
-void
-BLEPeripheral::poll()
-{
-    // no-op for now
-    delay(1);
-}
 
-void
-BLEPeripheral::end()
+BLEPeripheral::BLEPeripheral(void) :
+  _initCalled(false),
+  _lastService(NULL),
+  _lastCharacteristic(NULL)
 {
-    _stop();
 }
 
-uint8_t
-BLEPeripheral::getAdvertisingLength()
+BLEPeripheral::~BLEPeripheral(void)
 {
-    return _adv_data_len;
 }
 
-uint8_t*
-BLEPeripheral::getAdvertising()
+void BLEPeripheral::setAdvertisedServiceUuid(const char* advertisedServiceUuid)
 {
-    return _adv_data;
+    BLE.setAdvertisedServiceUuid(advertisedServiceUuid);
 }
 
-void
-BLEPeripheral::setAdvertisedServiceUuid(const char* advertisedServiceUuid)
+void BLEPeripheral::setLocalName(const char* localName)
 {
-    _advertise_service_uuid = advertisedServiceUuid;
+    BLE.setLocalName(localName);
 }
 
-void
-BLEPeripheral::setLocalName(const char* localName)
+void BLEPeripheral::setDeviceName(const char *deviceName)
 {
-    _local_name = localName;
+    BLE.setDeviceName(deviceName);
 }
 
-void
-BLEPeripheral::setAdvertisedServiceData(const char* serviceDataUuid, uint8_t* serviceData, uint8_t serviceDataLength)
+void BLEPeripheral::setAppearance(const unsigned short appearance)
 {
-    _service_data_uuid = serviceDataUuid;
-    _service_data = serviceData;
-    _service_data_length = serviceDataLength;
+    BLE.setAppearance(appearance);
 }
 
-void
-BLEPeripheral::setDeviceName(const char deviceName[])
+void BLEPeripheral::setConnectionInterval(const unsigned short minConnInterval, const unsigned short maxConnInterval)
 {
-    memset(_device_name, 0, sizeof(_device_name));
-    if (deviceName && deviceName[0]) {
-        int len = strlen(deviceName);
-        if (len > BLE_MAX_DEVICE_NAME)
-            len = BLE_MAX_DEVICE_NAME;
-        memcpy(_device_name, deviceName, len);
-    }
+    BLE.setConnectionInterval(minConnInterval, maxConnInterval);
 }
 
-void
-BLEPeripheral::setAppearance(const uint16_t appearance)
+void BLEPeripheral::addAttribute(BLEService& service)
 {
-    _appearance = appearance;
+    BLE.addService(service);
+    _lastService = &service;
 }
 
-void
-BLEPeripheral::setConnectionInterval(const unsigned short minConnInterval, const unsigned short maxConnInterval)
+void BLEPeripheral::addAttribute(BLECharacteristic& characteristic)
 {
-    _min_conn_interval = minConnInterval;
-    _max_conn_interval = maxConnInterval;
-
-    if (_min_conn_interval < MIN_CONN_INTERVAL) {
-        _min_conn_interval = MIN_CONN_INTERVAL;
-    } else if (_min_conn_interval > MAX_CONN_INTERVAL) {
-        _min_conn_interval = MAX_CONN_INTERVAL;
+    if (_lastService)
+    {
+        _lastService->addCharacteristic(characteristic);
+        _lastCharacteristic = &characteristic;
     }
-
-    if (_max_conn_interval < _min_conn_interval) {
-        _max_conn_interval = _min_conn_interval;
-    } else if (_max_conn_interval > MAX_CONN_INTERVAL) {
-        _max_conn_interval = MAX_CONN_INTERVAL;
-    }
-}
-
-void
-BLEPeripheral::setEventHandler(BLEPeripheralEvent event, BLEPeripheralEventHandler callback)
-{
-  if (event < sizeof(_event_handlers)) {
-    _event_handlers[event] = callback;
-  }
 }
 
-void
-BLEPeripheral::addAttribute(BLEAttribute& attribute)
+void BLEPeripheral::addAttribute(BLEDescriptor& descriptor)
 {
-    if (_attributes == NULL) {
-        _attributes = (BLEAttribute**)malloc(BLEAttribute::numAttributes() * sizeof(BLEAttribute*));
-    }
-
-    _attributes[_num_attributes] = &attribute;
-    _num_attributes++;
-
-    BLEAttributeType type = attribute.type();
-
-    if (BLETypeCharacteristic == type) {
-        _last_added_characteritic = (BLECharacteristic*)&attribute;
-    } else if (BLETypeDescriptor == type) {
-        if (_last_added_characteritic) {
-            BLEDescriptor* descriptor = (BLEDescriptor*)&attribute;
-
-            if (strcmp("2901", descriptor->uuid()) == 0) {
-                _last_added_characteritic->setUserDescription(descriptor);
-            } else if (strcmp("2904", descriptor->uuid()) == 0) {
-                _last_added_characteritic->setPresentationFormat(descriptor);
-            }
-        }
+    if (_lastCharacteristic)
+    {
+        _lastCharacteristic->addDescriptor(descriptor);
     }
 }
 
-bool
-BLEPeripheral::disconnect()
+void BLEPeripheral::setEventHandler(BLEPeripheralEvent event, BLEPeripheralEventHandler callback)
 {
-    BleStatus status;
-
-    if (BLE_PERIPH_STATE_CONNECTED == _state) {
-        status = ble_client_gap_disconnect(BLE_DISCONNECT_REASON_LOCAL_TERMINATION);
-    } else {
-        status = BLE_STATUS_WRONG_STATE;
+    if (BLEConnected == event || BLEDisconnected == event)
+    {
+        m_eventHandlers[event] = callback;
     }
-
-    return (status == BLE_STATUS_SUCCESS);
 }
 
-BLECentral
-BLEPeripheral::central()
+bool BLEPeripheral::begin(void)
 {
-    poll();
+    if (!_initCalled)
+    {
+        init();
+    }
+    
+    BLE.setEventHandler(BLEDisconnected, bleBackCompatiblePeripheralDisconnectHandler);
+    BLE.setEventHandler(BLEConnected, bleBackCompatiblePeripheralConnectHandler);
 
-    return _central;
+    BLE.advertise();
+    return true;
 }
 
-bool
-BLEPeripheral::connected()
+void BLEPeripheral::poll(void)
 {
-    poll();
-
-    return _central;
+    BLE.poll();
 }
 
-BleStatus
-BLEPeripheral::_init()
+void BLEPeripheral::end(void)
 {
-    BleStatus status;
-    int8_t txPower = 127;
-
-    if (BLE_PERIPH_STATE_NOT_READY != _state)
-        return BLE_STATUS_WRONG_STATE;
-
-    status = ble_client_init(blePeripheralGapEventHandler, this,
-                             blePeripheralGattsEventHandler, this);
-    if (BLE_STATUS_SUCCESS != status) {
-        return status;
-    }
-
-    status = ble_client_gap_set_enable_config(_device_name, &_local_bda, _appearance, txPower, _min_conn_interval, _max_conn_interval);
-    if (BLE_STATUS_SUCCESS != status) {
-        return status;
-    }
-
-    _state = BLE_PERIPH_STATE_READY;
-    return BLE_STATUS_SUCCESS;
+    BLE.end();
 }
 
-void
-BLEPeripheral::_advDataInit(void)
+bool BLEPeripheral::disconnect(void)
 {
-    uint8_t *adv_tmp = _adv_data;
-
-    memset(_adv_data, 0, sizeof(_adv_data));
-
-    /* Add flags */
-    *adv_tmp++ = 2;
-    *adv_tmp++ = BLE_ADV_TYPE_FLAGS;
-    *adv_tmp++ = BLE_SVC_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;
-    _adv_data_len = 3;
-
-    if (_advertise_service_uuid) {
-        BLEUuid bleUuid = BLEUuid(_advertise_service_uuid);
-        struct bt_uuid uuid = bleUuid.uuid();
-
-        if (BT_UUID16 == uuid.type) {
-            uint8_t *adv_tmp = &_adv_data[_adv_data_len];
-            *adv_tmp++ = (1 + sizeof(uint16_t)); /* Segment data length */
-            *adv_tmp++ = BLE_ADV_TYPE_COMP_16_UUID; /* Needed for Eddystone */
-            UINT16_TO_LESTREAM(adv_tmp, uuid.uuid16);
-            _adv_data_len += (2 + sizeof(uint16_t));
-        } else if (BT_UUID128 == uuid.type) {
-            uint8_t *adv_tmp = &_adv_data[_adv_data_len];
-            *adv_tmp++ = (1 + MAX_UUID_SIZE); /* Segment data length */
-            *adv_tmp++ = BLE_ADV_TYPE_INC_128_UUID;
-            memcpy(adv_tmp, uuid.uuid128, MAX_UUID_SIZE);
-            _adv_data_len += (2 + MAX_UUID_SIZE);
-        }
-    }
-
-    if (_local_name) {
-        /* Add device name (truncated if too long) */
-        uint8_t calculated_len;
-
-        adv_tmp = &_adv_data[_adv_data_len];
-        if (_adv_data_len + strlen(_local_name) + 2 <= BLE_MAX_ADV_SIZE) {
-            *adv_tmp++ = strlen(_local_name) + 1;
-            *adv_tmp++ = BLE_ADV_TYPE_COMP_LOCAL_NAME;
-            calculated_len = strlen(_local_name);
-        } else {
-            *adv_tmp++ = BLE_MAX_ADV_SIZE - _adv_data_len - 1;
-            *adv_tmp++ = BLE_ADV_TYPE_SHORT_LOCAL_NAME;
-            calculated_len = BLE_MAX_ADV_SIZE - _adv_data_len - 2;
-        }
-
-        memcpy(adv_tmp, _local_name, calculated_len);
-        _adv_data_len += calculated_len + 2;
-    }
-
-    if (_service_data) {
-        /* Add Service Data (if it will fit) */
-
-        BLEUuid bleUuid = BLEUuid(_service_data_uuid);
-        struct bt_uuid uuid = bleUuid.uuid();
-
-        /* A 128-bit Service Data UUID won't fit in an Advertising packet */
-        if (BT_UUID16 != uuid.type) {
-            return; /* We support service data only for 16-bit service UUID */
-        }
-
-        uint8_t block_len = 1 + sizeof(uint16_t) + _service_data_length;
-        if (_adv_data_len + 1 + block_len > BLE_MAX_ADV_SIZE) {
-            return; // Service data block is too large.
-        }
-
-        adv_tmp = &_adv_data[_adv_data_len];
-
-        *adv_tmp++ = block_len;
-        _adv_data_len++;
-
-        *adv_tmp++ = BLE_ADV_TYPE_SERVICE_DATA_16_UUID;
-        UINT16_TO_LESTREAM(adv_tmp, uuid.uuid16);
-        memcpy(adv_tmp, _service_data, _service_data_length);
-
-        _adv_data_len += block_len;
-    }
+    return BLE.disconnect();
 }
 
-BleStatus
-BLEPeripheral::_startAdvertising()
+BLECentral BLEPeripheral::central(void)
 {
-    BleStatus status;
-
-    if (_state != BLE_PERIPH_STATE_READY)
-        return BLE_STATUS_WRONG_STATE;
-
-    status = ble_client_gap_start_advertise(0); // 0 = no timeout
-    if (BLE_STATUS_SUCCESS != status)
-        return status;
-
-    _state = BLE_PERIPH_STATE_ADVERTISING;
-    return BLE_STATUS_SUCCESS;
+    BLEDevice centralBle = BLE.central();
+    return BLECentral(centralBle);
 }
 
-BleStatus
-BLEPeripheral::_stop(void)
+bool BLEPeripheral::connected(void)
 {
-    BleStatus status;
-
-    if (BLE_PERIPH_STATE_ADVERTISING == _state)
-        status = ble_client_gap_stop_advertise();
-    else
-        status = disconnect();
-
-    if (BLE_STATUS_SUCCESS != status)
-        return status;
-
-    _state = BLE_PERIPH_STATE_READY;
-    return BLE_STATUS_SUCCESS;
+    BLEDevice centralBle = BLE.central();
+    return centralBle.connected();
 }
 
-void
-BLEPeripheral::handleGapEvent(ble_client_gap_event_t event, struct ble_gap_event *event_data)
+void BLEPeripheral::init()
 {
-    if (BLE_CLIENT_GAP_EVENT_CONNECTED == event) {
-        _state = BLE_PERIPH_STATE_CONNECTED;
-        _central.setAddress(event_data->connected.peer_bda);
-
-        if (_event_handlers[BLEConnected]) {
-            _event_handlers[BLEConnected](_central);
-        }
-    } else if (BLE_CLIENT_GAP_EVENT_DISCONNECTED == event) {
-
-        for (int i = 0; i < _num_attributes; i++) {
-            BLEAttribute* attribute = _attributes[i];
-
-            if (attribute->type() == BLETypeCharacteristic) {
-                BLECharacteristic* characteristic = (BLECharacteristic*)attribute;
-
-                characteristic->setCccdValue(_central, 0x0000); // reset CCCD
-            }
-        }
-
-        if (_event_handlers[BLEDisconnected])
-            _event_handlers[BLEDisconnected](_central);
-
-        _state = BLE_PERIPH_STATE_READY;
-        _central.clearAddress();
-
-        _startAdvertising();
-    } else if (BLE_CLIENT_GAP_EVENT_CONN_TIMEOUT == event) {
-        _state = BLE_PERIPH_STATE_READY;
-
-        _startAdvertising();
+    if (!_initCalled)
+    {
+        BLE.begin();
+        _initCalled = true;
     }
 }
 
-void
-BLEPeripheral::handleGattsEvent(ble_client_gatts_event_t event, struct ble_gatts_evt_msg *event_data)
-{
-    if (BLE_CLIENT_GATTS_EVENT_WRITE == event) {
-        uint16_t handle = event_data->wr.attr_handle;
-
-        for (int i = 0; i < _num_attributes; i++) {
-            BLEAttribute* attribute = _attributes[i];
-
-            if (attribute->type() != BLETypeCharacteristic) {
-                continue;
-            }
-
-            BLECharacteristic* characteristic = (BLECharacteristic*)attribute;
-
-            if (characteristic->valueHandle() == handle) {
-                characteristic->setValue(_central, event_data->wr.data, event_data->wr.len);
-                break;
-            } else if (characteristic->cccdHandle() == handle) {
-                uint16_t cccdValue = 0;
-
-                memcpy(&cccdValue, event_data->wr.data, event_data->wr.len);
-
-                characteristic->setCccdValue(_central, cccdValue);
-                break;
-            }
-        }
-    }
-}
diff --git a/libraries/CurieBLE/src/BLEPeripheral.h b/libraries/CurieBLE/src/BLEPeripheral.h
index 054af330..f099efa5 100644
--- a/libraries/CurieBLE/src/BLEPeripheral.h
+++ b/libraries/CurieBLE/src/BLEPeripheral.h
@@ -1,258 +1,69 @@
-/*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-#ifndef _BLE_PERIPHERAL_H_INCLUDED
-#define _BLE_PERIPHERAL_H_INCLUDED
-
-#include "internal/ble_client.h"
-
-#include "BLEAttribute.h"
-#include "BLECentral.h"
-#include "BLECharacteristic.h"
-#include "BLECommon.h"
-
-/**
- * BLE Peripheral Events
- */
-enum BLEPeripheralEvent {
-  BLEConnected = 0,
-  BLEDisconnected = 1,
-
-  BLEPeripheralEventLast = 2
-};
-
-/** Function prototype for BLE Peripheral Device event callback */
-typedef void (*BLEPeripheralEventHandler)(BLECentral &central);
-
-/**
- * BLE Peripheral
- */
-class BLEPeripheral {
-public:
-    /**
-     * Default Constructor for BLE Peripheral Device
-     */
-    BLEPeripheral(void);
-
-    /**
-     * Destructor for BLE Peripheral Device
-     */
-    virtual ~BLEPeripheral(void);
-
-    /**
-     * Return the number of bytes in the advertising block.
-     * Useful for debugging advertising problems.
-     *
-     * @note Call only after calling begin().
-     */
-    uint8_t getAdvertisingLength();
-
-    /**
-     * Returns a pointer to the advertising block
-     * of length getAdvertisingLength().
-     * Useful for debugging advertising problems.
-     *
-     * @note Call only after calling begin().
-     */
-    uint8_t* getAdvertising();
-
-    /**
-     * Set the service UUID that the BLE Peripheral Device advertises
-     *
-     * @param advertisedServiceUuid  16-bit or 128-bit UUID to advertis
-     *                               (in string form)
-     *
-     * @note This method must be called before the begin method
-     */
-    void setAdvertisedServiceUuid(const char* advertisedServiceUuid);
-
-    /**
-     * Set the local name that the BLE Peripheral Device advertises
-     *
-     * @param localName  local name to advertise
-     *
-     * @note This method must be called before the begin method
-     */
-    void setLocalName(const char* localName);
-
-    /**
-     * Set the Service Data that the BLE Peripheral Device advertises
-     *
-     * @param serviceDataUuid 16-bit Service UUID for this Service Data
-     *   (in string form). Must match the UUID parameter
-     *   of setAdvertisedServiceUuid(). To fit into BLE_MAX_ADV_SIZE,
-     *   the UUID must be a 16-bit UUID.
-     *
-     * @param serviceData binary array of Service Data.
-     *
-     * @param serviceDataLength length (bytes) of serviceData[]
-     *
-     * @note the entire advertising packet must be no more than
-     *   BLE_MAX_ADV_SIZE bytes, which is currently 31.
-     *   This likely means that if you use Service Data
-     *   there will not be room for a Local Name.
-     *
-     * @note if serviceDataUuid isn't 16-bits long, or if
-     *   serviceDataLength won't fit in the advertising block,
-     *   the service data will silently not be copied
-     *   into the advertising block.
-     */
-    void setAdvertisedServiceData(const char* serviceDataUuid, uint8_t* serviceData, uint8_t serviceDataLength);
-
-    /**
-     * Set the device name for the BLE Peripheral Device
-     *
-     * If device name is not set, a default name will be used instead
-     *
-     * @param device  User-defined name string for this device.  Truncated if
-     *                   more than maximum allowed string length (20 bytes).
-     *
-     * @note This method must be called before the begin method
-     */
-    void setDeviceName(const char *deviceName);
-
-    /**
-     * Set the appearance type for the BLE Peripheral Device
-     *
-     * See https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.gap.appearance.xml
-     * for available options.
-     *
-     * @param appearance Appearance category identifier as defined by BLE Standard
-     *
-     * @return BleStatus indicating success or error
-     *
-     * @note This method must be called before the begin method
-     */
-    void setAppearance(const unsigned short appearance);
-
-    /**
-     * Set the min and max connection interval BLE Peripheral Device
-     *
-     * @param minConnInterval Minimum connection interval (1.25 ms units), minimum 0x0006 (7.5ms)
-     * @param maxConnInterval Maximum connection interval (1.25 ms units), maximum 0x095f (2998.75ms)
-     *
-     * @note This method must be called before the begin method
-     */
-    void setConnectionInterval(const unsigned short minConnInterval, const unsigned short maxConnInterval);
-
-    /**
-     * Add an attribute to the BLE Peripheral Device
-     *
-     * @param attribute Attribute to add to Peripheral
-     *
-     * @return BleStatus indicating success or error
-     *
-     * @note This method must be called before the begin method
-     */
-    void addAttribute(BLEAttribute& attribute);
-
-    /**
-     * Provide a function to be called when events related to this Device are raised
-     *
-     * @param event    Event type for callback
-     * @param callback Pointer to callback function to invoke when an event occurs.
-     */
-    void setEventHandler(BLEPeripheralEvent event, BLEPeripheralEventHandler callback);
-
-    /**
-     * Setup attributes and start advertising
-     *
-     * @return bool indicating success or error
-     */
-    bool begin(void);
-
-    /**
-     * Poll the peripheral for events
-     */
-    void poll(void);
-
-    /**
-     * Stop advertising and disconnect a central if connected
-     */
-    void end(void);
-
-    /**
-     * Disconnect the central connected if there is one connected
-     *
-     * @return bool indicating success or error
-     */
-    bool disconnect(void);
-
-    /**
-     * Setup attributes and start advertising
-     *
-     * @return BleStatus indicating success or error
-     */
-    BLECentral central(void);
-
-    /**
-     * Is a central connected?
-     *
-     * @return boolean_t true if central connected, otherwise false
-     */
-    bool connected(void);
-
-protected:
-    friend void blePeripheralGapEventHandler(ble_client_gap_event_t event, struct ble_gap_event *event_data, void *param);
-    friend void blePeripheralGattsEventHandler(ble_client_gatts_event_t event, struct ble_gatts_evt_msg *event_data, void *param);
-
-    void handleGapEvent(ble_client_gap_event_t event, struct ble_gap_event *event_data);
-    void handleGattsEvent(ble_client_gatts_event_t event, struct ble_gatts_evt_msg *event_data);
-
-private:
-    BleStatus _init(void);
-    BleStatus _startAdvertising(void);
-    BleStatus _stop(void);
-
-    void _advDataInit(void);
-
-private:
-
-    enum BLEPeripheralState {
-        BLE_PERIPH_STATE_NOT_READY = 0,
-        BLE_PERIPH_STATE_READY,
-        BLE_PERIPH_STATE_ADVERTISING,
-        BLE_PERIPH_STATE_CONNECTED,
-    };
-
-    BLEPeripheralState _state;
-
-    const char* _advertise_service_uuid;
-    const char* _local_name;
-    const char* _service_data_uuid;
-    uint8_t* _service_data;
-    uint8_t _service_data_length;
-    char       _device_name[BLE_MAX_DEVICE_NAME+1];
-    uint16_t   _appearance;
-    uint16_t   _min_conn_interval;
-    uint16_t   _max_conn_interval;
-    uint8_t    _adv_data[BLE_MAX_ADV_SIZE];
-    uint8_t    _adv_data_len;
-    ble_addr_t _local_bda;
-    BLECentral _central;
-
-    BLEPeripheralEventHandler _event_handlers[BLEPeripheralEventLast];
-
-    BLEAttribute** _attributes;
-    uint16_t _num_attributes;
-
-    BLECharacteristic* _last_added_characteritic;
-};
-
-#endif // _BLE_DEVICE_H_INCLUDED
+/*
+  BLE Peripheral API (deprecated)
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+// The API in this file is in DEPRECATED MODE, please DO NOT use it for Sketch construction
+#ifndef ARDUINO_BLE_PERIPHERAL_H
+#define ARDUINO_BLE_PERIPHERAL_H
+
+typedef void (*BLEPeripheralEventHandler)(BLECentral &central);
+
+typedef BLEDeviceEvent BLEPeripheralEvent;
+
+class BLEPeripheral {
+  public:
+    BLEPeripheral(void);
+    virtual ~BLEPeripheral(void);
+
+    void setAdvertisedServiceUuid(const char* advertisedServiceUuid); // set the advertised service uuid
+    void setLocalName(const char* localName); // set the local name
+
+
+    void setDeviceName(const char *deviceName); // set the device name    
+    void setAppearance(const unsigned short appearance); // set the appearance type
+
+    // Set the min and max connection interval
+    void setConnectionInterval(const unsigned short minConnInterval, const unsigned short maxConnInterval);
+
+    // Add an attribute to the BLE Peripheral Device
+    void addAttribute(BLEService& service);
+    void addAttribute(BLECharacteristic& characteristic);
+    void addAttribute(BLEDescriptor& descriptor);
+
+    void setEventHandler(BLEDeviceEvent event, BLEPeripheralEventHandler callback); // register an event handler
+
+    bool begin(void); // Setup attributes and start advertising
+
+    void poll(void); // poll the BLE radio for events
+
+    void end(void); // Stop advertising and disconnect a central if connected
+
+    bool disconnect(void); // disconnect the central if connected
+
+
+    BLECentral central(void);
+    bool connected(void); // Is a central connected?
+
+private:
+    void init();
+
+    bool _initCalled;
+    BLEService* _lastService;
+    BLECharacteristic* _lastCharacteristic;
+};
+
+#endif // ARDUINO_BLE_PERIPHERAL_H
diff --git a/libraries/CurieBLE/src/BLEService.cpp b/libraries/CurieBLE/src/BLEService.cpp
index f7569e3b..676cded1 100644
--- a/libraries/CurieBLE/src/BLEService.cpp
+++ b/libraries/CurieBLE/src/BLEService.cpp
@@ -1,40 +1,247 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-#include "internal/ble_client.h"
+  BLE Service API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
 
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
 #include "BLEService.h"
 
-BLEService::BLEService(const char* uuid) :
-    BLEAttribute(uuid, BLETypeService)
+#include "./internal/BLEProfileManager.h"
+#include "./internal/BLECharacteristicImp.h"
+
+#include "./internal/BLEUtils.h"
+
+BLEService::BLEService():_bledevice(),
+    _service_imp(NULL),
+    _service_local_imp(NULL)
+{
+    memset(_uuid_cstr, 0, sizeof (_uuid_cstr));
+}
+
+BLEService::BLEService(const char* uuid):_bledevice(),
+    _service_imp(NULL),
+    _service_local_imp(NULL)
+{
+    bt_uuid_128_t uuid_tmp;
+    memset(_uuid_cstr, 0, sizeof (_uuid_cstr));
+    
+    BLEUtils::uuidString2BT(uuid, (bt_uuid_t *)&uuid_tmp);
+    BLEUtils::uuidBT2String((const bt_uuid_t *)&uuid_tmp, _uuid_cstr);
+    
+    _bledevice.setAddress(*BLEUtils::bleGetLoalAddress());
+}
+
+BLEService::BLEService(const bt_uuid_t* uuid):_bledevice(),
+    _service_imp(NULL),
+    _service_local_imp(NULL)
+{
+    memset(_uuid_cstr, 0, sizeof (_uuid_cstr));
+    BLEUtils::uuidBT2String(uuid, _uuid_cstr);
+}
+
+BLEService::BLEService(BLEServiceImp* serviceImp, const BLEDevice* bledev):
+    _bledevice(bledev),_service_imp(serviceImp),
+    _service_local_imp(NULL)
+{
+    memset(_uuid_cstr, 0, sizeof (_uuid_cstr));
+    
+    BLEUtils::uuidBT2String(serviceImp->bt_uuid(), _uuid_cstr);
+}
+
+BLEService::~BLEService()
+{
+    if (NULL != _service_local_imp)
+    {
+        delete _service_local_imp;
+        _service_local_imp = NULL;
+    }
+}
+
+BLEService::operator bool() const
+{
+    return (strlen(_uuid_cstr) > 3);
+}
+
+BLEService& BLEService::operator= (const BLEService& service)
+{
+    memcpy(_uuid_cstr, service._uuid_cstr, sizeof(_uuid_cstr));
+    _bledevice.setAddress(*service._bledevice.bt_le_address());
+    _service_imp = service._service_imp;
+    _service_local_imp = NULL; // Not copy
+    return *this;
+}
+
+const char* BLEService::uuid() const
+{
+    return _uuid_cstr;
+}
+
+int BLEService::addCharacteristic(BLECharacteristic& characteristic)
+{
+    BLEServiceImp* serviceImp = getServiceImp();
+    int retVar = BLE_STATUS_ERROR;
+    
+    if (NULL != serviceImp)
+    {
+        retVar = serviceImp->addCharacteristic(_bledevice, characteristic);
+    }
+    else if (BLEUtils::isLocalBLE(_bledevice) == true)
+    {
+        // Only support the GATT server that create the service in local device.
+        _service_local_imp = new BLEServiceImp(*this);
+        if (NULL == _service_local_imp)
+        {
+            return BLE_STATUS_NO_MEMORY;
+        }
+        retVar = _service_local_imp->addCharacteristic(_bledevice, characteristic);
+    }
+    return retVar;
+}
+
+BLEServiceImp* BLEService::getLocalServiceImp()
+{
+    return _service_local_imp;
+}
+
+BLEServiceImp* BLEService::fetchOutLocalServiceImp()
+{
+    BLEServiceImp* temp = _service_local_imp;
+    _service_local_imp = NULL;
+    return temp;
+}
+
+int BLEService::characteristicCount() const
+{
+    int count = 0;
+    BLEServiceImp* serviceImp = getServiceImp();
+    if (NULL != serviceImp)
+    {
+        count = serviceImp->getCharacteristicCount();
+    }
+    return count;
+}
+
+bool BLEService::hasCharacteristic(const char* uuid) const
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceImp* serviceImp = getServiceImp();
+    if (NULL != serviceImp)
+    {
+        characteristicImp = serviceImp->characteristic(uuid);
+    }
+    return (NULL != characteristicImp);
+}
+
+bool BLEService::hasCharacteristic(const char* uuid, int index) const
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceImp* serviceImp = getServiceImp();
+    if (NULL != serviceImp)
+    {
+        characteristicImp = serviceImp->characteristic(index);
+        if (false == characteristicImp->compareUuid(uuid))
+        {
+            // UUID not align
+            characteristicImp = NULL;
+        }
+    }
+    return (NULL != characteristicImp);
+}
+
+BLECharacteristic BLEService::characteristic(int index) const
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceImp* serviceImp = getServiceImp();
+    if (NULL != serviceImp)
+    {
+        characteristicImp = serviceImp->characteristic(index);
+    }
+    if (NULL == characteristicImp)
+    {
+        BLECharacteristic temp;
+        return temp;
+    }
+    else
+    {
+        BLECharacteristic temp(characteristicImp, &_bledevice);
+        return temp;
+    }
+}
+
+BLECharacteristic BLEService::characteristic(const char * uuid) const
 {
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceImp* serviceImp = getServiceImp();
+    if (NULL != serviceImp)
+    {
+        characteristicImp = serviceImp->characteristic(uuid);
+    }
+    
+    if (NULL == characteristicImp)
+    {
+        BLECharacteristic temp;
+        return temp;
+    }
+    else
+    {
+        BLECharacteristic temp(characteristicImp, &_bledevice);
+        return temp;
+    }
 }
 
-bool
-BLEService::add() {
-    bt_uuid uuid = btUuid();
-    uint16_t handle = 0;
+BLECharacteristic BLEService::characteristic(const char * uuid, int index) const
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceImp* serviceImp = getServiceImp();
+    if (NULL != serviceImp)
+    {
+        characteristicImp = serviceImp->characteristic(index);
+        if (false == characteristicImp->compareUuid(uuid))
+        {
+            // UUID not align
+            characteristicImp = NULL;
+        }
+    }
+    if (NULL == characteristicImp)
+    {
+        BLECharacteristic temp;
+        return temp;
+    }
+    else
+    {
+        BLECharacteristic temp(characteristicImp, &_bledevice);
+        return temp;
+    }
+}
 
-    BleStatus status = ble_client_gatts_add_service(&uuid, BLE_GATT_SVC_PRIMARY, &handle);
-    if (BLE_STATUS_SUCCESS == status) {
-        setHandle(handle);
+BLEServiceImp*  BLEService::getServiceImp()
+{
+    if (NULL == _service_imp)
+    {
+        _service_imp = BLEProfileManager::instance()->service(_bledevice, uuid());
     }
+    return _service_imp;
+}
 
-    return (BLE_STATUS_SUCCESS == status);
+BLEServiceImp*  BLEService::getServiceImp() const
+{
+    return _service_imp;
+}
+
+void BLEService::setServiceImp(BLEServiceImp* serviceImp)
+{
+    _service_imp = serviceImp;
 }
+    
diff --git a/libraries/CurieBLE/src/BLEService.h b/libraries/CurieBLE/src/BLEService.h
index 17311f72..3d5785f3 100644
--- a/libraries/CurieBLE/src/BLEService.h
+++ b/libraries/CurieBLE/src/BLEService.h
@@ -1,44 +1,151 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-#ifndef _BLE_SERVICE_H_INCLUDED
-#define _BLE_SERVICE_H_INCLUDED
-
-#include "BLEAttribute.h"
-#include "BLECommon.h"
-
-/**
- * BLE GATT Service
- */
-class BLEService : public BLEAttribute {
+  BLE Service API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+#ifndef ARDUINO_BLE_SERVICE_H
+#define ARDUINO_BLE_SERVICE_H
+
+#include "CurieBLE.h"
+#include "BLEDevice.h"
+
+//class BLECharacteristic;
+class BLEServiceImp;
+
+class BLEService
+{
 public:
+    BLEService();
+    BLEService(const char* uuid);
+    virtual ~BLEService();
+    
+    virtual operator bool() const;  // is the service valid
+    BLEService& operator= (const BLEService& service);
+
+    const char* uuid() const;
+
     /**
-     * Constructor for BLE Service
+     * @brief   Add a characteristic in service
+     *
+     * @param   characteristic  The characteristic want to be added to service
+     *
+     * @return  int    0 - Success. Others - Error codes
      *
-     * @param uuid    16-bit or 128-bit UUID (in string form) defined by BLE standard
+     * @note  none
      */
-    BLEService(const char* uuid);
+    int addCharacteristic(BLECharacteristic& characteristic);
 
+    /**
+     * @brief   Get the number of characteristics the service has
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    int characteristicCount() const;
+    
+    /**
+     * @brief   Does the service have a characteristic with the specified UUID
+     *
+     * @param   uuid    The UUID of the characteristic
+     *
+     * @return  bool    true - Yes.     false - No
+     *
+     * @note  none
+     */
+    bool hasCharacteristic(const char* uuid) const;
+    
+    /**
+     * @brief   Does the service have an nth characteristic with the 
+     *           specified UUID
+     *
+     * @param   uuid    The UUID of the characteristic
+     *
+     * @param   index   The index of characteristic
+     *
+     * @return  bool    true - Yes.     false - No
+     *
+     * @note  none
+     */
+    bool hasCharacteristic(const char* uuid, int index) const;
+    
+    /**
+     * @brief   Return the nth characteristic of the service
+     *
+     * @param   index   The index of characteristic
+     *
+     * @return  BLECharacteristic   The characteristic
+     *
+     * @note  none
+     */
+    BLECharacteristic characteristic(int index) const;
+    
+    /**
+     * @brief   Return the characteristic with the specified UUID
+     *
+     * @param   uuid    The UUID of the characteristic
+     *
+     * @return  BLECharacteristic   The characteristic
+     *
+     * @note  none
+     */
+    BLECharacteristic characteristic(const char * uuid) const;
+    
+    /**
+     * @brief   return the nth characteristic with the specified UUID
+     *
+     * @param   uuid    The UUID of the characteristic
+     *
+     * @param   index   The index of characteristic
+     *
+     * @return  BLECharacteristic   The characteristic
+     *
+     * @note  none
+     */
+    BLECharacteristic characteristic(const char * uuid, int index) const;
+    
 protected:
-    friend BLEPeripheral;
+    friend class BLEDevice;
+    friend class BLEServiceImp;
+    friend class BLEProfileManager;
+    friend uint8_t profile_service_read_rsp_process(bt_conn_t *conn, 
+                                                    int err,
+                                                    bt_gatt_read_params_t *params,
+                                                    const void *data, 
+                                                    uint16_t length);
 
-    bool add(void);
+    BLEService(BLEServiceImp* serviceImp, const BLEDevice* bledev);
+    BLEService(const bt_uuid_t* uuid);
+    void setServiceImp(BLEServiceImp* serviceImp);
+    
+    BLEServiceImp* getLocalServiceImp();
+    BLEServiceImp* fetchOutLocalServiceImp();
+private:
+    BLEServiceImp* getServiceImp();
+    BLEServiceImp* getServiceImp() const;
+    
+private:
+    BLEDevice _bledevice;
+    BLEServiceImp* _service_imp;
+    char    _uuid_cstr[37];
+    
+    BLEServiceImp* _service_local_imp; // This not allow copy
 };
 
-#endif // _BLE_SERVICE_H_INCLUDED
+#endif
diff --git a/libraries/CurieBLE/src/BLETypedCharacteristic.h b/libraries/CurieBLE/src/BLETypedCharacteristic.h
index 77828aae..cd519ca6 100644
--- a/libraries/CurieBLE/src/BLETypedCharacteristic.h
+++ b/libraries/CurieBLE/src/BLETypedCharacteristic.h
@@ -20,25 +20,116 @@
 #ifndef _BLE_TYPED_CHARACTERISTIC_H_INCLUDED
 #define _BLE_TYPED_CHARACTERISTIC_H_INCLUDED
 
-#include "Arduino.h"
+#include "CurieBLE.h"
 
 #include "BLECharacteristic.h"
 
 template<typename T> class BLETypedCharacteristic : public BLECharacteristic
 {
 public:
+    /**
+     * @brief   The constructor of the template BLE Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLETypedCharacteristic(const char* uuid, unsigned char properties);
 
+    /**
+     * @brief   Set the characteristic value
+     *
+     * @param[in]   value   New value to set
+     *
+     * @return      bool    true - set value success, 
+     *                      false - on error
+     *
+     * @note  none
+     */
     bool setValue(T value);
+    
+    /**
+     * @brief   Update the characteristic value
+     *
+     * @param[in]   value   New value to set
+     *
+     * @return      bool    true - set value success, 
+     *                      false - on error
+     *
+     * @note  none
+     */
+    bool writeValue(T value);
+    
+    /**
+     * @brief   Get the value of the Characteristic
+     *
+     * @param   none
+     *
+     * @return  T       The value of characteristic
+     *
+     * @note  none
+     */
     T value(void);
 
+    /**
+     * @brief   Set the characteristic value in Little Endian
+     *
+     * @param[in]   value   New value to set
+     *
+     * @return      bool    true - set value success, 
+     *                      false - on error
+     *
+     * @note  none
+     */
     bool setValueLE(T value);
+    /**
+     * @brief   Get the value of the Characteristic in Little Endian
+     *
+     * @param   none
+     *
+     * @return  T       The value of characteristic
+     *
+     * @note  none
+     */
     T valueLE(void);
 
+    /**
+     * @brief   Set the characteristic value in Big Endian
+     *
+     * @param[in]   value   New value to set
+     *
+     * @return      bool    true - set value success, 
+     *                      false - on error
+     *
+     * @note  none
+     */
     bool setValueBE(T value);
+    /**
+     * @brief   Get the value of the Characteristic in Big Endian
+     *
+     * @param   none
+     *
+     * @return  T       The value of characteristic
+     *
+     * @note  none
+     */
     T valueBE(void);
 
 private:
+    /**
+     * @brief   Swap the bytes
+     *
+     * @param   value   The typed value
+     *
+     * @return  T       The swapped value
+     *
+     * @note  none
+     */
     T byteSwap(T value);
 };
 
@@ -55,6 +146,10 @@ template<typename T> bool BLETypedCharacteristic<T>::setValue(T value) {
     return BLECharacteristic::setValue((unsigned char*)&value, sizeof(T));
 }
 
+template<typename T> bool BLETypedCharacteristic<T>::writeValue(T value) {
+    return BLECharacteristic::writeValue((unsigned char*)&value, sizeof(T));
+}
+
 template<typename T> T BLETypedCharacteristic<T>::value() {
     T value;
 
diff --git a/libraries/CurieBLE/src/BLETypedCharacteristics.cpp b/libraries/CurieBLE/src/BLETypedCharacteristics.cpp
index c9c89e24..94bbb6ad 100644
--- a/libraries/CurieBLE/src/BLETypedCharacteristics.cpp
+++ b/libraries/CurieBLE/src/BLETypedCharacteristics.cpp
@@ -19,6 +19,10 @@
 
 #include "BLETypedCharacteristics.h"
 
+BLEByteCharacteristic::BLEByteCharacteristic(const char* uuid, unsigned char properties) :
+    BLETypedCharacteristic<byte>(uuid, properties) {
+}
+
 BLECharCharacteristic::BLECharCharacteristic(const char* uuid, unsigned char properties) :
     BLETypedCharacteristic<char>(uuid, properties) {
 }
diff --git a/libraries/CurieBLE/src/BLETypedCharacteristics.h b/libraries/CurieBLE/src/BLETypedCharacteristics.h
index fb76cfce..fc62d70c 100644
--- a/libraries/CurieBLE/src/BLETypedCharacteristics.h
+++ b/libraries/CurieBLE/src/BLETypedCharacteristics.h
@@ -24,56 +24,217 @@
 
 class BLEBoolCharacteristic : public BLETypedCharacteristic<bool> {
 public:
+    /**
+     * @brief   Instantiate a bool Typed Characteristic. 
+     *           Default constructor for BLE bool Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEBoolCharacteristic(const char* uuid, unsigned char properties);
 };
 
+class BLEByteCharacteristic : public BLETypedCharacteristic<byte> {
+public:
+    /**
+     * @brief   Instantiate a Byte Typed Characteristic. 
+     *           Default constructor for BLE Byte Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLEByteCharacteristic(const char* uuid, unsigned char properties);
+};
+
 class BLECharCharacteristic : public BLETypedCharacteristic<char> {
 public:
+    /**
+     * @brief   Instantiate a Char Typed Characteristic. 
+     *           Default constructor for BLE Char Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLECharCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEUnsignedCharCharacteristic : public BLETypedCharacteristic<unsigned char> {
 public:
+    /**
+     * @brief   Instantiate a Unsigned Char Typed Characteristic. 
+     *           Default constructor for BLE Unsigned Char Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEUnsignedCharCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEShortCharacteristic : public BLETypedCharacteristic<short> {
 public:
+    /**
+     * @brief   Instantiate a Short Typed Characteristic. 
+     *           Default constructor for BLE short Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEShortCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEUnsignedShortCharacteristic : public BLETypedCharacteristic<unsigned short> {
 public:
+    /**
+     * @brief   Instantiate a Unsigned Short Typed Characteristic. 
+     *           Default constructor for BLE Unsigned Short Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEUnsignedShortCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEIntCharacteristic : public BLETypedCharacteristic<int> {
 public:
+    /**
+     * @brief   Instantiate a Int Typed Characteristic. 
+     *           Default constructor for BLE Int Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEIntCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEUnsignedIntCharacteristic : public BLETypedCharacteristic<unsigned int> {
 public:
+    /**
+     * @brief   Instantiate a Unsigned Int Typed Characteristic. 
+     *           Default constructor for BLE Unsigned Int Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEUnsignedIntCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLELongCharacteristic : public BLETypedCharacteristic<long> {
 public:
+    /**
+     * @brief   Instantiate a Long Typed Characteristic. 
+     *           Default constructor for BLE Long Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLELongCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEUnsignedLongCharacteristic : public BLETypedCharacteristic<unsigned long> {
 public:
+    /**
+     * @brief   Instantiate a Unsigned Long Typed Characteristic. 
+     *           Default constructor for BLE Unsigned Long Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEUnsignedLongCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEFloatCharacteristic : public BLETypedCharacteristic<float> {
 public:
+    /**
+     * @brief   Instantiate a Float Typed Characteristic. 
+     *           Default constructor for BLE Float Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEFloatCharacteristic(const char* uuid, unsigned char properties);
 };
 
 class BLEDoubleCharacteristic : public BLETypedCharacteristic<double> {
 public:
+    /**
+     * @brief   Instantiate a Double Typed Characteristic. 
+     *           Default constructor for BLE Double Characteristic
+     *
+     * @param[in]   uuid        The characteristic UUID 16/128 bits
+     *
+     * @param[in]   properties  The property of the characteristic (BLERead, 
+     *                           BLEWrite or BLE Notify. Combine with | )
+     *
+     * @return  none
+     *
+     * @note  none
+     */
     BLEDoubleCharacteristic(const char* uuid, unsigned char properties);
 };
 
diff --git a/libraries/CurieBLE/src/BLEUuid.h b/libraries/CurieBLE/src/BLEUuid.h
deleted file mode 100644
index 39b8aff5..00000000
--- a/libraries/CurieBLE/src/BLEUuid.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-#ifndef _BLE_UUID_H_INCLUDED
-#define _BLE_UUID_H_INCLUDED
-
-#include "BLECommon.h"
-
-class BLEUuid
-{
-public:
-    BLEUuid(const char * str);
-
-    bt_uuid uuid(void) const;
-
-private:
-    struct bt_uuid _uuid;
-};
-
-#endif // _BLE_UUID_H_INCLUDED
diff --git a/libraries/CurieBLE/src/CurieBLE.h b/libraries/CurieBLE/src/CurieBLE.h
index fb51dd58..cb1828da 100644
--- a/libraries/CurieBLE/src/CurieBLE.h
+++ b/libraries/CurieBLE/src/CurieBLE.h
@@ -1,25 +1,48 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
-
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * Lesser General Public License for more details.
-
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
+  BLE API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+#ifndef ARDUINO_BLE_H
+#define ARDUINO_BLE_H
+
+#define ARDUINO_BLE_API_VERSION 10000 // version 1.0.0
+
+class BLEDevice;
+class BLECentral;
+class BLECharacteristic;
+class BLEDescriptor;
+class BLEService;
+class BLECharacteristicImp;
+class BLEDescriptorImp;
 
-#include "BLECharacteristic.h"
 #include "BLECommon.h"
+
+#include "BLEDevice.h"
+#include "BLEAttributeWithValue.h"
+#include "BLECharacteristic.h"
 #include "BLEDescriptor.h"
 #include "BLEService.h"
-#include "BLEPeripheral.h"
+
 #include "BLETypedCharacteristics.h"
+
+#include "BLECentral.h"
+#include "BLEPeripheral.h"
+
+extern BLEDevice BLE;
+
+#endif
diff --git a/libraries/CurieBLE/src/BLEAttribute.cpp b/libraries/CurieBLE/src/internal/BLEAttribute.cpp
similarity index 53%
rename from libraries/CurieBLE/src/BLEAttribute.cpp
rename to libraries/CurieBLE/src/internal/BLEAttribute.cpp
index d6f46591..fbb33ed9 100644
--- a/libraries/CurieBLE/src/BLEAttribute.cpp
+++ b/libraries/CurieBLE/src/internal/BLEAttribute.cpp
@@ -17,49 +17,48 @@
  *
  */
 
+#include "CurieBLE.h"
 #include "BLEAttribute.h"
 
-#include "BLEUuid.h"
+#include "./internal/BLEUtils.h"
 
-unsigned char BLEAttribute::_numAttributes = 0;
 
-BLEAttribute::BLEAttribute(const char* uuid, enum BLEAttributeType type) :
-    _uuid(uuid),
-    _type(type),
-    _handle(0)
+BLEAttribute::BLEAttribute(const char* uuid, BLEAttributeType type) :
+    _type(type)
 {
-    _numAttributes++;
+    memset(&_uuid, 0, sizeof (_uuid));
+    BLEUtils::uuidString2BT(uuid, (bt_uuid_t*)&_uuid);
 }
 
-const char*
-BLEAttribute::uuid() const {
-    return _uuid;
+BLEAttribute::BLEAttribute(const bt_uuid_t* uuid, BLEAttributeType type) :
+    _type(type)
+{
+    memcpy(&_uuid, uuid, sizeof (_uuid));
 }
 
-enum BLEAttributeType
-BLEAttribute::type() const {
-    return this->_type;
+const bt_uuid_t *BLEAttribute::bt_uuid(void)
+{
+    return (bt_uuid_t *)&_uuid;
 }
 
-uint16_t
-BLEAttribute::handle() {
-    return _handle;
-}
 
-void
-BLEAttribute::setHandle(uint16_t handle) {
-    _handle = handle;
+BLEAttributeType
+BLEAttribute::type() const {
+    return this->_type;
 }
 
-
-bt_uuid
-BLEAttribute::btUuid() const {
-    BLEUuid bleUuid = BLEUuid(uuid());
+bool BLEAttribute::compareUuid(const bt_uuid_t* uuid)
+{
+    int cmpresult = 0;
+    cmpresult = bt_uuid_cmp(uuid, (const bt_uuid_t*)&_uuid);
+    return (cmpresult == 0);
     
-    return bleUuid.uuid();
 }
 
-unsigned char
-BLEAttribute::numAttributes() {
-    return _numAttributes;
+bool BLEAttribute::compareUuid(const char* uuid)
+{
+    bt_uuid_128_t temp;
+    BLEUtils::uuidString2BT(uuid,(bt_uuid_t *)&temp);
+    return compareUuid((bt_uuid_t *)&temp);
 }
+
diff --git a/libraries/CurieBLE/src/BLEAttribute.h b/libraries/CurieBLE/src/internal/BLEAttribute.h
similarity index 50%
rename from libraries/CurieBLE/src/BLEAttribute.h
rename to libraries/CurieBLE/src/internal/BLEAttribute.h
index 8aaca067..db69cf32 100644
--- a/libraries/CurieBLE/src/BLEAttribute.h
+++ b/libraries/CurieBLE/src/internal/BLEAttribute.h
@@ -22,42 +22,52 @@
 
 #include "BLECommon.h"
 
-enum BLEAttributeType {
-    BLETypeService        = 0x2800,
-    BLETypeCharacteristic = 0x2803,
-    BLETypeDescriptor     = 0x2900
-};
+/// BLE attribute tyep enum
+typedef enum  {
+    BLETypeService        = 0x2800, ///< the service type
+    BLETypeCharacteristic = 0x2803, ///< the characteristic type
+    BLETypeDescriptor     = 0x2900  ///< the descriptor type
+}BLEAttributeType;
 
-class BLEPeripheral;
 
 class BLEAttribute {
 public:
+    /**
+     * @brief   Get the UUID raw data
+     *
+     * @param   none
+     *
+     * @return  bt_uuid_t*     The pointer of UUID
+     *
+     * @note  none
+     */
+    const bt_uuid_t *bt_uuid(void);
     
     /**
-     * Get the string representation of the Attribute
+     * @brief   Compare the UUID with the paramater data
+     *
+     * @param[in]   data 		The pointer of data
+     *
+     * @param[in]  uuidsize     The max size of UUID
      *
-     * @return const char* string representation of the Attribute
+     * @return  bool    true - UUID is the same with data
+     *                  		false- UUID is not the same with data
+     *
+     * @note  none
      */
-    const char* uuid(void) const;
-
-protected:
-    friend BLEPeripheral;
-
-    BLEAttribute(const char* uuid, enum BLEAttributeType type);
-
+    bool compareUuid(const char* uuid);
+    bool compareUuid(const bt_uuid_t* uuid);
+    
     BLEAttributeType type(void) const;
-    bt_uuid btUuid(void) const;
-    uint16_t handle(void);
-    void setHandle(uint16_t handle);
-
-    static unsigned char numAttributes(void);
 
+protected:
+    BLEAttribute(const char* uuid, BLEAttributeType type);
+    BLEAttribute(const bt_uuid_t* uuid, BLEAttributeType type);
 private:
-    static unsigned char _numAttributes;
-
-    const char* _uuid;
-    enum BLEAttributeType _type;
-    uint16_t _handle;
+    bt_uuid_128_t _uuid;
+    
+    BLEAttributeType _type;
+    
 };
 
 #endif // _BLE_ATTRIBUTE_H_INCLUDED
diff --git a/libraries/CurieBLE/src/internal/BLECallbacks.cpp b/libraries/CurieBLE/src/internal/BLECallbacks.cpp
new file mode 100644
index 00000000..328c196b
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLECallbacks.cpp
@@ -0,0 +1,285 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+#include <errno.h>
+
+#include "CurieBLE.h"
+
+#include "BLEAttribute.h"
+#include "BLECharacteristicImp.h"
+#include "BLEDeviceManager.h"
+#include "BLEProfileManager.h"
+
+// GATT Server Only
+ssize_t profile_read_process(bt_conn_t *conn,
+                             const bt_gatt_attr_t *attr,
+                             void *buf, uint16_t len,
+                             uint16_t offset)
+{
+    const unsigned char *pvalue;
+    BLEAttribute *bleattr = (BLEAttribute *)attr->user_data;
+    BLEAttributeType type = bleattr->type();
+    if (BLETypeCharacteristic == type)
+    {
+        BLECharacteristicImp* blecharacteritic = (BLECharacteristicImp*)bleattr;
+        pvalue = blecharacteritic->value();
+        return bt_gatt_attr_read(conn, attr, buf, len, offset, pvalue,
+                                 blecharacteritic->valueLength());
+    }
+    else if (BLETypeDescriptor == type)
+    {
+        BLEDescriptorImp* bledescriptor = (BLEDescriptorImp*)bleattr;
+        pvalue = bledescriptor->value();
+        return bt_gatt_attr_read(conn, attr, buf, len, offset, pvalue, bledescriptor->valueLength());
+    }
+    return 0;
+}
+
+// GATT server only
+ssize_t profile_write_process(bt_conn_t *conn,
+                              const bt_gatt_attr_t *attr,
+                              const void *buf, uint16_t len,
+                              uint16_t offset)
+{
+    pr_info(LOG_MODULE_BLE, "%s1", __FUNCTION__);
+    BLEAttribute *bleattr = (BLEAttribute *)attr->user_data;
+    BLECharacteristicImp* blecharacteritic;
+    BLEAttributeType type = bleattr->type();
+    if ((BLETypeCharacteristic != type) || 0 != offset)
+    {
+        return 0;
+    }
+    
+    blecharacteritic = (BLECharacteristicImp*)bleattr;
+    blecharacteritic->setValue((const uint8_t *) buf, len);
+    return len;
+}
+
+ssize_t profile_longwrite_process(struct bt_conn *conn,
+                                     const struct bt_gatt_attr *attr,
+                                     const void *buf, uint16_t len,
+                                     uint16_t offset)
+{
+    BLEAttribute *bleattr = (BLEAttribute *)attr->user_data;
+    BLEAttributeType type = bleattr->type();
+    if (BLETypeCharacteristic != type)
+    {
+        return 0;
+    }
+    BLECharacteristicImp *blecharacteritic = (BLECharacteristicImp*)bleattr;
+    
+    blecharacteritic->setBuffer((const uint8_t *) buf, len, offset);
+    
+    return len;
+}
+
+int profile_longflush_process(struct bt_conn *conn,
+                              const struct bt_gatt_attr *attr, 
+                              uint8_t flags)
+{
+    BLEAttribute *bleattr = (BLEAttribute *)attr->user_data;
+    BLEAttributeType type = bleattr->type();
+    if (BLETypeCharacteristic != type)
+    {
+        return 0;
+    }
+    BLECharacteristicImp *blecharacteritic = (BLECharacteristicImp*)bleattr;
+
+    switch (flags)
+    {
+    case BT_GATT_FLUSH_DISCARD:
+        /* Discard buffer reseting it back with data */
+        blecharacteritic->discardBuffer();
+        return 0;
+    case BT_GATT_FLUSH_SYNC:
+        /* Sync buffer to data */
+        blecharacteritic->syncupBuffer2Value();
+        return 0;
+    }
+
+    return -EINVAL;
+}
+
+
+// GATT client only
+uint8_t profile_notify_process (bt_conn_t *conn,
+                                bt_gatt_subscribe_params_t *params,
+                                const void *data, uint16_t length)
+{
+    //BLEPeripheralHelper* peripheral = BLECentralRole::instance()->peripheral(conn);// Find peripheral by bt_conn
+    //BLEAttribute* notifyatt = peripheral->attribute(params); // Find attribute by params
+    BLECharacteristicImp* chrc = NULL;
+    BLEDevice bleDevice(bt_conn_get_dst(conn));
+    chrc = BLEProfileManager::instance()->characteristic(bleDevice, params->value_handle);
+    
+    //assert(notifyatt->type() == BLETypeCharacteristic);
+    pr_debug(LOG_MODULE_APP, "%s1", __FUNCTION__);
+    if (NULL != chrc)
+    {
+        chrc->setValue((const unsigned char *)data, length);
+    }
+    return BT_GATT_ITER_CONTINUE;
+}
+
+// GATT client only
+uint8_t profile_discover_process(bt_conn_t *conn,
+                                 const bt_gatt_attr_t *attr,
+                                 bt_gatt_discover_params_t *params)
+{
+    uint8_t ret = BT_GATT_ITER_STOP;
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    ret = BLEProfileManager::instance()->discoverResponseProc(conn, attr, params);
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    return ret;
+}
+
+// GATT Client only
+uint8_t profile_read_rsp_process(bt_conn_t *conn, 
+                                 int err,
+                                 bt_gatt_read_params_t *params,
+                                 const void *data, 
+                                 uint16_t length)
+{
+    if (NULL == data && 0 != length)
+    {
+        return BT_GATT_ITER_STOP;
+    }
+    BLECharacteristicImp *chrc = NULL;
+    BLEDevice bleDevice(bt_conn_get_dst(conn));
+    
+    // Get characteristic by handle params->single.handle
+    chrc = BLEProfileManager::instance()->characteristic(bleDevice, params->single.handle);
+    
+    if (chrc)  // KW issue: may be NULL and will be dereferenced
+      chrc->setValue((const unsigned char *)data, length);
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    return BT_GATT_ITER_STOP;
+}
+
+uint8_t profile_descriptor_read_rsp_process(bt_conn_t *conn, 
+                                            int err,
+                                            bt_gatt_read_params_t *params,
+                                            const void *data, 
+                                            uint16_t length)
+{
+    if (NULL == data)
+    {
+        return BT_GATT_ITER_STOP;
+    }
+    BLEDescriptorImp *descriptor = NULL;
+    BLEDevice bleDevice(bt_conn_get_dst(conn));
+    
+    // Get characteristic by handle params->single.handle
+    descriptor = BLEProfileManager::instance()->descriptor(bleDevice, params->single.handle);
+    
+    //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    if (descriptor)
+    {
+        descriptor->writeValue((const unsigned char *)data, length, params->single.offset);
+    }
+    //pr_debug(LOG_MODULE_BLE, "%s-%d: desc len-%d", __FUNCTION__, __LINE__, descriptor->valueLength());
+    return BT_GATT_ITER_STOP;
+}
+
+uint8_t profile_service_read_rsp_process(bt_conn_t *conn, 
+                                 int err,
+                                 bt_gatt_read_params_t *params,
+                                 const void *data, 
+                                 uint16_t length)
+{
+    uint8_t ret = BLEProfileManager::instance()->serviceReadRspProc(conn, err, params, data, length);
+    pr_debug(LOG_MODULE_BLE, "%s-%d:ret-%d", __FUNCTION__, __LINE__, ret);
+    return ret;
+}
+
+uint8_t profile_characteristic_read_rsp_process(bt_conn_t *conn, 
+                                                 int err,
+                                                 bt_gatt_read_params_t *params,
+                                                 const void *data, 
+                                                 uint16_t length)
+{
+    BLEDevice bleDevice(bt_conn_get_dst(conn));
+    BLEServiceImp* service_imp = BLEProfileManager::instance()->getServiceBySubHandle(bleDevice, params->single.handle);
+    
+    uint8_t ret = service_imp->characteristicReadRspProc(conn, 
+                                                         err,
+                                                         params,
+                                                         data,
+                                                         length);
+    pr_debug(LOG_MODULE_BLE, "%s-%d:ret-%d", __FUNCTION__, __LINE__, ret);
+    return ret;
+}
+
+
+void bleConnectEventHandler(bt_conn_t *conn, 
+                            uint8_t err, 
+                            void *param)
+{
+    BLEDeviceManager* p = (BLEDeviceManager*)param;
+
+    p->handleConnectEvent(conn, err);
+}
+
+
+void bleDisconnectEventHandler(bt_conn_t *conn, 
+                                uint8_t reason, 
+                                void *param)
+{
+    BLEDeviceManager* p = (BLEDeviceManager*)param;
+    
+    pr_info(LOG_MODULE_BLE, "Connect lost. Reason: %d", reason);
+
+    p->handleDisconnectEvent(conn, reason);
+}
+
+void bleParamUpdatedEventHandler(bt_conn_t *conn, 
+                                 uint16_t interval,
+                                 uint16_t latency, 
+                                 uint16_t timeout, 
+                                 void *param)
+{
+    BLEDeviceManager* p = (BLEDeviceManager*)param;
+
+    p->handleParamUpdated(conn, interval, latency, timeout);
+}
+
+
+void ble_central_device_found(const bt_addr_le_t *addr, 
+                              int8_t rssi, 
+                              uint8_t type,
+                              const uint8_t *ad, 
+                              uint8_t len)
+{
+    //char dev[BT_ADDR_LE_STR_LEN];
+
+    //bt_addr_le_to_str(addr, dev, sizeof(dev));
+    //pr_debug(LOG_MODULE_BLE, "[DEVICE]: %s, AD evt type %u, AD data len %u, RSSI %i\n",
+    //       dev, type, len, rssi);
+
+    BLEDeviceManager::instance()->handleDeviceFound(addr, rssi, type,
+                                                  ad, len);
+}
+
+void ble_on_write_no_rsp_complete(struct bt_conn *conn, uint8_t err,
+                                         const void *data)
+{
+    BLECharacteristicImp::writeResponseReceived(conn, err, data);
+}
+
diff --git a/libraries/CurieBLE/src/internal/BLECallbacks.h b/libraries/CurieBLE/src/internal/BLECallbacks.h
new file mode 100644
index 00000000..deffe92a
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLECallbacks.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+#ifndef __BLECALLBACKS_H__
+#define __BLECALLBACKS_H__
+
+uint8_t profile_notify_process (bt_conn_t *conn,
+                                bt_gatt_subscribe_params_t *params,
+                                const void *data, uint16_t length);
+uint8_t profile_read_rsp_process(bt_conn_t *conn, int err,
+                                 bt_gatt_read_params_t *params,
+                                 const void *data, 
+                                 uint16_t length);
+
+uint8_t profile_descriptor_read_rsp_process(bt_conn_t *conn, 
+                                            int err,
+                                            bt_gatt_read_params_t *params,
+                                            const void *data, 
+                                            uint16_t length);
+
+int profile_longflush_process(struct bt_conn *conn,
+                              const struct bt_gatt_attr *attr, 
+                              uint8_t flags);
+ssize_t profile_longwrite_process(struct bt_conn *conn,
+                                     const struct bt_gatt_attr *attr,
+                                     const void *buf, uint16_t len,
+                                     uint16_t offset);
+ssize_t profile_write_process(bt_conn_t *conn,
+                              const bt_gatt_attr_t *attr,
+                              const void *buf, uint16_t len,
+                              uint16_t offset);
+ssize_t profile_read_process(bt_conn_t *conn,
+                             const bt_gatt_attr_t *attr,
+                             void *buf, uint16_t len,
+                             uint16_t offset);
+
+uint8_t profile_discover_process(bt_conn_t *conn,
+                                 const bt_gatt_attr_t *attr,
+                                 bt_gatt_discover_params_t *params);
+
+void bleConnectEventHandler(bt_conn_t *conn, 
+                            uint8_t err, 
+                            void *param);
+
+void bleDisconnectEventHandler(bt_conn_t *conn, 
+                                uint8_t reason, 
+                                void *param);
+
+void bleParamUpdatedEventHandler(bt_conn_t *conn, 
+                                 uint16_t interval,
+                                 uint16_t latency, 
+                                 uint16_t timeout, 
+                                 void *param);
+
+void ble_central_device_found(const bt_addr_le_t *addr, 
+                              int8_t rssi, 
+                              uint8_t type,
+                              const uint8_t *ad, 
+                              uint8_t len);
+
+uint8_t profile_service_read_rsp_process(bt_conn_t *conn, 
+                                 int err,
+                                 bt_gatt_read_params_t *params,
+                                 const void *data, 
+                                 uint16_t length);
+
+void ble_on_write_no_rsp_complete(struct bt_conn *conn, uint8_t err,
+                                         const void *data);
+uint8_t profile_characteristic_read_rsp_process(bt_conn_t *conn, 
+                                                 int err,
+                                                 bt_gatt_read_params_t *params,
+                                                 const void *data, 
+                                                 uint16_t length);
+
+#endif
+
diff --git a/libraries/CurieBLE/src/internal/BLECharacteristicImp.cpp b/libraries/CurieBLE/src/internal/BLECharacteristicImp.cpp
new file mode 100644
index 00000000..a7098e96
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLECharacteristicImp.cpp
@@ -0,0 +1,1073 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+#include <errno.h>
+
+#include "BLEAttribute.h"
+#include "BLEServiceImp.h"
+#include "BLECharacteristicImp.h"
+
+#include "BLECallbacks.h"
+#include "BLEUtils.h"
+
+bt_uuid_16_t BLECharacteristicImp::_gatt_chrc_uuid = {BT_UUID_TYPE_16, BT_UUID_GATT_CHRC_VAL};
+bt_uuid_16_t BLECharacteristicImp::_gatt_ccc_uuid = {BT_UUID_TYPE_16, BT_UUID_GATT_CCC_VAL};
+volatile bool BLECharacteristicImp::_gattc_writing = false;
+
+BLECharacteristicImp::BLECharacteristicImp(const bt_uuid_t* uuid, 
+                                           unsigned char properties,
+                                           uint16_t handle,
+                                           const BLEDevice& bledevice):
+    BLEAttribute(uuid, BLETypeCharacteristic),
+    _value_length(0),
+    _value_buffer(NULL),
+    _value_updated(false),
+    _value_handle(handle),
+    _cccd_handle(0),
+    _attr_chrc_value(NULL),
+    _attr_cccd(NULL),
+    _subscribed(false),
+    _reading(false),
+    _ble_device()
+{
+    _value_size = BLE_MAX_ATTR_DATA_LEN;// Set as MAX value. TODO: long read/write need to twist
+    _value = (unsigned char*)malloc(_value_size);
+
+    // TODO: Enable when max value is not set.
+    //    if (_value_size > BLE_MAX_ATTR_DATA_LEN)
+    //    {
+    //        _value_buffer = (unsigned char*)malloc(_value_size);
+    //    }
+
+    if (_value)
+    {
+      memset(_value, 0, _value_size);
+    }
+    else
+    {
+        errno = ENOMEM;
+    }
+    
+    memset(&_ccc_cfg, 0, sizeof(_ccc_cfg));
+    memset(&_ccc_value, 0, sizeof(_ccc_value));
+    memset(&_gatt_chrc, 0, sizeof(_gatt_chrc));
+    memset(&_sub_params, 0, sizeof(_sub_params));
+    memset(&_discover_params, 0, sizeof(_discover_params));
+    
+    _ccc_value.cfg = &_ccc_cfg;
+    _ccc_value.cfg_len = 1;
+    if (BLERead & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_READ;
+    }
+    if (BLEWrite & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_WRITE;
+    }
+    if (BLEWriteWithoutResponse & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_WRITE_WITHOUT_RESP;
+    }
+    if (BLENotify & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_NOTIFY;
+        _sub_params.value |= BT_GATT_CCC_NOTIFY;
+    }
+    if (BLEIndicate & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_INDICATE;
+        _sub_params.value |= BT_GATT_CCC_INDICATE;
+    }
+    _gatt_chrc.uuid = (bt_uuid_t*)this->bt_uuid();//&_characteristic_uuid;//this->uuid();
+    memset(_event_handlers, 0, sizeof(_event_handlers));
+    memset(_oldevent_handlers, 0, sizeof(_oldevent_handlers));
+    
+    _sub_params.notify = profile_notify_process;
+        
+    // Update BLE device object
+    _ble_device.setAddress(*bledevice.bt_le_address());
+    
+    memset(&_descriptors_header, 0, sizeof(_descriptors_header));
+}
+
+BLECharacteristicImp::BLECharacteristicImp(BLECharacteristic& characteristic, 
+                                           const BLEDevice& bledevice):
+    BLEAttribute(characteristic.uuid(), BLETypeCharacteristic),
+    _value_length(0),
+    _value_buffer(NULL),
+    _value_updated(false),
+    _value_handle(0),
+    _cccd_handle(0),
+    _attr_chrc_value(NULL),
+    _attr_cccd(NULL),
+    _subscribed(false),
+    _reading(false),
+    _ble_device()
+{
+    unsigned char properties = characteristic._properties;
+    _value_size = characteristic._value_size;
+    _value = (unsigned char*)malloc(_value_size);
+    if (_value == NULL)
+    {
+        errno = ENOMEM;
+    }
+    if (_value_size > BLE_MAX_ATTR_DATA_LEN)
+    {
+        _value_buffer = (unsigned char*)malloc(_value_size);
+    }
+    
+    memset(&_ccc_cfg, 0, sizeof(_ccc_cfg));
+    memset(&_ccc_value, 0, sizeof(_ccc_value));
+    memset(&_gatt_chrc, 0, sizeof(_gatt_chrc));
+    memset(&_sub_params, 0, sizeof(_sub_params));
+    memset(&_discover_params, 0, sizeof(_discover_params));
+    
+    _ccc_value.cfg = &_ccc_cfg;
+    _ccc_value.cfg_len = 1;
+    if (BLERead & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_READ;
+    }
+    if (BLEWrite & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_WRITE;
+    }
+    if (BLEWriteWithoutResponse & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_WRITE_WITHOUT_RESP;
+    }
+    if (BLENotify & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_NOTIFY;
+        _sub_params.value |= BT_GATT_CCC_NOTIFY;
+    }
+    if (BLEIndicate & properties)
+    {
+        _gatt_chrc.properties |= BT_GATT_CHRC_INDICATE;
+        _sub_params.value |= BT_GATT_CCC_INDICATE;
+    }
+    _gatt_chrc.uuid = (bt_uuid_t*)this->bt_uuid();//&_characteristic_uuid;//this->uuid();
+
+    memcpy(_event_handlers, characteristic._event_handlers, sizeof(_event_handlers));
+    memcpy(_oldevent_handlers, characteristic._oldevent_handlers, sizeof(_oldevent_handlers));
+    
+    _sub_params.notify = profile_notify_process;
+    
+    if (NULL != characteristic._value)
+    {
+        memcpy(_value, characteristic._value, _value_size);
+        _value_length = _value_size;
+    }
+        
+    // Update BLE device object
+    _ble_device.setAddress(*bledevice.bt_le_address());
+    
+    characteristic.setBLECharacteristicImp(this);
+    memset(&_descriptors_header, 0, sizeof(_descriptors_header));
+}
+
+BLECharacteristicImp::~BLECharacteristicImp()
+{
+    releaseDescriptors();
+    if (_value)
+    {
+        free(_value);
+        _value = (unsigned char *)NULL;
+    }
+    
+    if (_value_buffer)
+    {
+        free(_value_buffer);
+        _value_buffer = (unsigned char *)NULL;
+    }
+}
+
+unsigned char
+BLECharacteristicImp::properties() const
+{
+    return _gatt_chrc.properties;
+}
+
+bool BLECharacteristicImp::writeValue(const byte value[], int length)
+{
+    int status;
+    bool retVal = false;
+    
+    _setValue(value, length, 0);
+    
+    // Address same is GATT server. Send notification if CCCD enabled
+    // Different is GATT client. Send write request
+    if (true == BLEUtils::isLocalBLE(_ble_device) &&
+        NULL != _attr_chrc_value)
+    {
+        // Notify for peripheral.
+        status = bt_gatt_notify(NULL, _attr_chrc_value, value, length, NULL);
+    // Sid.  KW found status is always 0
+    //        if (!status)
+    //        {
+            retVal = true;
+    //        }
+    }
+    
+    //Not schedule write request for central
+    // The write request may failed. 
+    // If user want to get latest set value. Call read and get the real value
+    return retVal;
+}
+
+bool BLECharacteristicImp::writeValue(const byte value[], int length, int offset)
+{
+    int status;
+    bool retVal = false;
+    
+    _setValue(value, length, offset);
+    
+    // Address same is GATT server. Send notification if CCCD enabled
+    // Different is GATT client. Send write request
+    if (true == BLEUtils::isLocalBLE(_ble_device) &&
+        NULL != _attr_chrc_value)
+    {
+        // Notify for peripheral.
+        status = bt_gatt_notify(NULL, _attr_chrc_value, value, length, NULL);
+	// Sid.  KW found status is always 0.
+	//        if (!status)
+	//        {
+            retVal = true;
+	//        }
+    }
+    
+    //Not schedule write request for central
+    // The write request may failed. 
+    // If user want to get latest set value. Call read and get the real value
+    return retVal;
+}
+
+bool
+BLECharacteristicImp::setValue(const unsigned char value[], uint16_t length)
+{
+    _setValue(value, length, 0);
+    _value_updated = true;
+    if (BLEUtils::isLocalBLE(_ble_device) == true)
+    {
+        // GATT server
+        // Write request for GATT server
+        if (_event_handlers[BLEWritten]) 
+        {
+            BLECharacteristic chrcTmp(this, &_ble_device);
+            _event_handlers[BLEWritten](_ble_device, chrcTmp);
+        }
+        
+        if (_oldevent_handlers[BLEWritten]) 
+        {
+            BLECharacteristic chrcTmp(this, &_ble_device);
+            BLECentral central(_ble_device);
+            _oldevent_handlers[BLEWritten](central, chrcTmp);
+        }
+    }
+    else
+    {
+        // GATT client
+        // Discovered attribute
+        // Read response/Notification/Indication for GATT client
+        if (_reading)
+        {
+            // Read response received. Not block the other reading.
+            _reading = false;
+        }
+        
+        if (_event_handlers[BLEValueUpdated]) 
+        {
+            BLECharacteristic chrcTmp(this, &_ble_device);
+            _event_handlers[BLEValueUpdated](_ble_device, chrcTmp);
+        }
+        
+        if (_oldevent_handlers[BLEValueUpdated]) 
+        {
+            BLECharacteristic chrcTmp(this, &_ble_device);
+            BLECentral central(_ble_device);
+            _oldevent_handlers[BLEValueUpdated](central, chrcTmp);
+        }
+    }
+    
+    return true;
+}
+
+unsigned short
+BLECharacteristicImp::valueSize() const
+{
+    return _value_size;
+}
+
+const unsigned char*
+BLECharacteristicImp::value() const
+{
+    return _value;
+}
+
+unsigned short
+BLECharacteristicImp::valueLength() const
+{
+    return _value_length;
+}
+
+unsigned char
+BLECharacteristicImp::operator[] (int offset) const
+{
+    return _value[offset];
+}
+
+bool
+BLECharacteristicImp::written()
+{
+    bool written = false;
+    if (true == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT server. The characteristic on local device
+        written = _value_updated;
+        _value_updated = false;
+    }
+
+    return written;
+}
+
+bool BLECharacteristicImp::valueUpdated()
+{
+    bool updated = false;
+    if (false == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT client. The characteristic on remote device.
+        updated = _value_updated;
+        _value_updated = false;
+    }
+    return updated;
+}
+
+bool
+BLECharacteristicImp::subscribed()
+{
+    if (false == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT client
+        return _subscribed;
+    }
+    else
+    {
+        // GATT server
+        return (_ccc_value.value & (BT_GATT_CCC_NOTIFY | BT_GATT_CCC_INDICATE));
+    }
+}
+
+bool BLECharacteristicImp::canNotify()
+{
+    if (false == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT client can't subscribe
+        return false;
+    }
+    
+    // GATT server
+    return (_ccc_value.value & BT_GATT_CCC_NOTIFY);
+}
+
+bool BLECharacteristicImp::canIndicate()
+{
+    if (false == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT client can't subscribe
+        return false;
+    }
+    
+    // GATT server
+    return (_ccc_value.value & BT_GATT_CCC_INDICATE);
+}
+
+bool BLECharacteristicImp::unsubscribe(void)
+{
+    int retval = 0;
+    bt_conn_t* conn = NULL;
+    
+    if (true == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT server can't subscribe
+        return false;
+    }
+    
+    if (false == _subscribed)
+    {
+        return true;
+    }
+    
+    _sub_params.value = 0;
+    
+    if (0 == (_gatt_chrc.properties & (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE)))
+    {
+        // The characteristic not support the Notify and Indicate
+        return false;
+    }
+    
+    conn = bt_conn_lookup_addr_le(_ble_device.bt_le_address());
+    if (NULL == conn)
+    {
+        return false;
+    }
+    
+    bt_addr_le_copy(&_sub_params._peer, bt_conn_get_dst(conn));
+    _sub_params.ccc_handle = _cccd_handle;
+    _sub_params.value_handle = _value_handle;
+    
+    // Enable CCCD to allow peripheral send Notification/Indication
+    retval = bt_gatt_unsubscribe(conn, &_sub_params);
+    bt_conn_unref(conn);
+    if (0 == retval)
+    {
+        _subscribed = false;
+    }
+    return _subscribed;
+}
+
+bool BLECharacteristicImp::subscribe(void)
+{
+    int retval = 0;
+    bt_conn_t* conn = NULL;
+    
+    if (true == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT server can't subscribe
+        return false;
+    }
+    
+    if (_gatt_chrc.properties & BT_GATT_CHRC_NOTIFY)
+    {
+        _sub_params.value |= BT_GATT_CCC_NOTIFY;
+    }
+    
+    if (_gatt_chrc.properties & BT_GATT_CHRC_INDICATE)
+    {
+        _sub_params.value |= BT_GATT_CCC_INDICATE;
+    }
+    
+    if (_sub_params.value == 0)
+    {
+        return false;
+    }
+    
+    conn = bt_conn_lookup_addr_le(_ble_device.bt_le_address());
+    if (NULL == conn)
+    {
+        return false;
+    }
+    
+    bt_addr_le_copy(&_sub_params._peer, bt_conn_get_dst(conn));
+    _sub_params.ccc_handle = _cccd_handle;
+    _sub_params.value_handle = _value_handle;
+    
+    // Enable CCCD to allow peripheral send Notification/Indication
+    retval = bt_gatt_subscribe(conn, &_sub_params);
+    bt_conn_unref(conn);
+    if (0 == retval)
+    {
+        _subscribed = true;
+    }
+    return _subscribed;
+}
+
+void
+BLECharacteristicImp::setEventHandler(BLECharacteristicEvent event, BLECharacteristicEventHandler callback)
+{
+    noInterrupts();
+    if (event < BLECharacteristicEventLast) {
+        _event_handlers[event] = callback;
+    }
+    interrupts();
+}
+
+void
+BLECharacteristicImp::setEventHandler(BLECharacteristicEvent event, BLECharacteristicEventHandlerOld callback)
+{
+    noInterrupts();
+    if (event < BLECharacteristicEventLast) {
+        _oldevent_handlers[event] = callback;
+    }
+    interrupts();
+}
+
+void
+BLECharacteristicImp::setHandle(uint16_t handle)
+{
+    // GATT client
+    _value_handle = handle;
+}
+
+void
+BLECharacteristicImp::setCCCDHandle(uint16_t handle)
+{
+    // GATT client
+    _cccd_handle = handle;
+}
+
+uint16_t
+BLECharacteristicImp::valueHandle()
+{
+    uint16_t handle = 0;
+    if (NULL != _attr_chrc_value)
+    {
+        //GATT server
+        handle = _attr_chrc_value->handle;
+    }
+    else
+    {
+        // GATT client
+        handle = _value_handle;
+    }
+    
+    return handle;
+}
+
+BLEDescriptorImp* BLECharacteristicImp::descriptor(uint16_t handle)
+{
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLEDescriptorNodePtr node = link_node_get_first(&_descriptors_header);
+    while (NULL != node)
+    {
+        descriptorImp = node->value;
+        if (handle == descriptorImp->valueHandle())
+        {
+            break;
+        }
+        node = node->next;
+    }
+    if (NULL == node)
+    {
+        descriptorImp = NULL;
+    }
+    return descriptorImp;
+}
+
+void
+BLECharacteristicImp::_setValue(const uint8_t value[], uint16_t length, uint16_t offset)
+{
+    if (length + offset > _value_size)
+    {
+        if (_value_size > offset)
+        {
+            uint16_t temp_len = _value_size - offset;
+            if (length > temp_len)
+            {
+                length = temp_len;
+            }
+        }
+        else
+        {
+            return;
+        }
+    }
+    
+    memcpy(_value + offset, value, length);
+    _value_length = length;
+}
+
+_bt_gatt_ccc_t* BLECharacteristicImp::getCccCfg(void)
+{
+    return &_ccc_value;
+}
+
+bt_gatt_chrc_t* BLECharacteristicImp::getCharacteristicAttValue(void)
+{
+    return &_gatt_chrc;
+}
+
+uint8_t BLECharacteristicImp::getPermission(void)
+{
+    uint8_t perm = 0;
+    if (_gatt_chrc.properties & BT_GATT_CHRC_READ)
+    {
+        perm |= BT_GATT_PERM_READ;
+    }
+    if (_gatt_chrc.properties & (BT_GATT_CHRC_WRITE | BT_GATT_CHRC_WRITE_WITHOUT_RESP))
+    {
+        perm |= BT_GATT_PERM_WRITE;
+    }
+    return perm;
+}
+
+bt_uuid_t* BLECharacteristicImp::getCharacteristicAttributeUuid(void)
+{
+    return (bt_uuid_t*) &_gatt_chrc_uuid;
+}
+
+bt_uuid_t* BLECharacteristicImp::getClientCharacteristicConfigUuid(void)
+{
+    return (bt_uuid_t*) &_gatt_ccc_uuid;
+}
+
+bool BLECharacteristicImp::read(bool blocked)
+{
+    int retval = 0;
+    bool ret_bool = false;
+    bt_conn_t* conn = NULL;
+    
+    if (true == BLEUtils::isLocalBLE(_ble_device))
+    {
+        // GATT server can't write
+        return false;
+    }
+    
+    if (_reading)
+    {
+        // Already in reading state
+        return false;
+    }
+    
+    _read_params.func = profile_read_rsp_process;
+    _read_params.handle_count = 1;
+    _read_params.single.handle = _value_handle;
+    _read_params.single.offset = 0;
+    
+    if (0 == _read_params.single.handle)
+    {
+        // Discover not complete
+        return false;
+    }
+    
+    conn = bt_conn_lookup_addr_le(_ble_device.bt_le_address());
+    if (NULL == conn)
+    {
+        return false;
+    }
+    
+    // Send read request
+    retval = bt_gatt_read(conn, &_read_params);
+    if (0 == retval)
+    {
+        _reading = true;
+        ret_bool = true;
+        
+        // Block the call
+        if (blocked == true)
+        {
+            while (_reading == true && ret_bool)
+            {
+                delay(5);
+                ret_bool = _ble_device.connected();
+            }
+        }
+    }
+    bt_conn_unref(conn);
+    return ret_bool;
+}
+
+void BLECharacteristicImp::writeResponseReceived(struct bt_conn *conn, 
+                                                 uint8_t err,
+                                                 const void *data)
+{
+    _gattc_writing = false;
+}
+
+bool BLECharacteristicImp::write(const unsigned char value[], 
+                                 uint16_t length)
+{
+    int retval = 0;
+    bt_conn_t* conn = NULL;
+    
+    if (true == BLEUtils::isLocalBLE(_ble_device) || true == _gattc_writing)
+    {
+        // GATT server can't write
+        return false;
+    }
+    
+    conn = bt_conn_lookup_addr_le(_ble_device.bt_le_address());
+    if (NULL == conn)
+    {
+        return false;
+    }
+    
+    // Send write request
+    if (_gatt_chrc.properties & BT_GATT_CHRC_WRITE)
+    {
+        _gattc_writing = true;
+        retval = bt_gatt_write(conn, 
+                               _value_handle,
+                               0,
+                               value, 
+                               length, 
+                               ble_on_write_no_rsp_complete);
+        while (_gattc_writing)
+        {
+            delay(2);
+        }
+    } else if (_gatt_chrc.properties & BT_GATT_CHRC_WRITE_WITHOUT_RESP)
+    {
+        retval = bt_gatt_write_without_response(conn, 
+                                                _value_handle,
+                                                value, 
+                                                length, 
+                                                false);
+    }
+    bt_conn_unref(conn);
+    return (0 == retval);
+}
+
+void BLECharacteristicImp::setBuffer(const uint8_t value[], 
+                                      uint16_t length, 
+                                      uint16_t offset)
+{
+  if ((length + offset > _value_size) ||
+      ((unsigned char *)NULL == _value_buffer)) {
+        // Ignore the data
+        return;
+    }
+
+    memcpy(_value_buffer + offset, value, length);
+}
+
+void BLECharacteristicImp::syncupBuffer2Value()
+{
+    setValue(_value_buffer, _value_size);
+}
+
+void BLECharacteristicImp::discardBuffer()
+{
+  if(_value_buffer)
+    memcpy(_value_buffer, _value, _value_size);
+}
+
+bool BLECharacteristicImp::longCharacteristic()
+{
+    return (_value_size > BLE_MAX_ATTR_DATA_LEN);
+}
+
+int BLECharacteristicImp::updateProfile(bt_gatt_attr_t *attr_start, int& index)
+{
+    bt_gatt_attr_t *start = attr_start;
+    int base_index = index;
+    int offset = 0;
+    int counter = 0;
+    
+    // Characteristic declare
+    memset(start, 0, sizeof(bt_gatt_attr_t));
+    start->uuid = getCharacteristicAttributeUuid();
+    start->perm = BT_GATT_PERM_READ;
+    start->read = bt_gatt_attr_read_chrc;
+    start->user_data = this->getCharacteristicAttValue();
+    pr_info(LOG_MODULE_BLE, "chrc-%p, uuid type-%d", start, start->uuid->type);
+    
+    start++;
+    index++;
+    counter++;
+    
+    // Descriptor
+    memset(start, 0, sizeof(bt_gatt_attr_t));
+    start->uuid = (bt_uuid_t *)bt_uuid();
+    start->perm = this->getPermission();
+    start->user_data = (void*)((BLEAttribute*)this);
+    start->read = profile_read_process;
+
+    if (this->longCharacteristic() == false)
+    {
+        // Normal characteristic MAX. 20
+        start->write = profile_write_process;
+    }
+    else
+    {
+        // Long characteristic. MAX. 512
+        start->write = profile_longwrite_process;
+        start->flush = profile_longflush_process;
+    }
+    _attr_chrc_value = start;
+    pr_debug(LOG_MODULE_BLE, "chrcdescripor-%p, chimp-%p type-%d", start, this, this->type());
+    
+    start++;
+    index++;
+    counter++;
+    
+    if (0 != (_gatt_chrc.properties & (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE)))
+    {
+        // Descriptor
+        memset(start, 0, sizeof(bt_gatt_attr_t));
+        start->uuid = this->getClientCharacteristicConfigUuid();
+        start->perm = BT_GATT_PERM_READ | BT_GATT_PERM_WRITE;
+        start->read = bt_gatt_attr_read_ccc;
+        start->write = bt_gatt_attr_write_ccc;
+        start->user_data = this->getCccCfg();
+        
+        pr_info(LOG_MODULE_BLE, "cccd-%p", start);
+        
+        start++;
+        index++;
+        counter++;
+    }
+    
+    BLEDescriptorNodePtr node = _descriptors_header.next;
+    while (NULL != node)
+    {
+        BLEDescriptorImp *descriptorImp = node->value;
+        start = attr_start + index - base_index;
+        offset = descriptorImp->updateProfile(start, index);
+        counter += offset;
+        node = node->next;
+    }
+    pr_debug(LOG_MODULE_BLE, "%s:type-%d", __FUNCTION__, this->type());
+    return counter;
+}
+
+int BLECharacteristicImp::addDescriptor(BLEDescriptor& descriptor)
+{
+    BLEDescriptorImp* descriptorImp = descrptor(descriptor.uuid());
+    if (NULL != descriptorImp)
+    {
+        return BLE_STATUS_SUCCESS;
+    }
+    
+    descriptorImp = new BLEDescriptorImp(_ble_device, descriptor);
+    pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+    if (NULL == descriptorImp)
+    {
+        return BLE_STATUS_NO_MEMORY;
+    }
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+    BLEDescriptorNodePtr node = link_node_create(descriptorImp);
+    if (NULL == node)
+    {
+        delete descriptorImp;
+        return BLE_STATUS_NO_MEMORY;
+    }
+    link_node_insert_last(&_descriptors_header, node);
+    pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+    return BLE_STATUS_SUCCESS;
+}
+
+int BLECharacteristicImp::addDescriptor(const bt_uuid_t* uuid, 
+                                        unsigned char property, 
+                                        uint16_t handle)
+{
+    BLEDescriptorImp* descriptorImp = descrptor(uuid);
+    if (NULL != descriptorImp)
+    {
+        return BLE_STATUS_SUCCESS;
+    }
+    
+    descriptorImp = new BLEDescriptorImp(uuid, property, handle, _ble_device);
+    pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+    if (NULL == descriptorImp)
+    {
+        return BLE_STATUS_NO_MEMORY;
+    }
+    
+    BLEDescriptorNodePtr node = link_node_create(descriptorImp);
+    if (NULL == node)
+    {
+        delete descriptorImp;
+        return BLE_STATUS_NO_MEMORY;
+    }
+    link_node_insert_last(&_descriptors_header, node);
+    pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+    return BLE_STATUS_SUCCESS;
+}
+
+BLEDescriptorImp* BLECharacteristicImp::descrptor(const bt_uuid_t* uuid)
+{
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLEDescriptorNodePtr node = link_node_get_first(&_descriptors_header);
+    
+    while (NULL != node)
+    {
+        descriptorImp = node->value;
+        if (true == descriptorImp->compareUuid(uuid))
+        {
+            break;
+        }
+        node = node->next;
+    }
+    
+    if (NULL == node)
+    {
+        descriptorImp = NULL;
+    }
+    return descriptorImp;
+}
+
+BLEDescriptorImp* BLECharacteristicImp::descrptor(const char* uuid)
+{
+    bt_uuid_128_t uuid_tmp;
+    BLEUtils::uuidString2BT(uuid, (bt_uuid_t *)&uuid_tmp);
+    return descrptor((const bt_uuid_t *)&uuid_tmp);
+}
+
+
+BLEDescriptorImp* BLECharacteristicImp::descrptor(int index)
+{
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLEDescriptorNodePtr node = link_node_get_first(&_descriptors_header);
+    while (NULL != node)
+    {
+        if (0 >= index)
+        {
+            descriptorImp = node->value;
+            break;
+        }
+        index--;
+        node = node->next;
+    }
+    return descriptorImp;
+}
+
+void BLECharacteristicImp::releaseDescriptors()
+{
+    BLEDescriptorNodePtr node = link_node_get_first(&_descriptors_header);
+    
+    while (NULL != node)
+    {
+        BLEDescriptorImp* descriptorImp = node->value;
+        delete descriptorImp;
+        link_node_remove_first(&_descriptors_header);
+        node = link_node_get_first(&_descriptors_header);
+    }
+}
+
+int BLECharacteristicImp::getAttributeCount()
+{
+    int counter = link_list_size(&_descriptors_header) + 2; // Declaration and descriptor
+    // Notification/Indecation
+    if (_gatt_chrc.properties & (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE))
+    {
+        counter++;
+    }
+    return counter;
+}
+
+int BLECharacteristicImp::descriptorCount() const
+{
+    int counter = link_list_size(&_descriptors_header);
+    return counter;
+}
+
+bool BLECharacteristicImp::discoverAttributes(BLEDevice* device)
+{
+    
+    int err;
+    bt_conn_t* conn;
+    bt_gatt_discover_params_t* temp = NULL;
+    const bt_uuid_t* service_uuid = bt_uuid();
+    
+    if (service_uuid->type == BT_UUID_TYPE_16)
+    {
+        uint16_t uuid_tmp ;//= ((bt_uuid_16_t*)service_uuid)->val;
+        memcpy(&uuid_tmp, &((bt_uuid_16_t*)service_uuid)->val, sizeof(uuid_tmp));
+        if (BT_UUID_GAP_VAL == uuid_tmp ||
+            BT_UUID_GATT_VAL == uuid_tmp)
+        {
+            return false;
+        }
+    }
+
+    conn = bt_conn_lookup_addr_le(device->bt_le_address());
+    if (NULL == conn)
+    {
+        // Link lost
+        pr_debug(LOG_MODULE_BLE, "Can't find connection\n");
+        return false;
+    }
+    temp = &_discover_params;
+    temp->start_handle = _value_handle + 1;
+    temp->end_handle = _value_handle + 20; // TODO: the max descriptor is not more than 20
+    temp->uuid = NULL;
+    temp->type = BT_GATT_DISCOVER_DESCRIPTOR;
+    temp->func = profile_discover_process;
+    pr_debug(LOG_MODULE_BLE, "%s-%d-charc",__FUNCTION__, __LINE__);
+    err = bt_gatt_discover(conn, temp);
+    bt_conn_unref(conn);
+    if (err)
+    {
+        pr_debug(LOG_MODULE_BLE, "Discover failed(err %d)\n", err);
+        return false;
+    }
+    return true;
+}
+
+bool BLECharacteristicImp::isClientCharacteristicConfigurationDescriptor(const bt_uuid_t* uuid)
+{
+    bool ret = false;
+    uint16_t cccd_uuid = BT_UUID_GATT_CCC_VAL;
+    if (uuid->type == BT_UUID_TYPE_16)
+    {
+        if (0 == memcmp(&BT_UUID_16(uuid)->val, &cccd_uuid, sizeof(uint16_t)))
+        {
+            ret = true;
+        }
+    }
+    return ret;
+}
+
+uint8_t BLECharacteristicImp::discoverResponseProc(bt_conn_t *conn,
+                                                   const bt_gatt_attr_t *attr,
+                                                   bt_gatt_discover_params_t *params)
+{
+    const bt_addr_le_t* dst_addr = bt_conn_get_dst(conn);
+    BLEDevice device(dst_addr);
+    uint8_t retVal = BT_GATT_ITER_STOP;
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d: type-%d", __FUNCTION__, __LINE__, params->type);
+
+    // Process the service
+    switch (params->type)
+    {
+        case BT_GATT_DISCOVER_DESCRIPTOR:
+        {
+            if (NULL != attr)
+            {
+                retVal = BT_GATT_ITER_CONTINUE;
+                const bt_uuid_t* desc_uuid = attr->uuid;
+                uint16_t desc_handle = attr->handle;
+    pr_debug(LOG_MODULE_BLE, "%s-%d:handle-%d:%d", __FUNCTION__, __LINE__,attr->handle, desc_handle);
+                if (isClientCharacteristicConfigurationDescriptor(desc_uuid))
+                {
+                    setCCCDHandle(desc_handle);
+                }
+                else if (bt_uuid_cmp(BLEServiceImp::getPrimayUuid(), desc_uuid) == 0 ||
+                         bt_uuid_cmp(getCharacteristicAttributeUuid(), desc_uuid) == 0 )
+                {
+                    retVal = BT_GATT_ITER_STOP;
+                }
+                else
+                {
+                    int retval = (int)addDescriptor(desc_uuid,
+                                                    attr->perm,
+                                                    desc_handle);
+                    
+                    if (BLE_STATUS_SUCCESS != retval)
+                    {
+                        pr_error(LOG_MODULE_BLE, "%s-%d: Error-%d", 
+                                 __FUNCTION__, __LINE__, retval);
+                        errno = ENOMEM;
+                        retVal = BT_GATT_ITER_STOP;
+                    }
+                    
+                }
+            }
+            break;
+        }
+        default:
+        {
+            break;
+        }
+    }
+    return retVal;
+}
+
+
diff --git a/libraries/CurieBLE/src/internal/BLECharacteristicImp.h b/libraries/CurieBLE/src/internal/BLECharacteristicImp.h
new file mode 100644
index 00000000..af5e9839
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLECharacteristicImp.h
@@ -0,0 +1,347 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef _BLE_CHARACTERISTICIMP_H_INCLUDED
+#define _BLE_CHARACTERISTICIMP_H_INCLUDED
+
+//#include "BLECommon.h"
+
+//#include "BLEDevice.h"
+//#include "BLEDescriptor.h"
+
+#include "CurieBLE.h"
+#include "BLEDescriptorImp.h"
+
+#include "BLEDevice.h"
+
+#include "LinkList.h"
+class BLEDescriptorImp;
+/**
+ * BLE GATT Characteristic  : public BLEAttribute 
+ */
+class BLECharacteristicImp: public BLEAttribute{
+public:
+    
+    virtual ~BLECharacteristicImp();
+
+    
+    /**
+     * @brief   Add the characteristic's descriptor
+     *
+     * @param   descriptor  The descriptor for characteristic
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    int addDescriptor(BLEDescriptor& descriptor);
+    int addDescriptor(const bt_uuid_t* uuid, 
+                      unsigned char property, 
+                      uint16_t handle);
+    
+    void releaseDescriptors();
+
+    /**
+     * @brief   Write the value of the characteristic
+     *
+     * @param   value   The value buffer that want to write to characteristic
+     *
+     * @param   length  The value buffer's length
+     *
+     * @param   offset  The offset in the characteristic's data
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  none
+     */
+    bool writeValue(const byte value[], int length);
+    bool writeValue(const byte value[], int length, int offset);
+
+    /**
+     * Set the current value of the Characteristic
+     *
+     * @param[in] value  New value to set, as a byte array.  Data is stored in internal copy.
+     * @param[in] length Length, in bytes, of valid data in the array to write.
+     *               Must not exceed maxLength set for this characteristic.
+     *
+     * @return bool true set value success, false on error
+     */
+    bool setValue(const unsigned char value[], unsigned short length);
+
+    /**
+     * Get the property mask of the Characteristic
+     *
+     * @return unsigned char property mask of the Characteristic
+     */
+    unsigned char properties(void) const;
+
+    /**
+     * Get the (maximum) size of the Characteristic
+     *
+     * @return unsigned size of characateristic in bytes
+     */
+    unsigned short valueSize(void) const;
+
+    /**
+     * Get data pointer to the value of the Characteristic
+     *
+     * @return const unsigned char* pointer to the value of the Characteristic
+     */
+    const unsigned char* value(void) const;
+
+    /**
+     * Get the current length of the value of the Characteristic
+     *
+     * @return unsigned short size of characateristic value in bytes
+     */
+    unsigned short valueLength() const;
+
+    unsigned char operator[] (int offset) const;
+
+    /**
+     * Has the value of the Characteristic been written by a central
+     *
+     * @return bool true is central has updated characteristic value, otherwise false
+     */
+    bool written(void);
+    bool valueUpdated();
+
+    /**
+     * Is a central listening for notifications or indications of the Characteristic
+     *
+     * @return bool true is central is subscribed, otherwise false
+     */
+    bool subscribed(void);
+    bool canNotify();
+    bool canIndicate();
+    
+    bool subscribe(void);
+    bool unsubscribe(void);
+
+    /**
+     * Provide a function to be called when events related to this Characteristic are raised
+     *
+     * @param[in] event Event type to set event handler for
+     * @param[in] callback  Pointer to callback function to invoke when the event occurs.
+     */
+    void setEventHandler(BLECharacteristicEvent event, BLECharacteristicEventHandler callback);
+    void setEventHandler(BLECharacteristicEvent event, BLECharacteristicEventHandlerOld callback);
+    
+    /**
+     * @brief   Schedule the read request to read the characteristic in peripheral
+     *
+     * @param[in]   blocked    Flag the call is blocked or un-blocked
+     *
+     * @return  bool    Indicate the success or error
+     *
+     * @note  Only for GATT client
+     *        Default it is block call as per Arduino request
+     */
+    bool read(bool blocked = true);
+    
+    /**
+     * @brief   Schedule the write request to update the characteristic in peripheral
+     *
+     * @param[in]   value       New value to set, as a byte array.  Data is stored in internal copy.
+     * @param[in]   length      Length, in bytes, of valid data in the array to write.
+     *                      Must not exceed maxLength set for this characteristic.
+     *
+     * @return  bool true set value success, false on error
+     *
+     * @note  none
+     */
+    bool write(const unsigned char value[], 
+               uint16_t length);
+    
+    static void writeResponseReceived(struct bt_conn *conn, 
+                                      uint8_t err,
+                                      const void *data);
+
+    int descriptorCount() const;
+    uint8_t discoverResponseProc(bt_conn_t *conn,
+                                 const bt_gatt_attr_t *attr,
+                                 bt_gatt_discover_params_t *params);
+    
+    bool discoverAttributes(BLEDevice* device);
+    
+    BLEDescriptorImp* descrptor(const bt_uuid_t* uuid);
+    BLEDescriptorImp* descrptor(const char* uuid);
+    BLEDescriptorImp* descrptor(int index);
+
+protected:
+    friend class BLEProfileManager;
+    friend class BLEServiceImp;
+    friend class BLECharacteristic;
+    
+    BLEDescriptorImp* descriptor(uint16_t handle);
+    /**
+     * Constructor for BLE Characteristic
+     *
+     * @param[in] characteristic    The characteristic
+     * @param[in] bledevice         The device that has this characteristic
+     */
+    BLECharacteristicImp(BLECharacteristic& characteristic, const BLEDevice& bledevice);
+    BLECharacteristicImp(const bt_uuid_t* uuid, 
+                         unsigned char properties,
+                         uint16_t handle,
+                         const BLEDevice& bledevice);
+    
+    friend int profile_longflush_process(struct bt_conn *conn,
+                                         const struct bt_gatt_attr *attr, 
+                                         uint8_t flags);
+    friend ssize_t profile_longwrite_process(struct bt_conn *conn,
+                                     const struct bt_gatt_attr *attr,
+                                     const void *buf, uint16_t len,
+                                     uint16_t offset);
+    
+    int updateProfile(bt_gatt_attr_t *attr_start, int& index);
+    
+    int getAttributeCount();
+    
+    bool longCharacteristic();
+    
+    void setBuffer(const uint8_t value[], 
+                   uint16_t length, 
+                   uint16_t offset);
+    void discardBuffer();
+    void syncupBuffer2Value();
+    
+    /**
+     * @brief   Get the characteristic value handle
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  Only for peripheral
+     */
+    uint16_t valueHandle(void);
+    
+    /**
+     * @brief   Get characteristic configuration descriptor value handle
+     *
+     * @param   none
+     *
+     * @return  uint16_t        The value handle
+     *                           0 is invalid handle
+     *
+     * @note  Only for peripheral
+     */
+    uint16_t cccdHandle(void);
+    
+    inline _bt_gatt_ccc_t* getCccCfg(void);
+    inline bt_gatt_chrc_t* getCharacteristicAttValue(void);
+    static bt_uuid_t* getCharacteristicAttributeUuid(void);
+    static bt_uuid_t* getClientCharacteristicConfigUuid(void);
+    
+    /**
+     * @brief   Get the characteristic permission
+     *
+     * @param   none
+     *
+     * @return  uint8_t The characteristic permission
+     *
+     * @note  none
+     */
+    uint8_t getPermission(void);
+    
+    /**
+     * @brief   For central to discover the peripherial profile
+     *
+     * @param[in]   attr    The discover response
+     *
+     * @param[in]   params  The discover parameter that need to fill
+     *
+     * @return  none
+     *
+     * @note  Only for central
+     */
+    void discover(const bt_gatt_attr_t *attr,
+                  bt_gatt_discover_params_t *params);
+    
+    /**
+     * @brief   For central to discover the peripherial profile
+     *
+     * @param[in]   params  The discover parameter that need to fill
+     *
+     * @return  none
+     *
+     * @note  Only for central
+     */
+    void discover(bt_gatt_discover_params_t *params);
+    
+    /**
+     * @brief   Get the subscribe parameter
+     *
+     * @param   none
+     *
+     * @return bt_gatt_subscribe_params_t * the subscribe parameter
+     *
+     * @note  Only for central
+     */
+   bt_gatt_subscribe_params_t* getSubscribeParams();
+
+private:
+    
+    void setCCCDHandle(uint16_t handle);
+    void setHandle(uint16_t handle);
+    void _setValue(const uint8_t value[], uint16_t length, uint16_t offset);
+    bool isClientCharacteristicConfigurationDescriptor(const bt_uuid_t* uuid);
+
+private:
+    // Those 2 UUIDs are used for define the characteristic.
+    static bt_uuid_16_t _gatt_chrc_uuid;    // Characteristic UUID
+    static bt_uuid_16_t _gatt_ccc_uuid;     // CCCD UUID
+    
+    unsigned short _value_size;
+    unsigned short _value_length;
+    unsigned char* _value;
+    unsigned char* _value_buffer;
+    bool _value_updated;
+
+    uint16_t            _value_handle; // GATT client only
+    uint16_t            _cccd_handle;  // GATT client only
+    bt_gatt_discover_params_t _discover_params;// GATT client only
+    
+    bt_gatt_ccc_cfg_t   _ccc_cfg;
+    _bt_gatt_ccc_t      _ccc_value;
+    bt_gatt_chrc_t      _gatt_chrc;
+    
+    bt_gatt_attr_t *_attr_chrc_value; // GATT server only
+    bt_gatt_attr_t *_attr_cccd; // GATT server only
+    
+    // For GATT Client to subscribe the Notification/Indication
+    bt_gatt_subscribe_params_t _sub_params;
+    bool        _subscribed;
+    
+    volatile bool _reading;
+    static volatile bool _gattc_writing;
+    bt_gatt_read_params_t _read_params; // GATT read parameter
+    
+    typedef LinkNode<BLEDescriptorImp *>  BLEDescriptorLinkNodeHeader;
+    typedef LinkNode<BLEDescriptorImp *>* BLEDescriptorNodePtr;
+    typedef LinkNode<BLEDescriptorImp *>  BLEDescriptorNode;
+        
+    BLECharacteristicEventHandler _event_handlers[BLECharacteristicEventLast];
+    BLECharacteristicEventHandlerOld  _oldevent_handlers[BLECharacteristicEventLast];
+    BLEDescriptorLinkNodeHeader  _descriptors_header;
+    BLEDevice   _ble_device;
+};
+
+#endif // _BLE_CHARACTERISTIC_H_INCLUDED
diff --git a/libraries/CurieBLE/src/internal/BLEDescriptorImp.cpp b/libraries/CurieBLE/src/internal/BLEDescriptorImp.cpp
new file mode 100644
index 00000000..d773f63c
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEDescriptorImp.cpp
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+     
+#include "BLEAttribute.h"
+#include "BLEDescriptorImp.h"
+
+#include "internal/ble_client.h"
+
+#include "BLECallbacks.h"
+#include "BLEUtils.h"
+
+BLEDescriptorImp::BLEDescriptorImp(BLEDevice& bledevice,
+                                   BLEDescriptor &descriptor):
+    BLEAttribute(descriptor.uuid(), BLETypeDescriptor),
+    _value_handle(0),
+    _bledev(bledevice),
+    _reading(false),
+    _attr_desc_value(NULL)
+{
+    
+    _properties = descriptor.properties();
+    _value_length = descriptor.valueLength();
+    _value = (unsigned char*)malloc(_value_length);
+
+    if (_value)
+      memcpy(_value, descriptor.value(), _value_length);
+    else
+      _value_length = 0;
+}
+
+BLEDescriptorImp::BLEDescriptorImp(const bt_uuid_t* uuid, 
+                                   unsigned char properties, 
+                                   uint16_t handle,
+                                   BLEDevice& bledevice):
+    BLEAttribute(uuid, BLETypeDescriptor),
+    _value_length(0),
+    _value_handle(handle),
+    _value(NULL),
+    _properties(properties),
+    _bledev(bledevice),
+    _reading(false),
+    _attr_desc_value(NULL)
+{
+    memset(&_read_params, 0, sizeof(_read_params));
+}
+
+
+BLEDescriptorImp::BLEDescriptorImp(const BLEDescriptorImp& rhs) :
+    BLEAttribute(rhs),
+    _reading(false),
+    _attr_desc_value(rhs._attr_desc_value)
+{
+    _value_length = rhs._value_length;
+    _value = (unsigned char *)malloc(_value_length);
+    if (_value)
+        memcpy(_value, rhs._value, sizeof(_value_length));
+    else
+        _value_length = 0;
+
+    _value_handle = rhs._value_handle;
+    _properties = rhs._properties;
+    _bledev = BLEDevice(&rhs._bledev);
+}
+
+
+BLEDescriptorImp& BLEDescriptorImp::operator=(const BLEDescriptorImp& that)
+{
+    if (this != &that)
+    {
+        BLEAttribute::operator=(that);
+        if (_value)
+            free(_value);
+
+        _value_length = that._value_length;
+        _value = (unsigned char *)malloc(_value_length);
+        if (_value)
+            memcpy(_value, that._value, sizeof(_value_length));
+        else
+            _value_length = 0;
+
+        _value_handle = that._value_handle;
+        _properties = that._properties;
+        _bledev = BLEDevice(&that._bledev);
+        _attr_desc_value = that._attr_desc_value;
+    }
+    return *this;
+}
+
+BLEDescriptorImp::~BLEDescriptorImp() {
+    if (_value != (unsigned char *)NULL) {
+        free(_value);
+        _value = (unsigned char *)NULL;
+    }
+}
+
+const unsigned char*
+BLEDescriptorImp::value() const
+{
+    return _value;
+}
+
+unsigned short
+BLEDescriptorImp::valueLength() const
+{
+    return _value_length;
+}
+
+unsigned char
+BLEDescriptorImp::operator[] (int offset) const
+{
+    return _value[offset];
+}
+
+int BLEDescriptorImp::updateProfile(bt_gatt_attr_t *attr_start, int& index)
+{
+    bt_gatt_attr_t *start = attr_start;
+    start->uuid = (struct bt_uuid *)bt_uuid();
+    start->perm = BT_GATT_PERM_READ;
+    start->read = profile_read_process;
+    start->user_data = (void*)((BLEAttribute*)this);
+    
+    pr_debug(LOG_MODULE_BLE, "Descriptor-%p", start);
+    index++;
+    return 1;
+}
+
+uint16_t
+BLEDescriptorImp::valueHandle() const
+{
+    uint16_t handle = 0;
+    if (NULL != _attr_desc_value)
+    {
+        //GATT server
+        handle = _attr_desc_value->handle;
+    }
+    else
+    {
+        // GATT client
+        handle = _value_handle;
+    }
+    
+    return handle;
+}
+
+unsigned char BLEDescriptorImp::properties() const
+{
+    return _properties;
+}
+
+int BLEDescriptorImp::valueSize() const
+{
+    return _value_length;
+}
+
+bool BLEDescriptorImp::read()
+{
+    int retval = 0;
+    bt_conn_t* conn = NULL;
+    bool ret_bool = true;
+    
+    if (true == BLEUtils::isLocalBLE(_bledev))
+    {
+        // GATT server can't read
+        return false;
+    }
+    
+    if (_reading)
+    {
+        // Already in reading state
+        return false;
+    }
+    
+    _read_params.func = profile_descriptor_read_rsp_process;
+    _read_params.handle_count = 1;
+    _read_params.single.handle = _value_handle;
+    _read_params.single.offset = 0;
+    
+    if (0 == _read_params.single.handle)
+    {
+        // Discover not complete
+        return false;
+    }
+    
+    conn = bt_conn_lookup_addr_le(_bledev.bt_le_address());
+    if (NULL == conn)
+    {
+        return false;
+    }
+    
+    // Send read request
+    retval = bt_gatt_read(conn, &_read_params);
+    bt_conn_unref(conn);
+    if (0 == retval)
+    {
+        _reading = true;
+    }
+    
+    // Block the read
+    while (_reading == true && ret_bool)
+    {
+        delay(5);
+        ret_bool = _bledev.connected();
+    }
+    return _reading;
+}
+
+bool BLEDescriptorImp::writeValue(const byte value[], 
+                                  int length, 
+                                  int offset)
+{
+    bool ret = true;
+    int total_length = length + offset;
+    int write_len = length;
+    
+    if (_reading)
+    {
+        _reading = false;
+    }
+    
+    if (total_length > BLE_MAX_ATTR_DATA_LEN)
+    {
+        return false;
+    }
+    
+    if (NULL == _value)
+    {
+        _value_length = length + offset;
+        _value = (unsigned char*)malloc(_value_length);
+
+        if (NULL != _value)
+        {
+            memset(_value, 0, _value_length);
+        }
+        else
+        {
+            _value_length = 0;
+            ret = false;
+        }
+    }
+    
+    if (_value_length < total_length)
+    {
+        write_len = _value_length - offset;
+    }
+    
+    if (NULL != _value)
+    {
+        memcpy(_value + offset, value, write_len);
+    }
+    return ret;
+}
+
+
diff --git a/libraries/CurieBLE/src/internal/BLEDescriptorImp.h b/libraries/CurieBLE/src/internal/BLEDescriptorImp.h
new file mode 100644
index 00000000..701f3f83
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEDescriptorImp.h
@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef _BLE_DESCRIPTORIMP_H_INCLUDED
+#define _BLE_DESCRIPTORIMP_H_INCLUDED
+
+#include "CurieBLE.h"
+
+/**
+ * BLE GATT Descriptor class
+ */
+class BLEDescriptorImp: public BLEAttribute{
+public:
+    /**
+     * Constructor for BLE Descriptor
+     *
+     * @param[in] uuid        16-bit UUID (in string form) defined by BLE standard
+     * @param[in] value       Value of descriptor, as a byte array.  Data is stored in internal copy.
+     * @param[in] valueLength Data length required for descriptor value (<= BLE_MAX_ATTR_DATA_LEN)
+     */
+    BLEDescriptorImp(BLEDevice& bledevice, BLEDescriptor &descriptor);
+    BLEDescriptorImp(const bt_uuid_t* uuid, 
+                     unsigned char properties, 
+                     uint16_t handle,
+                     BLEDevice& bledevice);
+
+    BLEDescriptorImp(const BLEDescriptorImp& rhs);
+
+    BLEDescriptorImp& operator=(const BLEDescriptorImp& that);
+
+    virtual ~BLEDescriptorImp();
+
+    /**
+     * Get data pointer to the value of the Descriptor
+     *
+     * @return const unsigned char* pointer to the value of the Descriptor
+     */
+    const unsigned char* value(void) const;
+
+    /**
+     * Get the length of the value of the Descriptor
+     *
+     * @return unsigned short size of Descriptor value in bytes
+     */
+    unsigned short valueLength(void) const;
+
+    /**
+     * @brief   Fill the attribute for profile register structure
+     *
+     * @param   bt_gatt_attr_t *  The start pointer of the profile register structure array
+     *
+     * @param   int&                The current index in the profile structure array
+     *
+     * @return  int       Filled structure counter
+     *
+     * @note  none
+     */
+    int updateProfile(bt_gatt_attr_t *attr_start, int& index);
+
+    unsigned char operator[] (int offset) const;
+
+    /**
+     * @brief   Get the property mask of the descriptor
+     *
+     * @param   none
+     *
+     * @return  unsigned char       The property mask of the descriptor
+     *
+     * @note  none
+     */
+    unsigned char properties() const;
+    
+    /**
+     * @brief   Get the maximum size of the value
+     *
+     * @param   none
+     *
+     * @return  int     The maximum size of the value
+     *
+     * @note  none
+     */
+    int valueSize() const;
+    
+    /**
+     * @brief   Get the descriptor value handle
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    uint16_t valueHandle() const;
+    
+    /**
+     * @brief   Write the value of the descriptor
+     *
+     * @param   value   The value buffer that want to write to descriptor
+     *
+     * @param   length  The value buffer's length
+     *
+     * @param   offset  The offset in the descriptor's data
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  none
+     */
+    bool writeValue(const byte value[], int length, int offset);
+    
+    /**
+     * @brief   Read the descriptor value
+     *
+     * @param   none
+     *
+     * @return  bool    true - Success, false - Failed
+     *
+     * @note  Only for GATT client. Schedule read request to the GATT server
+     */
+    bool read();
+
+protected:
+
+
+private:
+    unsigned short _value_length;
+    unsigned short _value_handle;
+    unsigned char* _value;
+    unsigned char _properties;      // The characteristic property
+    
+    BLEDevice _bledev; 
+    
+    bool _reading;
+    bt_gatt_read_params_t _read_params; // GATT read parameter
+    
+    bt_gatt_attr_t *_attr_desc_value;   // GATT server only
+};
+
+#endif // _BLE_DESCRIPTOR_H_INCLUDED
diff --git a/libraries/CurieBLE/src/internal/BLEDeviceManager.cpp b/libraries/CurieBLE/src/internal/BLEDeviceManager.cpp
new file mode 100644
index 00000000..d5ddd181
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEDeviceManager.cpp
@@ -0,0 +1,1701 @@
+/*
+  BLE Device API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+#include "CurieBLE.h"
+#include "BLEDeviceManager.h"
+#include "BLEProfileManager.h"
+
+#include "internal/ble_client.h"
+
+#include <atomic.h>
+#include "../src/services/ble/conn_internal.h"
+
+#include "BLEUtils.h"
+#include "BLECallbacks.h"
+
+BLEDeviceManager* BLEDeviceManager::_instance;
+
+BLEDeviceManager::BLEDeviceManager():
+    _min_conn_interval(0),
+    _max_conn_interval(0),
+    _peer_temp_dev_index(0),
+    _adv_critical_local_name(""),
+    _wait_for_connect_peripheral_adv_data_len(0),
+    _wait_for_connect_peripheral_scan_rsp_data_len(0),
+    _wait_for_connect_peripheral_adv_rssi(0),
+    _available_for_connect_peripheral_adv_data_len(0),
+    _available_for_connect_peripheral_scan_rsp_data_len(0),
+    _available_for_connect_peripheral_adv_rssi(0),
+    _available_for_connect_peripheral_connectable(false),
+    _connecting(false),
+    _has_service_uuid(false),
+    _has_service_solicit_uuid(false),
+    _appearance(0),
+    _manufacturer_data_length(0),
+    _service_data_length(0),
+    _adv_type(0),
+    _adv_data_idx(0),
+    _scan_rsp_data_idx(0),
+    _local_name(""),
+    _state(BLE_PERIPH_STATE_NOT_READY),
+    _local_ble(NULL),
+    _peer_peripheral_index(0),
+    _duplicate_filter_header(0),
+    _duplicate_filter_tail(0),
+    _adv_duplicate_filter_enabled(false)
+{
+    memset(&_local_bda, 0, sizeof(_local_bda));
+    
+    
+    memset(&_peer_central, 0, sizeof (bt_addr_le_t));
+    
+    ble_client_get_factory_config(&_local_bda, _device_name);
+    
+    _adv_param.type = BT_LE_ADV_IND;
+    _adv_param.addr_type = _local_bda.type;
+    _adv_param.interval_min = 0xA0;
+    _adv_param.interval_max = 0xF0;
+    
+    _scan_param.type = BT_HCI_LE_SCAN_ACTIVE;
+    _scan_param.filter_dup   = BT_HCI_LE_SCAN_FILTER_DUP_ENABLE;
+    _scan_param.interval = BT_GAP_SCAN_FAST_INTERVAL;
+    _scan_param.window = BT_GAP_SCAN_FAST_WINDOW;
+
+    memset(_peer_adv_buffer, 0, sizeof(_peer_adv_buffer));
+    memset(_peer_adv_mill, 0, sizeof(_peer_adv_mill));
+    memset(_peer_adv_data, 0, sizeof(_peer_adv_data));
+    memset(_peer_adv_data_len, 0, sizeof(_peer_adv_data_len));
+    memset(_peer_scan_rsp_data, 0, sizeof(_peer_scan_rsp_data));
+    memset(_peer_scan_rsp_data_len, -1, sizeof(_peer_scan_rsp_data_len));
+    memset(_peer_adv_rssi, 0, sizeof(_peer_adv_rssi));
+    
+    memset(_peer_adv_connectable, 0, sizeof(_peer_adv_connectable));
+    
+    memset(_peer_temp_adv_buffer, 0, sizeof(_peer_temp_adv_buffer));
+    memset(_peer_temp_adv_data, 0, sizeof(_peer_temp_adv_data));
+    memset(_peer_temp_adv_data_len, 0, sizeof(_peer_temp_adv_data_len));
+    memset(_peer_temp_adv_connectable, 0, sizeof(_peer_adv_connectable));
+    
+    memset(&_adv_accept_critical, 0, sizeof(_adv_accept_critical));
+    memset(&_adv_critical_service_uuid, 0, sizeof(_adv_critical_service_uuid));
+    memset(&_adv_accept_device, 0, sizeof(_adv_accept_device));
+    
+    memset(&_wait_for_connect_peripheral, 0, sizeof(_wait_for_connect_peripheral));
+    memset(&_wait_for_connect_peripheral_adv_data, 0, sizeof(_wait_for_connect_peripheral_adv_data));
+    memset(&_wait_for_connect_peripheral_scan_rsp_data, 0, sizeof(_wait_for_connect_peripheral_scan_rsp_data));
+    
+    memset(&_available_for_connect_peripheral_adv_data, 0, sizeof(_available_for_connect_peripheral_adv_data));
+    memset(&_available_for_connect_peripheral_scan_rsp_data, 0, sizeof(_available_for_connect_peripheral_scan_rsp_data));
+    
+    memset(&_service_uuid, 0, sizeof(_service_uuid));
+    memset(&_service_solicit_uuid, 0, sizeof(_service_solicit_uuid));
+    memset(_manufacturer_data, 0, sizeof(_manufacturer_data));
+    
+    memset(&_service_data_uuid, 0, sizeof(_service_data_uuid));
+    memset(_service_data, 0, sizeof(_service_data));
+    memset(_service_data_buf, 0, sizeof(_service_data_buf));
+    
+    memset(_adv_data, 0, sizeof(_adv_data));
+    memset(_scan_rsp_data, 0, sizeof(_scan_rsp_data));
+    
+    memset(_peer_peripheral, 0, sizeof(_peer_peripheral));
+    memset(_peer_peripheral_adv_data, 0, sizeof(_peer_peripheral_adv_data));
+    memset(_peer_peripheral_adv_data_len, 0, sizeof(_peer_peripheral_adv_data_len));
+    memset(_peer_peripheral_scan_rsp_data, 0, sizeof(_peer_peripheral_scan_rsp_data));
+    memset(_peer_peripheral_scan_rsp_data_len, 0, sizeof(_peer_peripheral_scan_rsp_data_len));
+    memset(_peer_peripheral_adv_rssi, 0, sizeof(_peer_peripheral_adv_rssi));
+    
+    memset(_device_events, 0, sizeof(_device_events));
+}
+
+BLEDeviceManager::~BLEDeviceManager()
+{
+    
+}
+
+bool BLEDeviceManager::begin(BLEDevice *device)
+{
+    if (NULL == _local_ble)
+    {
+        _local_ble = device;
+        bt_le_set_mac_address(_local_bda);
+        
+        // Set device name    
+        setDeviceName();
+        _state = BLE_PERIPH_STATE_READY;
+        delay(4);
+        // TODO: Olny allow call one time
+        ble_client_init (bleConnectEventHandler, this,
+                         bleDisconnectEventHandler, this,
+                         bleParamUpdatedEventHandler, this);
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void BLEDeviceManager::poll()
+{
+    if (NULL != _device_events[BLEDiscovered])
+    {
+        BLEDevice tempdev = available();
+        
+        while (tempdev)
+        {
+            _device_events[BLEDiscovered](tempdev);
+            tempdev = available();
+        }
+    }
+}
+
+void BLEDeviceManager::end()
+{
+    stopScanning();
+    stopAdvertising();
+    // Disconnect the connections
+    disconnect(&BLE);
+}
+
+bool BLEDeviceManager::connected(const BLEDevice *device) const
+{
+    bt_conn_t* conn = bt_conn_lookup_addr_le(device->bt_le_address());
+    bool retval = false;
+    //pr_debug(LOG_MODULE_BLE, "%s-%d: add-%s", __FUNCTION__, __LINE__, device->address().c_str());
+    if (NULL != conn)
+    {
+        //pr_debug(LOG_MODULE_BLE, "%s-%d: state-%d", __FUNCTION__, __LINE__,conn->state);
+        if (conn->state == BT_CONN_CONNECTED)
+        {
+            retval = true;
+        }
+        bt_conn_unref(conn);
+    }
+    return retval;
+}
+
+bool BLEDeviceManager::disconnectSingle(const bt_addr_le_t *peer)
+{
+    int err = 0;
+    
+    bt_conn_t* conn = bt_conn_lookup_addr_le(peer);
+    if (NULL == conn)
+    {
+        return false;
+    }
+    
+    err = bt_conn_disconnect (conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
+    while (err == 0 && conn->state != BT_CONN_DISCONNECTED)
+    {
+        delay(10);
+    }
+    bt_conn_unref(conn);
+    return true;
+}
+
+
+bool BLEDeviceManager::disconnect(BLEDevice *device)
+{
+    bool ret = true;
+    if (false == BLEUtils::isLocalBLE(*device))
+    {
+        // Remote device disconnect one
+        ret = disconnectSingle(device->bt_le_address());
+    }
+    else
+    {
+        // Local device disconnect all connections
+        if (true == BLEUtils::macAddressValid(_peer_central))
+        {
+            ret = disconnectSingle(&_peer_central);
+        }
+        
+        for (int i = 0; i < BLE_MAX_CONN_CFG; i++)
+        {
+            if (true == BLEUtils::macAddressValid(_peer_peripheral[i]))
+            {
+                ret = disconnectSingle(&_peer_central);
+            }
+        }
+    }
+    return ret;
+}
+
+void BLEDeviceManager::setAdvertisedServiceUuid(const char* advertisedServiceUuid)
+{
+    _has_service_uuid = true;
+    BLEUtils::uuidString2BT(advertisedServiceUuid, (bt_uuid_t *)&_service_uuid);
+}
+
+void BLEDeviceManager::setAdvertisedServiceData(const bt_uuid_t* serviceDataUuid,
+                                                const uint8_t* serviceData,
+                                                uint8_t serviceDataLength)
+{
+    memcpy(&_service_data_uuid, serviceDataUuid, sizeof(_service_data_uuid));
+    if (serviceDataLength > BLE_MAX_ADV_SIZE)
+    {
+        serviceDataLength = BLE_MAX_ADV_SIZE;
+    }
+    
+    memcpy(_service_data, serviceData, serviceDataLength);
+    _service_data_length = serviceDataLength;
+}
+
+void BLEDeviceManager::setServiceSolicitationUuid(const char* serviceSolicitationUuid)
+{
+    _has_service_solicit_uuid = true;
+    BLEUtils::uuidString2BT(serviceSolicitationUuid, (bt_uuid_t *)&_service_solicit_uuid);
+}
+
+void BLEDeviceManager::setManufacturerData(const unsigned char manufacturerData[], 
+                                           unsigned char manufacturerDataLength)
+{
+    if (manufacturerDataLength > BLE_MAX_ADV_SIZE)
+    {
+        manufacturerDataLength = BLE_MAX_ADV_SIZE;
+    }
+    _manufacturer_data_length = manufacturerDataLength;
+    memcpy(_manufacturer_data, manufacturerData, manufacturerDataLength);
+}
+
+void BLEDeviceManager::setLocalName(const char *localName)
+{
+    _local_name = localName;
+}
+
+void BLEDeviceManager::setAdvertisingInterval(float advertisingInterval)
+{
+    uint16_t interval = (uint16_t) MSEC_TO_UNITS(advertisingInterval, UNIT_0_625_MS);
+    
+    _adv_param.interval_min = interval;
+    _adv_param.interval_max = interval;
+}
+
+void BLEDeviceManager::getConnectionInterval(BLEDevice *device, 
+                                             bt_le_conn_param* conn_param)
+{
+    bt_conn_t* conn = bt_conn_lookup_addr_le(device->bt_le_address());
+    if (NULL != conn)
+    {
+        conn_param->interval_max = conn->le.interval;
+        conn_param->interval_min = conn->le.interval;
+        conn_param->latency = conn->le.latency;
+        conn_param->timeout = conn->le.timeout;
+        bt_conn_unref(conn);
+    }
+}
+
+int BLEDeviceManager::setConnectionInterval(BLEDevice *device)
+{
+    bt_conn_t* conn = bt_conn_lookup_addr_le(device->bt_le_address());
+    int ret = 0;
+    if (NULL != conn)
+    {
+        ret = bt_conn_le_param_update(conn, device->bt_conn_param());
+        pr_debug(LOG_MODULE_BLE, "%s-ret:%d",__FUNCTION__, ret);
+        bt_conn_unref(conn);
+    }
+    return ret;
+}
+
+bool BLEDeviceManager::setTxPower(int txPower)
+{
+    ble_gap_set_tx_power(txPower);
+    return true;
+}
+
+void BLEDeviceManager::setConnectable(bool connectable)
+{
+    uint8_t type = BT_LE_ADV_IND;
+    if (connectable == false)
+    {
+        type = BT_LE_ADV_NONCONN_IND;
+    }
+    _adv_param.type = type;
+}
+
+void BLEDeviceManager::setDeviceName(const char* deviceName)
+{
+    memset(_device_name, 0, sizeof(_device_name));
+    if (deviceName && deviceName[0])
+    {
+        int len = strlen(deviceName);
+        if (len > BLE_MAX_DEVICE_NAME)
+            len = BLE_MAX_DEVICE_NAME;
+        memcpy(_device_name, deviceName, len);
+        if (NULL != _local_ble)
+        {
+            setDeviceName();
+        }
+    }
+}
+
+void
+BLEDeviceManager::setDeviceName()
+{
+    int len = strlen(_device_name);
+    bt_le_set_device_name(_device_name, len);
+}
+
+void BLEDeviceManager::setAppearance(unsigned short appearance)
+{
+    _appearance = appearance;
+}
+
+BLE_STATUS_T
+BLEDeviceManager::setAdvertiseData(uint8_t type, const uint8_t* data, uint8_t length)
+{
+    uint8_t lengthOfAdv = 0; // Flags data length
+    uint8_t lengthOfScanRsp = 0; // Flags data length
+    bt_data_t *fill_area = NULL;
+    
+    // Get the length of the Advertisement
+    for (uint8_t i = 0; i < _adv_data_idx; i++)
+    {
+        lengthOfAdv += _adv_data[i].data_len + 2;
+    }
+    
+    for (uint8_t i = 0; i < _scan_rsp_data_idx; i++)
+    {
+        lengthOfAdv += _scan_rsp_data[i].data_len + 2;
+    }
+        
+        
+    if (((length + lengthOfAdv) < BLE_MAX_ADV_SIZE) && 
+        (_adv_data_idx < ARRAY_SIZE(_adv_data)))
+    {
+        fill_area = &_adv_data[_adv_data_idx];
+        _adv_data_idx++;
+    }
+    else if ((length + lengthOfScanRsp) < BLE_MAX_ADV_SIZE && 
+             (_scan_rsp_data_idx < ARRAY_SIZE(_scan_rsp_data)))
+    {
+        fill_area = &_scan_rsp_data[_scan_rsp_data_idx];
+        _scan_rsp_data_idx++;
+    }
+    else
+    {
+        // Service data block is too large.
+        return BLE_STATUS_ERROR_PARAMETER;
+    }
+
+    if (fill_area)
+    {
+        fill_area->type = type;
+        fill_area->data = data;
+        fill_area->data_len = length;
+        
+        pr_info(LOG_MODULE_BLE, "ADV type %d Len - %d",type, length);
+    }
+    return BLE_STATUS_SUCCESS;
+}
+
+BLE_STATUS_T
+BLEDeviceManager::_advDataInit(void)
+{
+    BLE_STATUS_T ret = BLE_STATUS_SUCCESS;
+    // Clear the indexs
+    _adv_data_idx = 0;
+    _scan_rsp_data_idx = 0;
+    
+    /* Add flags */
+    _adv_type = (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR);
+    ret = setAdvertiseData (BT_DATA_FLAGS, &_adv_type, sizeof(_adv_type));
+    
+    if (_has_service_solicit_uuid && 
+        (BLE_STATUS_SUCCESS == ret)) 
+    {
+        uint8_t type;
+        uint8_t length;
+        uint8_t *data = NULL;
+        
+        pr_info(LOG_MODULE_BLE, "ADV Type-%d", _service_solicit_uuid.uuid.type);
+        if (BT_UUID_TYPE_16 == _service_solicit_uuid.uuid.type)
+        {
+            data = (uint8_t *)&(((bt_uuid_16_t *)&_service_solicit_uuid)->val);
+            length = UUID_SIZE_16;
+            type = BT_DATA_SOLICIT16;
+        }
+        else  // Sid. KW, default is BT_UUID_TYPE_128
+        {
+            data = _service_solicit_uuid.val;
+            length = UUID_SIZE_128;
+            type = BT_DATA_SOLICIT128;
+        }
+        
+        ret = setAdvertiseData(type, data, length);
+    }
+    
+    if (_has_service_uuid && 
+        (BLE_STATUS_SUCCESS == ret)) 
+    {
+        uint8_t type;
+        uint8_t length;
+        uint8_t *data = NULL;
+        
+        pr_info(LOG_MODULE_BLE, "ADV Type-%d", _service_uuid.uuid.type);
+        if (BT_UUID_TYPE_16 == _service_uuid.uuid.type)
+        {
+            data = (uint8_t *)&(((bt_uuid_16_t *)&_service_uuid)->val);
+            length = UUID_SIZE_16;
+            type = BT_DATA_UUID16_ALL;
+        }
+        else //  Sid. KW, default is BT_UUID_TYPE_128
+        {
+            data = _service_uuid.val;
+            length = UUID_SIZE_128;
+            type = BT_DATA_UUID128_ALL;
+        }
+        ret = setAdvertiseData(type, data, length);
+    }
+    
+    if (_manufacturer_data_length > 0  && 
+        (BLE_STATUS_SUCCESS == ret))
+    {
+        ret = setAdvertiseData (BT_DATA_MANUFACTURER_DATA, 
+                                _manufacturer_data, 
+                                _manufacturer_data_length);
+    }
+
+    if (_local_name.length() > 0  && 
+        (BLE_STATUS_SUCCESS == ret))
+    {
+        uint8_t length = _local_name.length();
+        ret = setAdvertiseData (BT_DATA_NAME_COMPLETE, 
+                                (const uint8_t*)_local_name.c_str(), 
+                                length);
+    }
+    
+    if (_service_data_length > 0  && 
+        (BLE_STATUS_SUCCESS == ret))
+    {
+        /* Add Service Data (if it will fit) */
+
+        /* A 128-bit Service Data UUID won't fit in an Advertising packet */
+        if (BT_UUID_TYPE_16 != _service_data_uuid.uuid.type)
+        {
+            /* We support service data only for 16-bit service UUID */
+            return BLE_STATUS_NOT_SUPPORTED;
+        }
+
+        uint8_t block_len = sizeof(uint16_t) + _service_data_length;
+        if (1 + block_len > BLE_MAX_ADV_SIZE)
+        {
+            // Service data block is too large.
+            return BLE_STATUS_ERROR_PARAMETER;
+        }
+        
+        ret = setAdvertiseData (BT_DATA_SVC_DATA16, 
+                                _service_data_buf, 
+                                block_len);
+
+        uint8_t *adv_tmp = _service_data_buf;
+
+        memcpy(adv_tmp, &((bt_uuid_16_t*)&_service_data_uuid)->val, sizeof(uint16_t));
+        adv_tmp += 2;
+        memcpy(adv_tmp, _service_data, _service_data_length);
+    }
+    
+    return ret;
+}
+
+BLE_STATUS_T BLEDeviceManager::startAdvertising()
+{
+    int ret;
+    BLE_STATUS_T status;
+    status = _advDataInit();
+    if (BLE_STATUS_SUCCESS != status)
+    {
+        return status;
+    }
+
+    pr_info(LOG_MODULE_BLE, "%s-ad_len%d", __FUNCTION__, _adv_data_idx);
+    if (_state != BLE_PERIPH_STATE_READY)
+        return BLE_STATUS_WRONG_STATE;
+    
+    ret = bt_le_adv_start(&_adv_param, 
+                          _adv_data, _adv_data_idx, 
+                          _scan_rsp_data, _scan_rsp_data_idx);
+    if (0 != ret)
+    {
+        pr_error(LOG_MODULE_APP, "[ADV] Start failed. Error: %d", ret);
+        return BLE_STATUS_WRONG_STATE;
+    }
+    delay(10);
+    _state = BLE_PERIPH_STATE_ADVERTISING;
+    return BLE_STATUS_SUCCESS;
+}
+
+bool BLEDeviceManager::advertising()
+{
+    return (BLE_PERIPH_STATE_ADVERTISING == _state);
+}
+
+BLE_STATUS_T BLEDeviceManager::stopAdvertising()
+{
+    int err_code = 0;
+    BLE_STATUS_T status = BLE_STATUS_WRONG_STATE;
+
+    if (BLE_PERIPH_STATE_ADVERTISING == _state)
+    {
+        err_code = bt_le_adv_stop();
+        status = errorno_to_ble_status(err_code);
+    }
+
+    if (BLE_STATUS_SUCCESS != status)
+        return status;
+
+    _state = BLE_PERIPH_STATE_READY;
+    return BLE_STATUS_SUCCESS;
+}
+
+BLEDevice BLEDeviceManager::central()
+{   
+    BLEDevice temp(&_peer_central);
+    return temp;
+}
+
+BLEDevice BLEDeviceManager::peripheral()
+{
+    BLEDevice temp;
+    for (int i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        if (_peer_peripheral_index >= BLE_MAX_CONN_CFG)
+        {
+            _peer_peripheral_index = 0;
+        }
+        const bt_addr_le_t & addr = _peer_peripheral[_peer_peripheral_index];
+        _peer_peripheral_index++;
+        
+        if (true == BLEUtils::macAddressValid(addr))
+        {
+            temp.setAddress(addr);
+            break;
+        }
+    }
+    return temp;
+}
+
+void BLEDeviceManager::_clearAdvertiseBuffer()
+{
+    
+    // Clear the previous found ADV
+    memset(_peer_temp_adv_buffer, 0, sizeof(_peer_temp_adv_buffer));
+    memset(_peer_temp_adv_data, 0, sizeof(_peer_temp_adv_data));
+    memset(_peer_temp_adv_data_len, 0, sizeof(_peer_temp_adv_data_len));
+    memset(_peer_temp_adv_connectable, 0, sizeof(_peer_adv_connectable));
+    
+    memset(_peer_adv_buffer, 0, sizeof(_peer_adv_buffer));
+    memset(_peer_adv_mill, 0, sizeof(_peer_adv_mill));
+    memset(_peer_adv_data, 0, sizeof(_peer_adv_data));
+    memset(_peer_adv_data_len, 0, sizeof(_peer_adv_data_len));
+    memset(_peer_scan_rsp_data, 0, sizeof(_peer_scan_rsp_data));
+    memset(_peer_scan_rsp_data_len, 0, sizeof(_peer_scan_rsp_data_len));
+    memset(_peer_adv_rssi, 0, sizeof(_peer_adv_rssi));
+    
+}
+
+bool BLEDeviceManager::startScanningWithDuplicates()
+{
+    _adv_duplicate_filter_enabled = false;
+    _scan_param.filter_dup   = BT_HCI_LE_SCAN_FILTER_DUP_ENABLE;
+
+    _clearAdvertiseBuffer();
+    
+    int err = bt_le_scan_start(&_scan_param, ble_central_device_found);
+    if (err)
+    {
+        pr_info(LOG_MODULE_BLE, "Scanning failed to start (err %d)\n", err);
+        return false;
+    }
+    return true;
+}
+
+bool BLEDeviceManager::startScanningNewPeripherals()
+{
+    _adv_duplicate_filter_enabled = true;
+    memset(_peer_duplicate_address_buffer, 0, sizeof(_peer_duplicate_address_buffer));
+    _duplicate_filter_header = _duplicate_filter_tail = 0;
+
+    _clearAdvertiseBuffer();
+    
+    _scan_param.filter_dup   = BT_HCI_LE_SCAN_FILTER_DUP_ENABLE;
+    int err = bt_le_scan_start(&_scan_param, ble_central_device_found);
+    if (err)
+    {
+        pr_info(LOG_MODULE_BLE, "Scanning failed to start (err %d)\n", err);
+        return false;
+    }
+    return true;
+}
+
+bool BLEDeviceManager::stopScanning()
+{
+    int err = bt_le_scan_stop();
+
+    if (err)  // Sid. TODO: KW detected bt_le_scan_stop return only 0.
+    {
+        pr_info(LOG_MODULE_BLE, "Stop LE scan failed (err %d)\n", err);
+        return false;
+    }
+    return true;
+}
+
+void BLEDeviceManager::clearAdvertiseCritical()
+{
+    memset(&_adv_accept_critical, 0, sizeof(_adv_accept_critical));
+    memset(&_adv_accept_device, 0, sizeof(_adv_accept_device));
+    //memset(&_adv_critical_service_uuid, 0, sizeof(_adv_critical_service_uuid));
+}
+
+void BLEDeviceManager::setAdvertiseCritical(String name)
+{
+    _adv_critical_local_name = name;
+    _adv_accept_critical.type = BT_DATA_NAME_COMPLETE;
+    _adv_accept_critical.data_len = name.length();
+    _adv_accept_critical.data = (const uint8_t*)_adv_critical_local_name.c_str();
+}
+
+void BLEDeviceManager::setAdvertiseCritical(BLEService& service)
+{
+    BLEUtils::uuidString2BT(service.uuid(),(bt_uuid_t *)&_adv_critical_service_uuid);
+    uint8_t type = 0;
+    uint8_t length = 0;
+    uint8_t *data = NULL;
+    
+    pr_info(LOG_MODULE_BLE, "ADV Type-%d", _adv_critical_service_uuid.uuid.type);
+    if (BT_UUID_TYPE_16 == _adv_critical_service_uuid.uuid.type)
+    {
+        data = (uint8_t *)&(((bt_uuid_16_t *)&_adv_critical_service_uuid)->val);
+        length = UUID_SIZE_16;
+        type = BT_DATA_UUID16_ALL;
+    }
+    else  // Sid. KW, default is BT_UUID_TYPE_128
+    {
+        data = _adv_critical_service_uuid.val;
+        length = UUID_SIZE_128;
+        type = BT_DATA_UUID128_ALL;
+    }
+    _adv_accept_critical.type = type;
+    _adv_accept_critical.data_len = length;
+    _adv_accept_critical.data = data;
+}
+
+void BLEDeviceManager::setAdvertiseCritical(const char* macaddress)
+{
+    BLEUtils::macAddressString2BT(macaddress, _adv_accept_device);
+}
+
+bool BLEDeviceManager::getDataFromAdvertiseByType(const BLEDevice* device,
+                                                  const uint8_t eir_type, 
+                                                  const uint8_t* &data,
+                                                  uint8_t &data_len) const
+{
+    const uint8_t* adv_data = NULL;
+    uint8_t adv_data_len = 0;
+    bool retval = false;
+    bool scan_response_proced = false;
+    
+    getDeviceAdvertiseBuffer(device->bt_le_address(),
+                             adv_data,
+                             adv_data_len);
+
+    while (NULL != adv_data)
+    {
+        while (adv_data_len > 1)
+        {
+            uint8_t len = adv_data[0];
+            uint8_t type = adv_data[1];
+
+            /* Check for early termination */
+            if ((len == 0) || ((len + 1) > adv_data_len)) {
+                break;
+            }
+
+            if (type == eir_type)
+            {
+                if (len >= BLE_MAX_ADV_SIZE)
+                {
+                    len = BLE_MAX_ADV_SIZE-1;
+                }
+                data = &adv_data[2];
+                data_len = len - 1;
+                retval = true;
+                break;
+            }
+
+            adv_data_len -= len + 1;
+            adv_data += len + 1;
+        }
+        if (retval == true || scan_response_proced == true)
+        {
+            break;
+        }
+        getDeviceScanResponseBuffer(device->bt_le_address(),
+                                    adv_data,
+                                    adv_data_len);
+        scan_response_proced = true;
+    }
+    return retval;
+}
+
+
+bool BLEDeviceManager::hasLocalName(const BLEDevice* device) const
+{
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        return (_local_name.length() != 0);
+    }
+    
+    const uint8_t* local_name = NULL;
+    uint8_t local_name_len = 0;
+    bool retval = getDataFromAdvertiseByType(device, 
+                                             BT_DATA_NAME_COMPLETE,
+                                             local_name, 
+                                             local_name_len);
+    if (false == retval)
+    {
+        retval = getDataFromAdvertiseByType(device, 
+                                            BT_DATA_NAME_SHORTENED,
+                                            local_name, 
+                                            local_name_len);
+    }
+    return retval;
+}
+
+bool BLEDeviceManager::hasManufacturerData(const BLEDevice* device) const
+{
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        return (_manufacturer_data_length != 0);
+    }
+    
+    const uint8_t* manufactgurer_data = NULL;
+    uint8_t manufactgurer_data_len = 0;
+    return getDataFromAdvertiseByType(device, 
+                                      BT_DATA_MANUFACTURER_DATA,
+                                      manufactgurer_data, 
+                                      manufactgurer_data_len);
+}
+
+bool BLEDeviceManager::getManufacturerData (const BLEDevice* device, 
+                                            uint8_t* manu_data, 
+                                            uint8_t&manu_data_len) const
+{
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        return (_manufacturer_data_length != 0);
+    }
+    
+    const uint8_t* manufactgurer_data = NULL;
+    uint8_t manufactgurer_data_len = 0;
+    bool retval = getDataFromAdvertiseByType(device, 
+                                              BT_DATA_MANUFACTURER_DATA,
+                                              manufactgurer_data, 
+                                              manufactgurer_data_len);
+    if (retval)
+    {
+        memcpy (manu_data, manufactgurer_data, manufactgurer_data_len);
+        manu_data_len = manufactgurer_data_len;
+    }
+    return retval;
+}
+
+bool BLEDeviceManager::hasAdvertisedServiceUuid(const BLEDevice* device) const
+{
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        return _has_service_uuid;
+    }
+    
+    uint8_t service_cnt = advertisedServiceUuidCount(device);
+    return (service_cnt > 0);
+}
+
+bool BLEDeviceManager::hasAdvertisedServiceUuid(const BLEDevice* device, int index) const
+{
+    uint8_t service_cnt = advertisedServiceUuidCount(device);
+    return (service_cnt > index);
+}
+
+void BLEDeviceManager::getDeviceAdvertiseBuffer(const bt_addr_le_t* addr, 
+                                                const uint8_t* &adv_data,
+                                                uint8_t &adv_len) const
+{
+    const bt_addr_le_t* temp   = NULL;
+    // Connected device
+    for (int i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        temp = &_peer_peripheral[i];
+        if (bt_addr_le_cmp(temp, addr) == 0)
+        {
+            adv_data = _peer_peripheral_adv_data[i];
+            adv_len = _peer_peripheral_adv_data_len[i];
+            return;
+        }
+    }
+    
+    // Connecting device
+    if (bt_addr_le_cmp(&_wait_for_connect_peripheral, addr) == 0)
+    {
+        adv_data = _wait_for_connect_peripheral_adv_data;
+        adv_len = _wait_for_connect_peripheral_adv_data_len;
+        return;
+    }
+    
+    // Available device
+    if (bt_addr_le_cmp(&_available_for_connect_peripheral, addr) == 0)
+    {
+        adv_data = _available_for_connect_peripheral_adv_data;
+        adv_len = _available_for_connect_peripheral_adv_data_len;
+        return;
+    }
+    return;
+}
+
+void BLEDeviceManager::getDeviceScanResponseBuffer(const bt_addr_le_t* addr, 
+                                                   const uint8_t* &adv_data,
+                                                   uint8_t &adv_len) const
+{
+    const bt_addr_le_t* temp   = NULL;
+    // Connected device
+    for (int i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        temp = &_peer_peripheral[i];
+        if (bt_addr_le_cmp(temp, addr) == 0)
+        {
+            adv_data = _peer_peripheral_scan_rsp_data[i];
+            adv_len = _peer_peripheral_scan_rsp_data_len[i];
+            return;
+        }
+    }
+    
+    // Connecting device
+    if (bt_addr_le_cmp(&_wait_for_connect_peripheral, addr) == 0)
+    {
+        adv_data = _wait_for_connect_peripheral_scan_rsp_data;
+        adv_len = _wait_for_connect_peripheral_scan_rsp_data_len;
+        return;
+    }
+    
+    // Available device
+    if (bt_addr_le_cmp(&_available_for_connect_peripheral, addr) == 0)
+    {
+        adv_data = _available_for_connect_peripheral_scan_rsp_data;
+        adv_len = _available_for_connect_peripheral_scan_rsp_data_len;
+        return;
+    }
+    return;
+}
+
+int BLEDeviceManager::advertisedServiceUuidCount(const BLEDevice* device) const
+{
+    const uint8_t* adv_data = NULL;
+    uint8_t adv_data_len = 0;
+    uint8_t service_cnt = 0;
+    
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        if (_has_service_uuid)
+            service_cnt++;
+        return  service_cnt;
+    }
+    
+    getDeviceAdvertiseBuffer(device->bt_le_address(),
+                             adv_data,
+                             adv_data_len);
+    if (NULL == adv_data)
+    {
+        return service_cnt;
+    }
+    
+    while (adv_data_len > 1)
+    {
+        uint8_t len = adv_data[0];
+        uint8_t type = adv_data[1];
+
+        /* Check for early termination */
+        if (len == 0 || ((len + 1) > adv_data_len))
+        {
+            return service_cnt;
+        }
+
+	/* Sid, 2/15/2017.  Sandeep reported that Apple devices may use
+	   BT_DATA_UUID16_SOME and BT_DATA_UUID128_SOME in addition to ALL.
+	   Practically, these types are same as ALL. */
+        if (type == BT_DATA_UUID16_ALL ||
+            type == BT_DATA_UUID128_ALL ||
+	    type == BT_DATA_UUID16_SOME ||
+	    type == BT_DATA_UUID128_SOME)
+        {
+            service_cnt++;
+        }
+
+        adv_data_len -= len + 1;
+        adv_data += len + 1;
+    }
+    return service_cnt;
+}
+
+String BLEDeviceManager::localName(const BLEDevice* device) const
+{
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        return _local_name;
+    }
+
+    const uint8_t* local_name = NULL;
+    uint8_t local_name_len = 0;
+    String temp("");
+    char local_name_buff[BLE_MAX_ADV_SIZE];
+    bool retval = getDataFromAdvertiseByType(device, 
+                                             BT_DATA_NAME_COMPLETE,
+                                             local_name, 
+                                             local_name_len);
+    if (false == retval)
+    {
+        retval = getDataFromAdvertiseByType(device,
+                                            BT_DATA_NAME_SHORTENED,
+                                            local_name,
+                                            local_name_len);
+    }
+
+    if (true == retval) 
+    {
+        if (local_name_len >= BLE_MAX_ADV_SIZE)
+        {
+            local_name_len = BLE_MAX_ADV_SIZE - 1;
+        }
+        memcpy(local_name_buff, local_name, local_name_len);
+        local_name_buff[local_name_len] = '\0';
+        temp = local_name_buff;
+    }
+    
+    return temp;
+}
+
+String BLEDeviceManager::advertisedServiceUuid(const BLEDevice* device) const
+{
+    return advertisedServiceUuid(device, 0);
+}
+
+String BLEDeviceManager::advertisedServiceUuid(const BLEDevice* device, int index) const
+{
+    const uint8_t* adv_data = NULL;
+    uint8_t adv_data_len = 0;
+    uint8_t service_cnt = 0;
+    bt_uuid_128_t service_uuid;
+    char uuid_string[37];
+    
+    memset(uuid_string, 0, sizeof(uuid_string));
+    
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        // Local device only support advertise 1 service now.
+        if (_has_service_uuid && index == 0)
+        {
+            BLEUtils::uuidBT2String(&_service_uuid.uuid, uuid_string);
+        }
+        return  String(uuid_string);
+    }
+    
+    getDeviceAdvertiseBuffer(device->bt_le_address(),
+                             adv_data,
+                             adv_data_len);
+    
+    if ((uint8_t *)NULL == adv_data)
+    {
+        return String(uuid_string);
+    }
+    
+    while (adv_data_len > 1)
+    {
+        uint8_t len = adv_data[0];
+        uint8_t type = adv_data[1];
+
+        /* Check for early termination */
+        if (len == 0 || ((len + 1) > adv_data_len))
+        {
+            break;
+        }
+
+        if (type == BT_DATA_UUID16_ALL ||
+            type == BT_DATA_UUID128_ALL ||
+	    type == BT_DATA_UUID16_SOME ||
+	    type == BT_DATA_UUID128_SOME)
+        {
+            service_cnt++;
+        }
+        
+        if (index < service_cnt)
+        {
+            if (type == BT_DATA_UUID16_ALL ||
+		type == BT_DATA_UUID16_SOME)
+            {
+                service_uuid.uuid.type = BT_UUID_TYPE_16;
+                memcpy(&BT_UUID_16(&service_uuid.uuid)->val, &adv_data[2], 2);
+            }
+            else
+            {
+                service_uuid.uuid.type = BT_UUID_TYPE_128;
+                memcpy(service_uuid.val, &adv_data[2], 16);
+            }
+            
+            BLEUtils::uuidBT2String(&service_uuid.uuid, uuid_string);
+            
+            break;
+        }
+
+        adv_data_len -= len + 1;
+        adv_data += len + 1;
+    }
+    return String(uuid_string);
+}
+
+int BLEDeviceManager::rssi(const BLEDevice* device) const
+{
+    const bt_addr_le_t* temp   = NULL;
+    const bt_addr_le_t* addr   = device->bt_le_address();
+    // Connected device
+    for (int i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        temp = &_peer_peripheral[i];
+        if (bt_addr_le_cmp(temp, addr) == 0)
+        {
+            return _peer_peripheral_adv_rssi[i];
+        }
+    }
+    
+    // Connecting device
+    if (bt_addr_le_cmp(&_wait_for_connect_peripheral, addr) == 0)
+    {
+        return _wait_for_connect_peripheral_adv_rssi;
+    }
+    
+    // Available device
+    if (bt_addr_le_cmp(&_available_for_connect_peripheral, addr) == 0)
+    {
+        return _available_for_connect_peripheral_adv_rssi;
+    }
+    return 0;
+}
+
+bool BLEDeviceManager::connect(BLEDevice &device)
+{
+    // 
+    uint64_t timestamp = millis();
+    uint64_t timestampcur = timestamp;
+    bool ret = true;
+    if (_available_for_connect_peripheral_connectable == false)
+    {
+        return false;
+    }
+    
+    bt_addr_le_copy(&_wait_for_connect_peripheral, device.bt_le_address());
+    // Buffer the ADV data
+    memcpy(_wait_for_connect_peripheral_adv_data, _available_for_connect_peripheral_adv_data, BLE_MAX_ADV_SIZE);
+    memcpy(_wait_for_connect_peripheral_scan_rsp_data, _available_for_connect_peripheral_scan_rsp_data, BLE_MAX_ADV_SIZE);
+    _wait_for_connect_peripheral_adv_data_len = _available_for_connect_peripheral_adv_data_len;
+    _wait_for_connect_peripheral_scan_rsp_data_len = _available_for_connect_peripheral_scan_rsp_data_len;
+    _wait_for_connect_peripheral_adv_rssi = _available_for_connect_peripheral_adv_rssi;
+
+    startScanningWithDuplicates();
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    // Wait for the connection
+    while (ret && (true == BLEUtils::macAddressValid(_wait_for_connect_peripheral)))
+    {
+        timestampcur = millis();
+        // TODO: dismiss the magic number
+        ret = (timestampcur - timestamp < 3000); // Time out
+    }
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+        
+    if (ret == false)
+    {
+        memset(&_wait_for_connect_peripheral, 0, sizeof(_wait_for_connect_peripheral));
+        if (_connecting == true)
+        {
+            ret = true;
+            while (_connecting == true)
+            {
+                pr_info(LOG_MODULE_BLE, "%s-%d: Connect request sent, wait for response", __FUNCTION__, __LINE__);
+                delay(2000);
+            }
+        }
+        else
+        {
+            stopScanning();
+        }
+    }
+    return ret;
+}
+
+bool BLEDeviceManager::connectToDevice(BLEDevice &device)
+{
+    bt_addr_le_t* temp   = NULL;
+    bt_addr_le_t* unused = NULL;
+    bool link_existed = false;
+    bool retval = false;
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d-1", __FUNCTION__, __LINE__);
+    
+    // Find free peripheral Items
+    for (int i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        temp = &_peer_peripheral[i];
+        if (true == BLEUtils::macAddressValid(*temp))
+        {
+            if (bt_addr_le_cmp(temp, device.bt_le_address()) == 0)
+            {
+                // Connect request has scheduled but connection don't established.
+                //  The central can see the ADV and no need to send connect request.
+                link_existed = true;
+                break;
+            }
+        }
+        else
+        {
+            if (NULL == unused)
+            {
+                unused = temp;
+                // Buffer the ADV data
+                memcpy(_peer_peripheral_adv_data[i], 
+                       _wait_for_connect_peripheral_adv_data, 
+                       BLE_MAX_ADV_SIZE);
+                _peer_peripheral_adv_data_len[i] = _wait_for_connect_peripheral_adv_data_len;
+                memcpy(_peer_peripheral_scan_rsp_data[i], 
+                       _wait_for_connect_peripheral_scan_rsp_data, 
+                       BLE_MAX_ADV_SIZE);
+                _peer_peripheral_scan_rsp_data_len[i] = _wait_for_connect_peripheral_scan_rsp_data_len;
+                _peer_peripheral_adv_rssi[i] = _wait_for_connect_peripheral_adv_rssi;
+            }
+        }
+    }
+    pr_debug(LOG_MODULE_BLE, "%s-%d:link_existed-%d unused-%p", __FUNCTION__, __LINE__, link_existed, unused);
+    
+    if (!link_existed && NULL != unused)
+    {
+    pr_debug(LOG_MODULE_BLE, "%s-%d-Device:%s", __FUNCTION__, __LINE__, device.address().c_str());
+        // Send connect request
+        bt_conn_t* conn = bt_conn_create_le(device.bt_le_address(), device.bt_conn_param());
+        if (NULL != conn)
+        {
+            memcpy(unused, device.bt_le_address(), sizeof(bt_addr_le_t));
+            retval = true;
+            _connecting = true;
+            bt_conn_unref(conn);
+        }
+    }
+    return retval;
+}
+
+String BLEDeviceManager::deviceName(const BLEDevice* device)
+{
+    if (BLEUtils::isLocalBLE(*device) == true)
+    {
+        return _device_name;
+    }
+    return String("");
+}
+
+int BLEDeviceManager::appearance()
+{
+    return _appearance;
+}
+
+BLEDeviceManager* BLEDeviceManager::instance()
+{
+    if (_instance == NULL)
+    {
+        _instance = new BLEDeviceManager();
+        BLE_LIB_ASSERT(_instance != NULL);
+    }
+    return _instance;
+}
+
+void BLEDeviceManager::setEventHandler(BLEDeviceEvent event, 
+                                       BLEDeviceEventHandler eventHandler)
+{
+    if (event < BLEDeviceLastEvent)
+        _device_events[event] = eventHandler;
+}
+
+void BLEDeviceManager::handleConnectEvent(bt_conn_t *conn, uint8_t err)
+{
+    struct bt_conn_info role_info;
+    bt_conn_get_info(conn, &role_info);
+    pr_info(LOG_MODULE_BLE, "%s-%d: role-%d", __FUNCTION__, __LINE__, role_info.role);
+    if (BT_CONN_ROLE_SLAVE == role_info.role)
+    {
+        // Central has established the connection with this peripheral device
+        memcpy(&_peer_central, bt_conn_get_dst(conn), sizeof (bt_addr_le_t));
+    }
+    else
+    {
+        // Peripheral has established the connection with this Central device
+        memset(&_wait_for_connect_peripheral, 0, sizeof(_wait_for_connect_peripheral));
+        _connecting = false;
+    }
+    // The peripheral and central can work as GATT server. Reserve one buffer for peer device
+    BLEProfileManager::instance()->handleConnectedEvent(bt_conn_get_dst(conn));
+    
+    if (NULL != _device_events[BLEConnected])
+    {
+        BLEDevice tempdev(bt_conn_get_dst(conn));
+        _device_events[BLEConnected](tempdev);
+    }
+}
+
+void BLEDeviceManager::handleDisconnectEvent(bt_conn_t *conn, uint8_t reason)
+{
+    struct bt_conn_info role_info;
+    bt_conn_get_info(conn, &role_info);
+    pr_info(LOG_MODULE_BLE, "%s-%d: role-%d", __FUNCTION__, __LINE__, role_info.role);
+    if (BT_CONN_ROLE_SLAVE == role_info.role)
+    {
+        // Central has established the connection with this peripheral device
+        memset(&_peer_central, 0, sizeof (bt_addr_le_t));
+    }
+    else
+    {
+        bt_addr_le_t* temp   = NULL;
+        const bt_addr_le_t* disConnAddr = bt_conn_get_dst(conn);
+        for (int i = 0; i < BLE_MAX_CONN_CFG; i++)
+        {
+            temp = &_peer_peripheral[i];
+            if (bt_addr_le_cmp(temp, disConnAddr) == 0)
+            {
+                memset(temp, 0, sizeof(bt_addr_le_t));
+                memset(_peer_peripheral_adv_data[i], 0, BLE_MAX_ADV_SIZE);
+                _peer_peripheral_adv_data_len[i] = 0;
+                _peer_peripheral_adv_rssi[i] = 0;
+                memset(_peer_peripheral_scan_rsp_data[i], 0, BLE_MAX_ADV_SIZE);
+                _peer_peripheral_scan_rsp_data_len[i] = 0;
+                break;
+            }
+        }
+        // Peripheral has established the connection with this Central device
+        BLEProfileManager::instance()->handleDisconnectedEvent(bt_conn_get_dst(conn));
+    }
+    
+    if (NULL != _device_events[BLEDisconnected])
+    {
+        BLEDevice tempdev(bt_conn_get_dst(conn));
+        _device_events[BLEDisconnected](tempdev);
+    }
+}
+
+void BLEDeviceManager::handleParamUpdated (bt_conn_t *conn, 
+                                           uint16_t interval,
+                                           uint16_t latency, 
+                                           uint16_t timeout)
+{
+    if (NULL != _device_events[BLEConParamUpdate])
+    {
+        BLEDevice tempdev(bt_conn_get_dst(conn));
+        _device_events[BLEConParamUpdate](tempdev);
+    }
+}
+
+bool BLEDeviceManager::advertiseDataProc(uint8_t type, 
+                                         const uint8_t *dataPtr, 
+                                         uint8_t data_len)
+{
+    //Serial1.print("[AD]:");
+    //Serial1.print(type);
+    //Serial1.print(" data_len ");
+    //Serial1.println(data_len);
+    
+    //const bt_data_t zero = {0, 0,0};
+    if (_adv_accept_critical.type == 0 && 
+        _adv_accept_critical.data_len == 0 &&
+        _adv_accept_critical.data == NULL)
+    {
+        // Not set the critical. Accept all.
+        return true;
+    }
+    if (type == _adv_accept_critical.type &&
+        data_len == _adv_accept_critical.data_len &&
+        0 == memcmp(dataPtr, _adv_accept_critical.data, data_len))
+    {
+        // Now Only support 1 critical. Change those code if want support multi-criticals
+        return true;
+    }
+
+    return false;
+}
+
+bool BLEDeviceManager::deviceInDuplicateFilterBuffer(const bt_addr_le_t* addr)
+{
+    bool retVal = false;
+    for (uint8_t i = 0; 
+         i < (sizeof(_peer_duplicate_address_buffer) / sizeof(bt_addr_le_t)); 
+         i++)
+    {
+        if (0 == bt_addr_le_cmp(addr, &_peer_duplicate_address_buffer[i]))
+        {
+            retVal = true;
+            break;
+        }
+    }
+    return retVal;
+}
+
+void BLEDeviceManager::updateDuplicateFilter(const bt_addr_le_t* addr)
+{
+    uint8_t i = (_duplicate_filter_header + 1) % (ARRAY_SIZE(_peer_duplicate_address_buffer));
+    if (deviceInDuplicateFilterBuffer(addr))
+    {
+        return;
+    }
+    bt_addr_le_copy(&_peer_duplicate_address_buffer[_duplicate_filter_header],
+                 addr);
+    if (i == _duplicate_filter_tail)
+    {
+        _duplicate_filter_tail = (_duplicate_filter_tail + 1) % (ARRAY_SIZE(_peer_duplicate_address_buffer));
+    }
+    _duplicate_filter_header = i;
+}
+
+BLEDevice BLEDeviceManager::available()
+{
+    BLEDevice tempdevice;
+    bt_addr_le_t* temp = NULL;
+    uint64_t timestamp = millis();
+    uint8_t index = BLE_MAX_ADV_BUFFER_CFG;
+    uint8_t i = 0;
+    uint64_t max_delta = 0;
+    
+    for (i = 0; i < BLE_MAX_ADV_BUFFER_CFG; i++)
+    {
+        uint64_t timestamp_delta = timestamp - _peer_adv_mill[i];
+        temp = &_peer_adv_buffer[i];
+        if ((timestamp_delta <= 2000) && (max_delta < timestamp_delta) && (_peer_scan_rsp_data_len[i] >= 0 || !_peer_adv_connectable[i]))
+        {
+            // Eable the duplicate filter
+            if (_adv_duplicate_filter_enabled && 
+                true == deviceInDuplicateFilterBuffer(temp))
+            {
+                _peer_adv_mill[i] -= 2000; // Invalid the item
+                continue;
+            }
+            max_delta = timestamp_delta;
+            index = i;
+        }
+    }
+    //pr_debug(LOG_MODULE_BLE, "%s-%d:index %d, i-%d", __FUNCTION__, __LINE__, index, i);
+    
+    if (index < BLE_MAX_ADV_BUFFER_CFG)
+    {
+        temp = &_peer_adv_buffer[index];
+        if (true == BLEUtils::macAddressValid(*temp))
+        {
+            tempdevice.setAddress(*temp);
+            bt_addr_le_copy(&_available_for_connect_peripheral, temp);
+            memcpy(_available_for_connect_peripheral_adv_data, _peer_adv_data[index], BLE_MAX_ADV_SIZE);
+            memcpy(_available_for_connect_peripheral_scan_rsp_data, _peer_scan_rsp_data[index], BLE_MAX_ADV_SIZE);
+            _available_for_connect_peripheral_scan_rsp_data_len = _peer_scan_rsp_data_len[index];
+            _available_for_connect_peripheral_adv_data_len = _peer_adv_data_len[index];
+            _available_for_connect_peripheral_adv_rssi = _peer_adv_rssi[index];
+            _available_for_connect_peripheral_connectable = _peer_adv_connectable[index];
+            //pr_debug(LOG_MODULE_BLE, "%s-%d:Con addr-%s", __FUNCTION__, __LINE__, BLEUtils::macAddressBT2String(*temp).c_str());
+            _peer_adv_mill[index] -= 2000; // Set it as expired
+            if (_adv_duplicate_filter_enabled)
+            {
+                updateDuplicateFilter(temp);
+            }
+        }
+    }
+    return tempdevice;
+}
+
+bool BLEDeviceManager::setAdvertiseBuffer(const bt_addr_le_t* bt_addr,
+                                          const uint8_t *ad, 
+                                          uint8_t data_len,
+                                          int8_t rssi,
+                                          bool connectable)
+{
+    bt_addr_le_t* temp = NULL;
+    uint64_t timestamp = millis();
+    uint8_t index = BLE_MAX_ADV_BUFFER_CFG;
+    uint8_t i = 0;
+    uint64_t max_delta = 0;
+    bool retval = false;
+    //pr_debug(LOG_MODULE_BLE, "%s-%d-1", __FUNCTION__, __LINE__);
+    for (i = 0; i < BLE_MAX_ADV_BUFFER_CFG; i++)
+    {
+        uint64_t timestamp_delta = timestamp - _peer_adv_mill[i];
+        temp = &_peer_adv_buffer[i];
+        if (max_delta < timestamp_delta)
+        {
+            max_delta = timestamp_delta;
+            if (max_delta > 2000) // expired
+            {
+                index = i;
+                _peer_scan_rsp_data_len[index] = -1; // Invalid the scan response
+            }
+        }
+        
+        if (bt_addr_le_cmp(temp, bt_addr) == 0)
+        {
+            // The device alread in the buffer
+            index = i;
+            break;
+        }
+    }
+    //pr_debug(LOG_MODULE_BLE, "%s-%d:index %d, i-%d", __FUNCTION__, __LINE__, index, i);
+    
+    //pr_debug(LOG_MODULE_BLE, "%s-%d-2", __FUNCTION__, __LINE__);
+    if (index < BLE_MAX_ADV_BUFFER_CFG)
+    {
+        temp = &_peer_adv_buffer[index];
+        if (i >= BLE_MAX_ADV_BUFFER_CFG)
+        {
+            memcpy(temp, bt_addr, sizeof (bt_addr_le_t));
+        }
+        if (data_len > BLE_MAX_ADV_SIZE)
+        {
+            data_len = BLE_MAX_ADV_SIZE;
+        }
+        memcpy(_peer_adv_data[index], ad, data_len);
+        _peer_adv_data_len[index] = data_len;
+        _peer_adv_rssi[index] = rssi;
+        // Update the timestamp
+        _peer_adv_mill[index] = timestamp;
+        _peer_adv_connectable[index] = connectable;
+        retval = true;
+    }
+    
+    return retval;
+}
+
+bool BLEDeviceManager::setScanRespBuffer(const bt_addr_le_t* bt_addr,
+                                          const uint8_t *ad, 
+                                          uint8_t data_len,
+                                          int8_t rssi)
+{
+    bt_addr_le_t* temp = NULL;
+    uint64_t timestamp = millis();
+    uint8_t index = BLE_MAX_ADV_BUFFER_CFG;
+    uint8_t i = 0;
+    bool retval = false;
+    //pr_debug(LOG_MODULE_BLE, "%s-%d-1", __FUNCTION__, __LINE__);
+    for (i = 0; i < BLE_MAX_ADV_BUFFER_CFG; i++)
+    {
+        temp = &_peer_adv_buffer[i];
+        
+        if (bt_addr_le_cmp(temp, bt_addr) == 0)
+        {
+            // The device alread in the buffer
+            index = i;
+            break;
+        }
+    }
+    
+    //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    if (index < BLE_MAX_ADV_BUFFER_CFG)
+    {
+        if (data_len > BLE_MAX_ADV_SIZE)
+        {
+            data_len = BLE_MAX_ADV_SIZE;
+        }
+        memcpy(_peer_scan_rsp_data[index], ad, data_len);
+        _peer_scan_rsp_data_len[index] = data_len;
+        //_peer_adv_rssi[index] = rssi;
+        // Update the timestamp
+        _peer_adv_mill[index] = timestamp;
+        retval = true;
+    }
+    
+    return retval;
+}
+
+uint8_t BLEDeviceManager::getTempAdvertiseIndexFromBuffer(const bt_addr_le_t* bt_addr)
+{
+    bt_addr_le_t* temp = NULL;
+    uint8_t i = 0;
+    
+    for (i = 0; i < BLE_MAX_ADV_BUFFER_CFG; i++)
+    {
+        temp = &_peer_temp_adv_buffer[i];
+        
+        if (bt_addr_le_cmp(temp, bt_addr) == 0)
+        {
+            // The device alread in the buffer
+            break;
+        }
+    }
+    
+    return i;
+}
+
+void BLEDeviceManager::setTempAdvertiseBuffer(const bt_addr_le_t* bt_addr, 
+                                              int8_t rssi, 
+                                              const uint8_t *ad, 
+                                              uint8_t data_len, 
+                                              bool connectable)
+{
+    bt_addr_le_t* temp = NULL;
+    uint8_t i = getTempAdvertiseIndexFromBuffer(bt_addr);
+    if (i >= BLE_MAX_ADV_BUFFER_CFG)
+    {
+        _peer_temp_dev_index = (_peer_temp_dev_index + 1) % BLE_MAX_ADV_BUFFER_CFG;
+        i = _peer_temp_dev_index;
+    }
+    
+    temp = &_peer_temp_adv_buffer[i];
+    memcpy(temp, bt_addr, sizeof (bt_addr_le_t));
+    if (data_len > BLE_MAX_ADV_SIZE)
+    {
+        data_len = BLE_MAX_ADV_SIZE;
+    }
+    
+    memcpy(_peer_temp_adv_data[i], ad, data_len);
+    _peer_temp_adv_data_len[i] = data_len;
+    _peer_temp_adv_connectable[i] = connectable;
+    
+    return;
+}
+
+void BLEDeviceManager::advertiseAcceptHandler(const bt_addr_le_t *addr, 
+                                              int8_t rssi, 
+                                              uint8_t type,
+                                              const uint8_t *ad, 
+                                              uint8_t data_len)
+{
+    if (true == BLEUtils::macAddressValid(_wait_for_connect_peripheral))
+    {
+        // Not add to the buffer when try to establish the connection
+        if (true == BLEUtils::macAddressSame(*addr, _wait_for_connect_peripheral))
+        {
+            BLEDevice testdev(addr);
+            stopScanning();
+            connectToDevice(testdev);
+        }
+    }
+    else
+    {
+        const uint8_t *adv_data = ad;
+        uint8_t adv_data_len = data_len;
+        bool connectable = (BT_LE_ADV_NONCONN_IND != type);
+        bool update_advertise_data = true;
+        // The critical is accepted
+        //  Find the oldest and expired buffer 
+        if (BT_LE_ADV_SCAN_RSP == type)
+        {
+            update_advertise_data = false;
+            pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+            if (false == setScanRespBuffer(addr, ad, data_len, rssi))
+            {
+                // Find the device in the ADV temp buffer
+                uint8_t tempIndex = getTempAdvertiseIndexFromBuffer(addr);
+                if (tempIndex < BLE_MAX_ADV_BUFFER_CFG)
+                {
+                    adv_data = _peer_temp_adv_data[tempIndex];
+                    adv_data_len = _peer_temp_adv_data_len[tempIndex];
+                    connectable = _peer_temp_adv_connectable[tempIndex];
+                    update_advertise_data = true;
+                }
+            }
+        }
+        pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+        
+        if (true == update_advertise_data)
+        {
+            if (false == setAdvertiseBuffer(addr, 
+                                            adv_data, 
+                                            adv_data_len, 
+                                            rssi, 
+                                            connectable))
+            {
+                pr_info(LOG_MODULE_BLE, "No buffer to store the ADV\n");
+            }
+            else if (BT_LE_ADV_SCAN_RSP == type)
+            {
+                setScanRespBuffer(addr, ad, data_len, rssi);
+            }
+        }
+    }
+    
+}
+
+void BLEDeviceManager::handleDeviceFound(const bt_addr_le_t *addr, 
+                                         int8_t rssi, 
+                                         uint8_t type,
+                                         const uint8_t *ad, 
+                                         uint8_t data_len)
+{
+    const uint8_t *data = ad;
+    uint8_t real_adv_len = data_len;
+    
+    /* We're only interested in connectable events */
+    //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    // Filter address
+    if (BLEUtils::macAddressValid(_adv_accept_device) == true && 
+       (memcmp(addr->val, _adv_accept_device.val, sizeof (addr->val)) != 0))
+    {
+        pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+        return;
+    }
+    
+    while (data_len > 1)
+    {
+        uint8_t len = data[0];
+
+        /* Check for early termination */
+        if (len == 0)
+        {
+            return;
+        }
+
+        if ((len + 1) > data_len) {    // Sid. KW, cannot be (data_len < 2)
+            pr_info(LOG_MODULE_BLE, "AD malformed\n");
+            return;
+        }
+
+        if (true == advertiseDataProc(data[1], &data[2], len - 1))
+        {
+            advertiseAcceptHandler(addr, rssi, type, ad, real_adv_len);
+            //pr_debug(LOG_MODULE_BLE, "%s-%d: Done", __FUNCTION__, __LINE__);
+            return;
+        }
+
+        data_len -= len + 1;
+        data += len + 1;
+    }
+    //pr_debug(LOG_MODULE_BLE, "%s: done", __FUNCTION__);
+    // Doesn't accept the ADV/scan data
+    // Check it in the buffer
+    if (BT_LE_ADV_SCAN_RSP == type)
+    {
+        // Find the ADV and set response
+        setScanRespBuffer(addr, ad, real_adv_len, rssi);
+    }
+    else
+    {
+        // Add advertise into buffer
+        setTempAdvertiseBuffer(addr, rssi, ad, real_adv_len, BT_LE_ADV_NONCONN_IND != type);
+    }
+    
+}
+
+
+    
diff --git a/libraries/CurieBLE/src/internal/BLEDeviceManager.h b/libraries/CurieBLE/src/internal/BLEDeviceManager.h
new file mode 100644
index 00000000..60aa6f17
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEDeviceManager.h
@@ -0,0 +1,480 @@
+/*
+  BLE Device API
+  Copyright (c) 2016 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+#ifndef ARDUINO_BLE_DEVICE_MANAGER_H
+#define ARDUINO_BLE_DEVICE_MANAGER_H
+
+#include <Arduino.h>
+
+class BLEDeviceManager
+{
+  public:
+    /**
+     * @brief   The BLE device constructure
+     *
+     * @param   bleaddress  BLE device address
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLEDeviceManager();
+    
+    virtual ~BLEDeviceManager();
+
+    
+    /**
+     * @brief Initiliaze the BLE hardware
+     *
+     * @return bool indicating success or error
+     *
+     * @note  This method are for real BLE device. 
+     *          Not for peer BLE device.
+     */
+    bool begin(BLEDevice *device);
+    
+    /**
+     * @brief   Poll for events
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  This method are for real BLE device. 
+     *          Not for peer BLE device.
+     */
+    void poll(); // Do we need add the return value or 
+                 //  input parameter to get the events?
+                 // Events may inlcue:
+                 //  GAP : Connected, Disconnected, Update connetion parameter
+                 //  GATT: Discovered
+    
+    /**
+     * @brief   Deinitiliaze the BLE hardware
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  This method are for real BLE device. 
+     *          Not for peer BLE device.
+     */
+    void end(); 
+
+    /**
+     * @brief   Is the device connected with another BLE device.
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    bool connected(const BLEDevice *device) const;
+    
+    /**
+     * @brief   Disconnect the connected device/s.
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  The BLE may connected multiple devices.
+     *          This call will disconnect all conected devices.
+     */
+    bool disconnect(BLEDevice *device);
+    
+    void setEventHandler(BLEDeviceEvent event, 
+                         BLEDeviceEventHandler eventHandler);
+    /**
+     * @brief   Set the service UUID that the BLE Peripheral Device advertises
+     *
+     * @param[in] advertisedServiceUuid  16-bit or 128-bit UUID to advertis
+     *                               (in string form)
+     *
+     * @note This method must be called before the begin method
+     *        Only for peripheral mode.
+     */
+    void setAdvertisedServiceUuid(const char* advertisedServiceUuid);
+        
+    /**
+     * @brief   Set the service UUID that is solicited in the BLE Peripheral 
+     *           Device advertises
+     *
+     * @param[in] advertisedServiceUuid  16-bit or 128-bit UUID to advertis
+     *                               (in string form)
+     *
+     * @note This method must be called before the begin method
+     *        Only for peripheral mode.
+     */
+    void setServiceSolicitationUuid(const char* serviceSolicitationUuid);
+    void setAdvertisedServiceData(const bt_uuid_t* serviceDataUuid,
+                                  const uint8_t* serviceData,
+                                  uint8_t serviceDataLength);
+
+    /**
+     * @brief   Set the manufacturer data in the BLE Peripheral Device advertises
+     *
+     * @param[in] manufacturerData          The data about manufacturer will 
+     *                                       be set in advertisement
+     * @param[in] manufacturerDataLength    The length of the manufacturer data
+     *
+     * @note This method must be called before the begin method
+     *        Only for peripheral mode.
+     */
+    void setManufacturerData(const unsigned char manufacturerData[], 
+                             unsigned char manufacturerDataLength);
+    bool getManufacturerData (const BLEDevice* device, 
+                              uint8_t* manu_data, 
+                              uint8_t&manu_data_len) const;
+    bool hasManufacturerData(const BLEDevice* device) const;
+    
+    /**
+     * Set the local name that the BLE Peripheral Device advertises
+     *
+     * @param[in] localName  local name to advertise
+     *
+     * @note This method must be called before the begin method
+     */
+    void setLocalName(const char *localName);
+
+    /**
+     * @brief   Set advertising interval
+     *
+     * @param[in]   advertisingInterval    Advertising Interval in ms
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void setAdvertisingInterval(float advertisingInterval);
+    
+    /**
+     * @brief   Set the connection parameters and send connection 
+     *           update request in both BLE peripheral and central
+     *
+     * @param[in]   intervalmin     Minimum Connection Interval (ms)
+     *
+     * @param[in]   intervalmax     Maximum Connection Interval (ms)
+     *
+     * @param[in]   latency         Connection Latency
+     *
+     * @param[in]   timeout         Supervision Timeout (ms)
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    int setConnectionInterval (BLEDevice *device);
+    void getConnectionInterval(BLEDevice *device, 
+                               bt_le_conn_param* conn_param);
+    /**
+     * @brief   Set TX power of the radio in dBM
+     *
+     * @param[in]   tx_power    The antenna TX power
+     *
+     * @return boolean_t true if established connection, otherwise false
+     */
+    bool setTxPower(int txPower);
+    
+    /**
+     * @brief   Set advertising type as connectable/non-connectable
+     *
+     * @param[in]   connectable     true  - The device connectable
+     *                              false - The device non-connectable
+     *
+     * @return  none
+     *
+     * @note  Only for peripheral mode.
+     *         Default value is connectable
+     */
+    void setConnectable(bool connectable);
+
+    /**
+     * @brief   Set the value of the device name characteristic 
+     *
+     * @param[in] device  User-defined name string for this device.  Truncated if
+     *                   more than maximum allowed string length (20 bytes).
+     *
+     * @note This method must be called before the begin method
+     *       If device name is not set, a default name will be used
+     */
+    void setDeviceName(const char* deviceName);
+    void setDeviceName();
+    /**
+     * @brief   Set the appearance type for the BLE Peripheral Device
+     *
+     * See https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.gap.appearance.xml
+     * for available options.
+     *
+     * @param[in] appearance Appearance category identifier as defined by BLE Standard
+     *
+     * @return BleStatus indicating success or error
+     *
+     * @note This method must be called before the begin method
+     */
+    void setAppearance(unsigned short appearance);
+
+    /**
+     * @brief   Add a Service to the BLE Peripheral Device
+     *
+     * @param[in] attribute     The service that will add to Peripheral
+     *
+     * @return BLE_STATUS_T     Indicating success or error type
+     *
+     * @note This method must be called before the begin method
+     */
+    BLE_STATUS_T addService(BLEService& attribute);
+    
+    /**
+     * @brief   Construct the ADV data and start send advertisement
+     *
+     * @param   none
+     *
+     * @return  BLE_STATUS_T       0 - Success. Others - error code
+     *
+     * @note  none
+     */
+    BLE_STATUS_T startAdvertising();
+
+    bool advertising();
+    
+    /**
+     * @brief   Stop send advertisement
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLE_STATUS_T stopAdvertising();
+
+    /**
+     * @brief   Get currently connected central 
+     *
+     * @return BLEDeviceManager Connected central device
+     *
+     * @note  Peripheral mode only
+     */
+    BLEDevice central();
+    
+    /**
+     * @brief   Get currently connected peripheral 
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  Central mode only. How to distinguish the peripheral?
+     */
+    BLEDevice peripheral();
+    
+    operator bool() const;
+
+    // central mode
+    void clearAdvertiseCritical();
+    void setAdvertiseCritical(String name);
+    void setAdvertiseCritical(BLEService& service);
+    void setAdvertiseCritical(const char* macaddress);
+    bool startScanningNewPeripherals(); // start scanning for new peripherals, don't report the detected ones
+    bool startScanningWithDuplicates(); // start scanning for peripherals, and report all duplicates
+    bool stopScanning(); // stop scanning for peripherals
+    
+    void setAcceptAdvertiseLocalName(String name);
+    void setAcceptAdvertiseLocalName(BLEService& service);
+    void setAcceptAdvertiseCallback(String name);
+    
+    BLEDevice available(); // retrieve a discovered peripheral
+
+    bool hasLocalName(const BLEDevice* device) const; // does the peripheral advertise a local name
+    bool hasAdvertisedServiceUuid(const BLEDevice* device) const; // does the peripheral advertise a service
+    bool hasAdvertisedServiceUuid(const BLEDevice* device, int index) const; // does the peripheral advertise a service n
+    int advertisedServiceUuidCount(const BLEDevice* device) const; // number of services the peripheral is advertising
+
+    String localName(const BLEDevice* device) const; // returns the advertised local name as a String
+    String advertisedServiceUuid(const BLEDevice* device) const; // returns the advertised service as a UUID String
+    String advertisedServiceUuid(const BLEDevice* device, int index) const; // returns the nth advertised service as a UUID String
+
+    int rssi(const BLEDevice* device) const; // returns the RSSI of the peripheral at discovery
+
+    bool connect(BLEDevice &device); // connect to the peripheral
+    bool connectToDevice(BLEDevice &device);
+
+    String deviceName(const BLEDevice* device); // read the device name attribute of the peripheral, and return String value
+    int appearance(); // read the appearance attribute of the peripheral and return value as int
+
+    static BLEDeviceManager* instance();
+    
+    void handleConnectEvent(bt_conn_t *conn, uint8_t err);
+    void handleDisconnectEvent(bt_conn_t *conn, uint8_t reason);
+    void handleParamUpdated (bt_conn_t *conn, 
+                             uint16_t interval,
+                             uint16_t latency, 
+                             uint16_t timeout);
+    void handleDeviceFound(const bt_addr_le_t *addr, 
+                           int8_t rssi, 
+                           uint8_t type,
+                           const uint8_t *ad, 
+                           uint8_t data_len);
+    
+protected:
+    
+private:
+    BLE_STATUS_T setAdvertiseData (uint8_t type, 
+                                   const uint8_t* data, 
+                                   uint8_t length);
+    BLE_STATUS_T _advDataInit(void);
+    void _clearAdvertiseBuffer();
+    bool advertiseDataProc(uint8_t type, 
+                           const uint8_t *dataPtr, 
+                           uint8_t data_len);
+    bool setAdvertiseBuffer(const bt_addr_le_t* bt_addr,
+                            const uint8_t *ad, 
+                            uint8_t data_len,
+                            int8_t rssi,
+                            bool connectable);
+    void getDeviceAdvertiseBuffer(const bt_addr_le_t* addr, 
+                                  const uint8_t* &adv_data,
+                                  uint8_t &adv_len) const;
+    bool setScanRespBuffer(const bt_addr_le_t* bt_addr,
+                           const uint8_t *ad, 
+                           uint8_t data_len,
+                           int8_t rssi);
+    void getDeviceScanResponseBuffer(const bt_addr_le_t* addr, 
+                                     const uint8_t* &adv_data,
+                                     uint8_t &adv_len) const;
+    bool getDataFromAdvertiseByType(const BLEDevice* device,
+                                    const uint8_t eir_type, 
+                                    const uint8_t* &data,
+                                    uint8_t &data_len) const;
+    bool disconnectSingle(const bt_addr_le_t *peer);
+    void updateDuplicateFilter(const bt_addr_le_t* addr);    
+    bool deviceInDuplicateFilterBuffer(const bt_addr_le_t* addr);
+    void advertiseAcceptHandler(const bt_addr_le_t *addr, 
+                                int8_t rssi, 
+                                uint8_t type,
+                                const uint8_t *ad, 
+                                uint8_t data_len);
+    void setTempAdvertiseBuffer(const bt_addr_le_t* bt_addr, 
+                                int8_t rssi, 
+                                const uint8_t *ad, 
+                                uint8_t data_len,
+                                bool connectable);
+    uint8_t getTempAdvertiseIndexFromBuffer(const bt_addr_le_t* bt_addr);
+
+private:
+    uint16_t   _min_conn_interval;
+    uint16_t   _max_conn_interval;
+    bt_addr_le_t _local_bda;
+    char       _device_name[BLE_MAX_DEVICE_NAME + 1];
+    
+    // For Central
+    bt_le_scan_param_t _scan_param;     // Scan parameter
+    bt_addr_le_t _peer_adv_buffer[BLE_MAX_ADV_BUFFER_CFG];   // Accepted peer device adress
+    uint64_t     _peer_adv_mill[BLE_MAX_ADV_BUFFER_CFG];     // The ADV found time stamp
+    uint8_t    _peer_adv_data[BLE_MAX_ADV_BUFFER_CFG][BLE_MAX_ADV_SIZE];
+    uint8_t    _peer_adv_data_len[BLE_MAX_ADV_BUFFER_CFG];
+    uint8_t    _peer_scan_rsp_data[BLE_MAX_ADV_BUFFER_CFG][BLE_MAX_ADV_SIZE];
+    int8_t     _peer_scan_rsp_data_len[BLE_MAX_ADV_BUFFER_CFG];
+    int8_t     _peer_adv_rssi[BLE_MAX_ADV_BUFFER_CFG];
+    bool       _peer_adv_connectable[BLE_MAX_ADV_BUFFER_CFG];
+    
+    // The accept critical may include in scan response
+    bt_addr_le_t _peer_temp_adv_buffer[BLE_MAX_ADV_BUFFER_CFG];
+    uint8_t    _peer_temp_dev_index;
+    uint8_t    _peer_temp_adv_data[BLE_MAX_ADV_BUFFER_CFG][BLE_MAX_ADV_SIZE];
+    uint8_t    _peer_temp_adv_data_len[BLE_MAX_ADV_BUFFER_CFG];
+    bool       _peer_temp_adv_connectable[BLE_MAX_ADV_BUFFER_CFG];
+    
+    // The critical for central scan
+    bt_data_t   _adv_accept_critical;   // The filters for central device
+    String  _adv_critical_local_name;
+    bt_uuid_128_t _adv_critical_service_uuid;
+    bt_addr_le_t _adv_accept_device;
+    
+    bt_addr_le_t _wait_for_connect_peripheral;
+    uint8_t    _wait_for_connect_peripheral_adv_data[BLE_MAX_ADV_SIZE];
+    uint8_t    _wait_for_connect_peripheral_adv_data_len;
+    uint8_t    _wait_for_connect_peripheral_scan_rsp_data[BLE_MAX_ADV_SIZE];
+    uint8_t    _wait_for_connect_peripheral_scan_rsp_data_len;
+    int8_t     _wait_for_connect_peripheral_adv_rssi;
+    
+    bt_addr_le_t _available_for_connect_peripheral;
+    uint8_t    _available_for_connect_peripheral_adv_data[BLE_MAX_ADV_SIZE];
+    uint8_t    _available_for_connect_peripheral_adv_data_len;
+    uint8_t    _available_for_connect_peripheral_scan_rsp_data[BLE_MAX_ADV_SIZE];
+    uint8_t    _available_for_connect_peripheral_scan_rsp_data_len;
+    int8_t     _available_for_connect_peripheral_adv_rssi;
+    bool       _available_for_connect_peripheral_connectable;
+    volatile bool    _connecting;
+    
+    // For peripheral
+    struct bt_le_adv_param _adv_param;
+    bool _has_service_uuid;
+    bt_uuid_128_t _service_uuid;
+    bool _has_service_solicit_uuid;
+    bt_uuid_128_t _service_solicit_uuid;
+    uint16_t   _appearance;
+    uint8_t    _manufacturer_data[BLE_MAX_ADV_SIZE];
+    uint8_t    _manufacturer_data_length;
+    bt_uuid_128_t _service_data_uuid;
+    uint8_t     _service_data[BLE_MAX_ADV_SIZE];
+    uint8_t     _service_data_buf[BLE_MAX_ADV_SIZE];
+    uint8_t     _service_data_length;
+    
+    // ADV data for peripheral
+    uint8_t     _adv_type;
+    bt_data_t   _adv_data[6];  // KW: fount _advDataInit() can use 6 slots.
+    size_t      _adv_data_idx;
+    bt_data_t   _scan_rsp_data[6];
+    size_t      _scan_rsp_data_idx;
+    
+    String      _local_name;
+    // Peripheral states
+    enum BLEPeripheralState {
+        BLE_PERIPH_STATE_NOT_READY = 0,
+        BLE_PERIPH_STATE_READY,
+        BLE_PERIPH_STATE_ADVERTISING,
+        BLE_PERIPH_STATE_CONNECTED,
+    };
+
+    BLEPeripheralState _state;
+    
+    // Local
+    static BLEDeviceManager* _instance;
+    BLEDevice *_local_ble;
+    // Connected device object
+    bt_addr_le_t _peer_central;
+    bt_addr_le_t _peer_peripheral[BLE_MAX_CONN_CFG];
+    uint8_t      _peer_peripheral_index;
+    uint8_t    _peer_peripheral_adv_data[BLE_MAX_CONN_CFG][BLE_MAX_ADV_SIZE];
+    uint8_t    _peer_peripheral_adv_data_len[BLE_MAX_CONN_CFG];
+    uint8_t    _peer_peripheral_scan_rsp_data[BLE_MAX_CONN_CFG][BLE_MAX_ADV_SIZE];
+    uint8_t    _peer_peripheral_scan_rsp_data_len[BLE_MAX_CONN_CFG];
+    uint8_t    _peer_peripheral_adv_rssi[BLE_MAX_CONN_CFG];
+    bt_addr_le_t _peer_duplicate_address_buffer[BLE_MAX_ADV_FILTER_SIZE_CFG];
+    uint8_t     _duplicate_filter_header;
+    uint8_t     _duplicate_filter_tail;
+    bool        _adv_duplicate_filter_enabled;
+
+    BLEDeviceEventHandler _device_events[BLEDeviceLastEvent];
+};
+
+#endif
diff --git a/libraries/CurieBLE/src/internal/BLEProfileManager.cpp b/libraries/CurieBLE/src/internal/BLEProfileManager.cpp
new file mode 100644
index 00000000..aba7981b
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEProfileManager.cpp
@@ -0,0 +1,1134 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <errno.h>
+#include "BLECommon.h"
+#include "BLEProfileManager.h"
+#include "BLECharacteristicImp.h"
+
+#include "BLECallbacks.h"
+#include "BLEUtils.h"
+
+BLEDevice BLE(BLEUtils::bleGetLoalAddress());
+
+BLEProfileManager* BLEProfileManager::_instance = NULL;
+
+BLEProfileManager* BLEProfileManager::instance()
+{
+    if (NULL == _instance)
+    {
+        _instance = new BLEProfileManager();
+        BLE_LIB_ASSERT(_instance != NULL);
+    }
+    //pr_debug(LOG_MODULE_BLE, "%s-%d: %p", __FUNCTION__, __LINE__, _instance);
+    return _instance;
+}
+
+BLEProfileManager::BLEProfileManager ():
+    _start_discover(false),
+    _discovering(false),
+    _discover_rsp_timestamp(0),
+    _cur_discover_service(NULL),
+    _discover_one_service(false),
+    _reading(false),
+    _attr_base(NULL),
+    _attr_index(0),
+    _profile_registered(false),
+    _disconnect_bitmap(0)
+{
+    //memset(_service_header_array, 0, sizeof(_service_header_array));
+    memset(_discover_params, 0, sizeof(_discover_params));
+    memset(_discover_uuid, 0, sizeof(_discover_uuid));
+    
+    memset(_addresses, 0, sizeof(_addresses));
+    memset(&_discovering_ble_addresses, 0, sizeof(_discovering_ble_addresses));
+    memset(&_read_params, 0, sizeof(_read_params));
+    memset(&_read_service_header, 0, sizeof(_read_service_header));
+    bt_addr_le_copy(&_addresses[BLE_MAX_CONN_CFG], BLEUtils::bleGetLoalAddress());
+    for (int i = 0; i <= BLE_MAX_CONN_CFG; i++)
+    {
+        _service_header_array[i].next = NULL;
+        _service_header_array[i].value = NULL;
+    }
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d: Construct", __FUNCTION__, __LINE__);
+}
+
+BLEProfileManager::~BLEProfileManager (void)
+{
+    if (_attr_base)
+    {
+      free(_attr_base);
+      _attr_base = (bt_gatt_attr_t *)NULL;
+    }
+    ServiceReadLinkNodePtr node = link_node_get_first(&_read_service_header);
+    while (NULL != node)
+    {
+        link_node_remove_first(&_read_service_header);
+        node = link_node_get_first(&_read_service_header);
+    }
+}
+
+BLEServiceImp *
+BLEProfileManager::addService (BLEDevice &bledevice, BLEService& service)
+{
+    
+    BLEServiceLinkNodeHeader* serviceheader = getServiceHeader(bledevice);
+    if (NULL == serviceheader)
+    {
+        int index = getUnusedIndex();
+        if (index >= BLE_MAX_CONN_CFG)
+        {
+            return NULL;
+        }
+        serviceheader = &_service_header_array[index];
+        bt_addr_le_copy(&_addresses[index], bledevice.bt_le_address());
+    }
+    BLEServiceImp *serviceImp = NULL;//this->service(bledevice, service.uuid());
+    //if (NULL != serviceImp)
+    //{
+        // The service alreay exist
+    //    return serviceImp;
+    //}
+    
+    //if (NULL == serviceImp) // May trigger KW warning
+    {
+        serviceImp = service.fetchOutLocalServiceImp();
+    }
+    
+    if (NULL == serviceImp)
+    {
+        serviceImp = new BLEServiceImp(service);
+        if (NULL == serviceImp)
+        {
+            return serviceImp;
+        }
+    }
+    
+    BLEServiceNodePtr node = link_node_create(serviceImp);
+    if (NULL == node)
+    {
+        delete serviceImp;
+        return NULL;
+    }
+    link_node_insert_last(serviceheader, node);
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    return serviceImp;
+}
+
+BLEServiceImp *
+BLEProfileManager::addService (BLEDevice &bledevice, const bt_uuid_t* uuid)
+{
+    BLEService svc_obj(uuid);
+    return addService(bledevice, svc_obj);
+}
+
+BLEProfileManager::BLEServiceLinkNodeHeader* BLEProfileManager::getServiceHeader(const BLEDevice &bledevice)
+{
+    int i;
+    for (i = 0; i <= BLE_MAX_CONN_CFG; i++)
+    {
+        if ((bt_addr_le_cmp(bledevice.bt_le_address(), &_addresses[i]) == 0))
+        //if (true == BLEUtils::macAddressSame(*bledevice.bt_le_address(), _addresses[i]))
+        {
+            break;
+        }
+    }
+    if (i > BLE_MAX_CONN_CFG)
+    {
+        return NULL;
+    }
+    return &_service_header_array[i];
+}
+
+const BLEProfileManager::BLEServiceLinkNodeHeader* BLEProfileManager::getServiceHeader(const BLEDevice &bledevice) const
+{
+    int i;
+    for (i = 0; i <= BLE_MAX_CONN_CFG; i++)
+    {
+        if ((bt_addr_le_cmp(bledevice.bt_le_address(), &_addresses[i]) == 0))
+        //if (true == BLEUtils::macAddressSame(*bledevice.bt_le_address(), _addresses[i]))
+        {
+            break;
+        }
+    }
+    if (i > BLE_MAX_CONN_CFG)
+    {
+        return NULL;
+    }
+    return &_service_header_array[i];
+}
+
+int BLEProfileManager::getUnusedIndex()
+{
+    int i;
+    for (i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        if (BLEUtils::macAddressValid(_addresses[i]) == false)
+        {
+            break;
+        }
+    }
+    
+    return i;
+}
+
+int BLEProfileManager::getAttributeCount(BLEDevice &bledevice)
+{
+    BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        return 0;
+    }
+    
+    int attrCounter = 0;
+    
+    BLEServiceNodePtr node = serviceHeader->next;
+    while (node)
+    {
+        BLEServiceImp *service = node->value;
+        attrCounter += service->getAttributeCount();
+        node = node->next;
+    }
+    return attrCounter;
+}
+
+int BLEProfileManager::characteristicCount(const BLEDevice &bledevice) const
+{
+    const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        return 0;
+    }
+    
+    int counter = 0;
+    
+    BLEServiceNodePtr node = serviceHeader->next;
+    while (node)
+    {
+        BLEServiceImp *service = node->value;
+        counter += service->getCharacteristicCount();
+        node = node->next;
+    }
+    return counter;
+}
+
+int BLEProfileManager::serviceCount(const BLEDevice &bledevice) const
+{
+    const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        return 0;
+    }
+    return link_list_size(serviceHeader);
+}
+
+int BLEProfileManager::registerProfile(BLEDevice &bledevice)
+{
+    int ret = 0;
+    
+    bt_gatt_attr_t *start;
+    BleStatus err_code = BLE_STATUS_SUCCESS;
+    
+    // The device is local BLE device. Register the service only allow local BLE device
+    BLEServiceLinkNodeHeader* serviceHeader = &_service_header_array[BLE_MAX_CONN_CFG];
+    if ((bt_addr_le_cmp(bledevice.bt_le_address(), &_addresses[BLE_MAX_CONN_CFG]) != 0))
+    {
+        return BLE_STATUS_FORBIDDEN;
+    }
+    
+    int attr_counter = getAttributeCount(bledevice);
+    if (0 == attr_counter)
+    {
+        return BLE_STATUS_NO_SERVICE;
+    }
+    
+    if (NULL == _attr_base)
+    {
+        _attr_base = (bt_gatt_attr_t *)malloc(attr_counter * sizeof(bt_gatt_attr_t));
+        if (NULL == _attr_base) {
+            err_code = BLE_STATUS_NO_MEMORY;
+        }
+        else 
+        {
+            memset((void *)_attr_base, 0x00, (attr_counter * sizeof(bt_gatt_attr_t)));
+            pr_info(LOG_MODULE_BLE, "_attr_base_-%p, size-%d, attr_counter-%d", _attr_base, sizeof(_attr_base), attr_counter);
+        }
+    }
+    
+    if (BLE_STATUS_SUCCESS != err_code)
+    {
+        if (NULL != _attr_base)
+        {
+            free(_attr_base);
+        }
+        return err_code;
+    }
+    
+    pr_info(LOG_MODULE_BLE, "_attr_base_-%p", _attr_base);
+
+    BLEServiceNodePtr node = serviceHeader->next;
+    while (node)
+    {
+        BLEServiceImp *service = node->value;
+        start = _attr_base + _attr_index;
+        service->updateProfile(start, _attr_index);
+        node = node->next;
+    }
+    
+#if 0
+    // Start debug
+    int i;
+
+    for (i = 0; i < _attr_index; i++) {
+        {
+            pr_info(LOG_MODULE_APP, "gatt-: i %d, type %d, u16 0x%x",
+                   i, 
+                   _attr_base[i].uuid->type,
+                   BT_UUID_16(_attr_base[i].uuid)->val);
+        }
+    }
+    
+    delay(1000);
+    // End for debug
+#endif
+    
+    ret = bt_gatt_register(_attr_base,
+                            _attr_index);
+    pr_debug(LOG_MODULE_APP, "%s: ret, %d,_attr_index-%d", __FUNCTION__, ret, _attr_index);
+    if (0 == ret)
+    {
+        _profile_registered = true;
+    }
+    return ret;
+}
+
+void BLEProfileManager::clearProfile(BLEServiceLinkNodeHeader* serviceHeader)
+{
+    if (NULL == serviceHeader)
+    {
+        return;
+    }
+    
+    BLEServiceNodePtr node = link_node_get_first(serviceHeader);
+    
+    while (NULL != node)
+    {
+        BLEServiceImp *service = node->value;
+        delete service;
+        link_node_remove_first(serviceHeader);
+        node = link_node_get_first(serviceHeader);
+    }
+}
+
+BLEDescriptorImp* BLEProfileManager::descriptor(const BLEDevice &bledevice, uint16_t handle)
+{
+    BLEDescriptorImp* descriptorImp = NULL;
+    BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    
+    BLEServiceNodePtr node = serviceHeader->next;
+    while (node != NULL)
+    {
+        BLEServiceImp *service = node->value;
+        descriptorImp = service->descriptor(handle);
+        if (NULL != descriptorImp)
+        {
+            break;
+        }
+        node = node->next;
+    }
+    return descriptorImp;
+}
+
+BLECharacteristicImp* BLEProfileManager::characteristic(const BLEDevice &bledevice, int index)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    int counter = 0;
+    BLEServiceNodePtr node = serviceHeader->next;
+    while (node != NULL)
+    {
+        BLEServiceImp *service = node->value;
+        int counterTmp = service->getCharacteristicCount();
+        if (counter + counterTmp > index)
+        {
+            break;
+        }
+        counter += counterTmp;
+        node = node->next;
+    }
+
+    if (NULL != node)
+    {
+        BLEServiceImp *service = node->value;
+        characteristicImp = service->characteristic(index - counter);
+    }
+    return characteristicImp;
+}
+
+BLECharacteristicImp* BLEProfileManager::characteristic(const BLEDevice &bledevice, uint16_t handle)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    
+    BLEServiceNodePtr node = serviceHeader->next;
+    while (node != NULL)
+    {
+        BLEServiceImp *service = node->value;
+        characteristicImp = service->characteristic(handle);
+        if (NULL != characteristicImp)
+        {
+            break;
+        }
+        node = node->next;
+    }
+    return characteristicImp;
+}
+
+BLECharacteristicImp* BLEProfileManager::characteristic(const BLEDevice &bledevice, 
+                                                        const char* uuid, 
+                                                        int index)
+{
+    BLECharacteristicImp* characteristicImp = characteristic(bledevice, index);
+    if (NULL != characteristicImp)
+    {
+        if (false == characteristicImp->compareUuid(uuid))
+        {
+            // UUID not align
+            characteristicImp = NULL;
+        }
+    }
+    return characteristicImp;
+}
+
+BLECharacteristicImp* BLEProfileManager::characteristic(const BLEDevice &bledevice, 
+                                                        const char* uuid)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    BLEServiceNodePtr node = serviceHeader->next;
+    while (node != NULL)
+    {
+        BLEServiceImp *service = node->value;
+        characteristicImp = service->characteristic(uuid);
+        if (NULL != characteristicImp)
+        {
+            break;
+        }
+        node = node->next;
+    }
+    
+    return characteristicImp;
+}
+
+BLEServiceImp* BLEProfileManager::service(const BLEDevice &bledevice, const char * uuid) const
+{
+    bt_uuid_128_t uuid_tmp;
+    BLEUtils::uuidString2BT(uuid, (bt_uuid_t *)&uuid_tmp);
+    //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    return service(bledevice, (const bt_uuid_t *)&uuid_tmp);
+}
+
+BLEServiceImp* BLEProfileManager::service(const BLEDevice &bledevice, const bt_uuid_t* uuid) const
+{
+    BLEServiceImp* serviceImp = NULL;
+    #if 1
+    const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    BLEServiceNodePtr node = serviceHeader->next;
+    
+    // Just for debug
+    char uuid_tmp[37];
+    BLEUtils::uuidBT2String(uuid, uuid_tmp);
+    pr_debug(LOG_MODULE_BLE, "%s-%d: %s", __FUNCTION__, __LINE__, uuid_tmp);
+    
+    while (node != NULL)
+    {
+        serviceImp = node->value;
+        if (true == serviceImp->compareUuid(uuid))
+        {
+            break;
+        }
+        node = node->next;
+    }
+    
+    if (NULL == node)
+    {
+        serviceImp = NULL;
+    }
+    #endif
+    return serviceImp;
+}
+
+BLEServiceImp* BLEProfileManager::service(const BLEDevice &bledevice, int index) const
+{
+    BLEServiceImp* serviceImp = NULL;
+    const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    BLEServiceNodePtr node = serviceHeader->next;
+    
+    while (node != NULL)
+    {
+        if (0 == index)
+        {
+            break;
+        }
+        index--;
+        node = node->next;
+    }
+    if (NULL == node)
+    {
+        serviceImp = NULL;
+    }
+    else
+    {
+        serviceImp = node->value;
+    }
+    return serviceImp;
+}
+
+BLEServiceImp* BLEProfileManager::getServiceBySubHandle(const BLEDevice &bledevice, uint16_t handle) const
+{
+    BLEServiceImp* serviceImp = NULL;
+    uint16_t start_handle;
+    uint16_t end_handle;
+
+    const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    BLEServiceNodePtr node = serviceHeader->next;
+    
+    while (node != NULL)
+    {
+        serviceImp = node->value;
+        start_handle = serviceImp->startHandle();
+        end_handle = serviceImp->endHandle();
+        if (handle >= start_handle && handle <= end_handle)
+        {
+            break;
+        }
+        node = node->next;
+    }
+    
+    if (NULL == node)
+    {
+        serviceImp = NULL;
+    }
+
+    return serviceImp;
+}
+
+void BLEProfileManager::handleConnectedEvent(const bt_addr_le_t* deviceAddr)
+{
+    int index = getUnusedIndex();
+    if (index >= BLE_MAX_CONN_CFG)
+    {
+        //BLE_STATUS_NO_MEMORY
+        return;
+    }
+    bt_addr_le_copy(&_addresses[index], deviceAddr);
+}
+
+void BLEProfileManager::handleDisconnectedEvent(const bt_addr_le_t* deviceAddr)
+{    
+    int i;
+    if ((bt_addr_le_cmp(deviceAddr, &_discovering_ble_addresses) == 0))
+    {
+        _start_discover = false;
+        memset(&_discovering_ble_addresses, 0, sizeof(_discovering_ble_addresses));
+    }
+    for (i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        if ((bt_addr_le_cmp(deviceAddr, &_addresses[i]) == 0))
+        {
+            bitSet(_disconnect_bitmap, i);
+            break;
+        }
+    }
+
+}
+
+void BLEProfileManager::handleDisconnectedPutOffEvent()
+{    
+    BLEServiceLinkNodeHeader* serviceheader = NULL;
+    int i;
+    if (_disconnect_bitmap == 0)
+    {
+        return;
+    }
+    
+    for (i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        if (bitRead(_disconnect_bitmap, i) != 0)
+        {
+            serviceheader = &_service_header_array[i];
+            clearProfile(serviceheader);
+            memset(&_addresses[i], 0, sizeof(bt_addr_le_t));
+            bitClear(_disconnect_bitmap, i);
+        }
+    }
+}
+
+bool BLEProfileManager::discoverAttributes(BLEDevice* device)
+{
+    int err;
+    bt_conn_t* conn;
+    int i = getDeviceIndex(device);
+    bool ret = false;
+    bt_gatt_discover_params_t* temp = NULL;
+    
+    errno = 0;
+    pr_debug(LOG_MODULE_BLE, "%s-%d: index-%d,fun-%p", __FUNCTION__, __LINE__, i,profile_discover_process);
+
+    if (_start_discover)
+    {
+        // Already in discover state
+        return false;
+    }
+    
+
+    if (i >= BLE_MAX_CONN_CFG)
+    {
+        // The device already in the buffer.
+        //  This function only be called after connection established.
+        return ret;
+    }
+    
+    conn = bt_conn_lookup_addr_le(device->bt_le_address());
+    if (NULL == conn)
+    {
+        // Link lost
+        pr_debug(LOG_MODULE_BLE, "Can't find connection\n");
+        return ret;
+    }
+    temp = &_discover_params[i];
+    temp->start_handle = 1;
+    temp->end_handle = 0xFFFF;
+    temp->uuid = NULL;
+    temp->type = BT_GATT_DISCOVER_PRIMARY;
+    temp->func = profile_discover_process;
+    
+    err = bt_gatt_discover(conn, temp);
+    bt_conn_unref(conn);
+    if (err)
+    {
+        pr_debug(LOG_MODULE_BLE, "Discover failed(err %d)\n", err);
+        return ret;
+    }
+    // Block it 
+    memcpy(&_discovering_ble_addresses, device->bt_le_address(), sizeof(_discovering_ble_addresses));
+    _discover_rsp_timestamp = millis();
+    _start_discover = true;
+    ret = true;
+    while (_start_discover)  // Sid. KW warning acknowldged
+    {
+        delay(10);
+        if ((millis() - _discover_rsp_timestamp) > 5000)
+        {
+            // Doesn't receive the Service read response
+            _start_discover = false;
+            _cur_discover_service = NULL;
+            ret = false;
+            _reading = false;
+        }
+        
+        if (ENOMEM == errno)
+        {
+            pr_debug(LOG_MODULE_BLE, "%s-%d:Sys errno(err %d)\n", __FUNCTION__, __LINE__, errno);
+            ret = false;
+            break;
+        }
+    }
+    return ret;
+}
+
+bool BLEProfileManager::discoverAttributesByService(BLEDevice* device, const bt_uuid_t* svc_uuid)
+{
+    errno = 0;
+    if (_start_discover)
+    {
+        // Already in discover state
+        return false;
+    }
+    
+    bool ret = discoverService(device, svc_uuid);
+    if (false == ret)
+    {
+        return false;
+    }
+    // Block it 
+    memcpy(&_discovering_ble_addresses, device->bt_le_address(), sizeof(_discovering_ble_addresses));
+    _discover_rsp_timestamp = millis();
+    _start_discover = true;
+    _discover_one_service = true;
+    
+    while (_start_discover)  // Sid. KW warning acknowldged
+    {
+        delay(10);
+        if ((millis() - _discover_rsp_timestamp) > 5000)
+        {
+            // Doesn't receive the Service read response
+            _start_discover = false;
+            _cur_discover_service = NULL;
+            ret = false;
+            _reading = false;
+        }
+        
+        if (ENOMEM == errno)
+        {
+            pr_debug(LOG_MODULE_BLE, "%s-%d:Sys errno(err %d)", __FUNCTION__, __LINE__, errno);
+            ret = false;
+            break;
+        }
+    }
+    pr_debug(LOG_MODULE_BLE, "%s-%d:Discover Done", __FUNCTION__, __LINE__);
+    _discover_one_service = false;
+    
+    return ret;
+}
+
+
+int BLEProfileManager::getDeviceIndex(const bt_addr_le_t* macAddr)
+{
+    int i;
+    for (i = 0; i < BLE_MAX_CONN_CFG; i++)
+    {
+        if ((bt_addr_le_cmp(macAddr, &_addresses[i]) == 0))
+        {
+            break;
+        }
+    }
+    return i;
+}
+
+int BLEProfileManager::getDeviceIndex(const BLEDevice* device)
+{
+    return getDeviceIndex(device->bt_le_address());
+}
+
+bool BLEProfileManager::discovering()
+{
+    bool ret = _discovering;
+    if (_cur_discover_service != NULL)
+    {
+        ret = ret || _cur_discover_service->discovering();
+    }
+    return ret;
+}
+
+void BLEProfileManager::setDiscovering(bool discover)
+{
+    _discovering = discover;
+}
+
+uint8_t BLEProfileManager::discoverResponseProc(bt_conn_t *conn,
+                                                const bt_gatt_attr_t *attr,
+                                                bt_gatt_discover_params_t *params)
+{
+    const bt_addr_le_t* dst_addr = bt_conn_get_dst(conn);
+    int i = getDeviceIndex(dst_addr);
+    BLEDevice device(dst_addr);
+    uint8_t retVal = BT_GATT_ITER_STOP;
+    BLEServiceImp* service_tmp = NULL;
+    _discover_rsp_timestamp = millis();
+    //pr_debug(LOG_MODULE_BLE, "%s-%d: index-%d", __FUNCTION__, __LINE__, i);
+    
+    if (i >= BLE_MAX_CONN_CFG)
+    {
+        return BT_GATT_ITER_STOP;
+    }
+
+    // Process the service
+    switch (params->type)
+    {
+        case BT_GATT_DISCOVER_CHARACTERISTIC:
+        case BT_GATT_DISCOVER_DESCRIPTOR:
+        {
+            if (NULL != _cur_discover_service)
+            {
+                retVal = _cur_discover_service->discoverResponseProc(conn, 
+                                                                     attr, 
+                                                                     params);
+            }
+            break;
+        }
+        case BT_GATT_DISCOVER_PRIMARY:
+        {
+            if (NULL != attr)
+            {
+                struct bt_gatt_service *svc_value = (struct bt_gatt_service *)attr->user_data;
+                const bt_uuid_t* svc_uuid = svc_value->uuid;
+                uint16_t le16;
+                memcpy(&le16, &BT_UUID_16(svc_uuid)->val, sizeof(le16));
+                setDiscovering(false);
+                
+                if (svc_uuid->type == BT_UUID_TYPE_16 && 
+                    le16 == 0)
+                {
+                    // Discover failed. The service may unknow type. 
+                    //  Need read the value and discovery again.
+                    readService(device, attr->handle);
+                    retVal = BT_GATT_ITER_CONTINUE;
+                }
+                else
+                {
+                    service_tmp = addService(device, svc_value->uuid);
+                    params->uuid = NULL;
+                    
+                    if (NULL != service_tmp)
+                    {
+                        service_tmp->setHandle(attr->handle);
+                        service_tmp->setEndHandle(svc_value->end_handle);
+                        if (_discover_one_service == false)
+                            retVal = BT_GATT_ITER_CONTINUE;
+                    }
+                    else
+                    {
+                        retVal = BT_GATT_ITER_STOP;
+                        errno = ENOMEM;
+                        pr_debug(LOG_MODULE_BLE, "%s-%d: Add service failed", 
+                                 __FUNCTION__, __LINE__);
+                    }
+                }
+            }
+            else
+            {
+                // Service discover complete
+                retVal = BT_GATT_ITER_STOP;
+            }
+        }   
+        default:
+        {
+            break;
+        }
+    }
+    
+    if (retVal == BT_GATT_ITER_STOP)
+    {
+        if (errno == ENOMEM)
+        {
+            // No memory. Stop discovery
+            _cur_discover_service = NULL;
+            _discover_one_service = false;
+            return retVal;
+        }
+        
+        pr_debug(LOG_MODULE_BLE, "%s-%d: Discover one service-%d", 
+                             __FUNCTION__, __LINE__, _discover_one_service);
+        if (true == _discover_one_service)
+        {
+            if (NULL != service_tmp)
+            {
+                pr_debug(LOG_MODULE_BLE, "%s-%d: Discover service", 
+                                     __FUNCTION__, __LINE__);
+                bool result = service_tmp->discoverAttributes(&device);
+                if (result == true)
+                {
+                    // Record the current discovering service
+                    _cur_discover_service = service_tmp;
+                }
+                else
+                {
+                    // Failed
+                    _discover_one_service = false;
+                }
+            }
+            else
+            {
+                if (discovering() == false)
+                {
+                    // Complete
+                    _cur_discover_service = NULL;
+                    _discover_one_service = false;
+                }
+            }
+            
+            if (_discover_one_service == false)
+            {
+                // Discover complete
+                _start_discover = false;
+                memset(&_discovering_ble_addresses, 0, sizeof(_discovering_ble_addresses));
+            }
+            return retVal;
+        }
+        
+        checkReadService();
+        if (discovering() == false)
+        {
+            const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(device);
+            BLEServiceImp* serviceCurImp = NULL;
+            if (NULL == serviceHeader)
+            {
+                // Doesn't find the service
+                return BT_GATT_ITER_STOP;
+            }
+            BLEServiceNodePtr node = serviceHeader->next;
+            
+            // Discover next service
+            while (node != NULL)
+            {
+                serviceCurImp = node->value;
+                
+                if (NULL == _cur_discover_service)
+                {
+                    bool result = serviceCurImp->discoverAttributes(&device);
+                    if (result == true)
+                    {
+                        // Record the current discovering service
+                        _cur_discover_service = serviceCurImp;
+                        break;
+                    }
+                }
+                else if (_cur_discover_service == serviceCurImp)
+                {
+                    // Find next discoverable service
+                    _cur_discover_service = NULL;
+                }
+                
+                node = node->next;
+            }
+            if (NULL == node)
+            {
+                pr_debug(LOG_MODULE_BLE, "%s-%d: Discover completed", 
+                                     __FUNCTION__, __LINE__);
+                _start_discover = false;
+                memset(&_discovering_ble_addresses, 0, sizeof(_discovering_ble_addresses));
+            }
+        }
+    }
+    return retVal;
+}
+
+void BLEProfileManager::serviceDiscoverComplete(const BLEDevice &bledevice)
+{
+    BLEServiceImp* serviceCurImp = NULL;
+    BLEServiceImp* servicePrevImp = NULL;
+    const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return ;
+    }
+    
+    BLEServiceNodePtr node = serviceHeader->next;
+    if (NULL != node)
+    {
+        servicePrevImp = node->value;
+        node = node->next;  
+    }
+    
+    // Update the service handles
+    while (node != NULL)
+    {
+        serviceCurImp = node->value;
+        if (NULL != serviceCurImp)
+        {
+            if (servicePrevImp)  // KW issue: Chk for NULL.
+                servicePrevImp->setEndHandle(serviceCurImp->startHandle() - 1);
+        }
+        
+        if (servicePrevImp)
+        {
+            pr_debug(LOG_MODULE_BLE, "Curr: start-%d, end-%d", servicePrevImp->startHandle(), servicePrevImp->endHandle());
+        }
+        servicePrevImp = serviceCurImp;
+        if (servicePrevImp)  // KW issue: Chk for NULL.
+            pr_debug(LOG_MODULE_BLE, "Curr: start-%d, end-%d", servicePrevImp->startHandle(), servicePrevImp->endHandle());
+        node = node->next;
+    }
+    return;
+}
+
+bool BLEProfileManager::readService(const BLEDevice &bledevice, uint16_t handle)
+{
+    int retval = 0;
+    bt_conn_t* conn = NULL;
+    
+    if (true == BLEUtils::isLocalBLE(bledevice))
+    {
+        // GATT server can't write
+        return false;
+    }
+    
+    if (_reading)
+    {
+        // Read response not back
+        // Add to buffer
+        ServiceRead_t temp;
+        bt_addr_le_copy(&temp.address, bledevice.bt_le_address());
+        temp.handle = handle;
+        ServiceReadLinkNodePtr node = link_node_create(temp);
+        link_node_insert_last(&_read_service_header, node);
+        return true;
+    }
+    
+    _read_params.func = profile_service_read_rsp_process;
+    _read_params.handle_count = 1;
+    _read_params.single.handle = handle;
+    _read_params.single.offset = 0;
+    
+    if (0 == _read_params.single.handle)
+    {
+        // Discover not complete
+        return false;
+    }
+    
+    conn = bt_conn_lookup_addr_le(bledevice.bt_le_address());
+    if (NULL == conn)
+    {
+        return false;
+    }
+    // Send read request
+    retval = bt_gatt_read(conn, &_read_params);
+    bt_conn_unref(conn);
+    if (0 == retval)
+    {
+        setDiscovering(true);
+        _reading = true;
+    }
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    return _reading;
+}
+
+void BLEProfileManager::checkReadService()
+{
+    ServiceReadLinkNodePtr node = link_node_get_first(&_read_service_header);
+    while (NULL != node)
+    {
+        BLEDevice temp(&node->value.address);
+        bool readResult = readService(temp, node->value.handle);
+        link_node_remove_first(&_read_service_header);
+        if (true == readResult)
+        {
+            break;
+        }
+        node = link_node_get_first(&_read_service_header);
+    }
+}
+
+bool BLEProfileManager::discoverService(BLEDevice* device, const bt_uuid_t* svc_uuid)
+{
+    int err = 0;
+    bt_conn_t* conn;
+    int i = getDeviceIndex(device);
+    bool ret = false;
+    bt_gatt_discover_params_t* temp = NULL;
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d: index-%d,fun-%p", __FUNCTION__, __LINE__, i,profile_discover_process);
+
+    if (i >= BLE_MAX_CONN_CFG)
+    {
+        // The device already in the buffer.
+        //  This function only be called after connection established.
+        return ret;
+    }
+    
+    //BLEServiceImp* serviceImp = service(device, svc_uuid);
+    //if (NULL == serviceImp)
+    {
+       //return ret;
+    }
+    
+    conn = bt_conn_lookup_addr_le(device->bt_le_address());
+    if (NULL == conn)
+    {
+        // Link lost
+        pr_debug(LOG_MODULE_BLE, "Can't find connection\n");
+        return ret;
+    }
+    
+    memcpy(&_discover_uuid[i], svc_uuid, sizeof(bt_uuid_128_t));
+    
+    temp = &_discover_params[i];
+    temp->start_handle = 1;
+    temp->end_handle = 0xFFFF;
+    temp->uuid = (bt_uuid_t*) &_discover_uuid[i];
+    temp->type = BT_GATT_DISCOVER_PRIMARY;
+    temp->func = profile_discover_process;
+    
+    err = bt_gatt_discover(conn, temp);
+    bt_conn_unref(conn);
+    if (err)
+    {
+        pr_debug(LOG_MODULE_BLE, "Discover failed(err %d)\n", err);
+        return ret;
+    }
+    return true;
+}
+
+uint8_t BLEProfileManager::serviceReadRspProc(bt_conn_t *conn, 
+                                           int err,
+                                           bt_gatt_read_params_t *params,
+                                           const void *data, 
+                                           uint16_t length)
+{
+    _reading = false;
+    _discover_rsp_timestamp = millis();
+    if (NULL == data)
+    {
+        return BT_GATT_ITER_STOP;
+    }
+    BLEDevice bleDevice(bt_conn_get_dst(conn));
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d:length-%d", __FUNCTION__, __LINE__, length);
+    if (length == UUID_SIZE_128)
+    {
+        bt_uuid_128_t uuid_tmp;
+        uuid_tmp.uuid.type = BT_UUID_TYPE_128;
+        memcpy(uuid_tmp.val, data, UUID_SIZE_128);
+        BLEProfileManager::instance()->discoverService(&bleDevice, (const bt_uuid_t *)&uuid_tmp);
+    }
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    
+    return BT_GATT_ITER_STOP;
+}
+
+
+
diff --git a/libraries/CurieBLE/src/internal/BLEProfileManager.h b/libraries/CurieBLE/src/internal/BLEProfileManager.h
new file mode 100644
index 00000000..1c05fc99
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEProfileManager.h
@@ -0,0 +1,222 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef __BLE_PROFILE_MANAGER_H__
+#define __BLE_PROFILE_MANAGER_H__
+
+#include "CurieBLE.h"
+
+#include "BLEServiceImp.h"
+
+//#include "BLECommon.h"
+//#include "BLEDevice.h"
+//#include "BLEService.h"
+typedef struct {
+    bt_addr_le_t  address;
+    uint16_t      handle;
+}ServiceRead_t;
+
+class BLEProfileManager{
+public:
+    /**
+     * @brief   Get the BLEProfile Manager instance
+     *
+     * @param   none
+     *
+     * @return  BLEProfileManager*  BLE Profile manager
+     *
+     * @note  none
+     */
+    static BLEProfileManager* instance();
+    
+    /**
+     * @brief   Add an service to the BLE Device
+     *
+     * @param[in] bledevice The BLE device that owned the service
+     *
+     * @param[in] service   The service to add to BLE device profile
+     *
+     * @return BleStatus indicating success or error
+     *
+     * @note This method must be called before the begin method in GATT server role
+     *       Or be called in discover process.
+     */
+    BLEServiceImp *addService (BLEDevice &bledevice, BLEService& service);
+    BLEServiceImp *addService (BLEDevice &bledevice, const bt_uuid_t* uuid);
+    
+    /**
+     * @brief   Register the profile to Nordic BLE stack
+     *
+     * @param[in]   bledevice       The BLE Device
+     *
+     * @return  int     std C errno
+     *
+     * @note  none
+     */
+    int registerProfile(BLEDevice &bledevice);
+    
+    inline bool hasRegisterProfile(){return _profile_registered;}
+    
+    BLEDescriptorImp* descriptor(const BLEDevice &bledevice, uint16_t handle);
+    /**
+     * @brief   Get the BLE's Characteristic implementation object by uuid and index
+     *
+     * @param[in]   bledevice   The BLE device
+     *
+     * @param[in]   uuid        The characteristic UUID
+     *
+     * @param[in]   index       The characteristic index in the profile
+     *
+     * @return  BLECharacteristicImp*   The BLE characteristic implementation object
+     *
+     * @note  none
+     */
+    BLECharacteristicImp* characteristic(const BLEDevice &bledevice, 
+                                         const char* uuid, 
+                                         int index);
+    BLECharacteristicImp* characteristic(const BLEDevice &bledevice, 
+                                         const char* uuid);
+    BLECharacteristicImp* characteristic(const BLEDevice &bledevice, 
+                                         int index);
+    BLECharacteristicImp* characteristic(const BLEDevice &bledevice, 
+                                         uint16_t handle);
+    BLEServiceImp* service(const BLEDevice &bledevice, const char * uuid) const;
+    BLEServiceImp* service(const BLEDevice &bledevice, int index) const;
+    BLEServiceImp* service(const BLEDevice &bledevice, const bt_uuid_t* uuid) const;
+    BLEServiceImp* getServiceBySubHandle(const BLEDevice &bledevice, uint16_t handle) const;
+    int serviceCount(const BLEDevice &bledevice) const;
+    int characteristicCount(const BLEDevice &bledevice) const;
+    
+    uint8_t discoverResponseProc(bt_conn_t *conn,
+                                 const bt_gatt_attr_t *attr,
+                                 bt_gatt_discover_params_t *params);
+    
+    bool discoverAttributes(BLEDevice* device);
+    bool discoverAttributesByService(BLEDevice* device, const bt_uuid_t* svc_uuid);
+    void handleConnectedEvent(const bt_addr_le_t* deviceAddr);
+    void handleDisconnectedEvent(const bt_addr_le_t* deviceAddr);
+    void handleDisconnectedPutOffEvent();
+    uint8_t serviceReadRspProc(bt_conn_t *conn, 
+                               int err,
+                               bt_gatt_read_params_t *params,
+                               const void *data, 
+                               uint16_t length);
+protected:
+    friend ssize_t profile_write_process(bt_conn_t *conn,
+                                     const bt_gatt_attr_t *attr,
+                                     const void *buf, uint16_t len,
+                                     uint16_t offset);
+    bool discoverService(BLEDevice* device, const bt_uuid_t* svc_uuid);
+private:
+    typedef LinkNode<BLEServiceImp *> BLEServiceLinkNodeHeader;
+    typedef LinkNode<BLEServiceImp *>* BLEServiceNodePtr;
+    typedef LinkNode<BLEServiceImp *> BLEServiceNode;
+    
+    typedef LinkNode<ServiceRead_t> ServiceReadLinkNodeHeader;
+    typedef LinkNode<ServiceRead_t>* ServiceReadLinkNodePtr;
+    typedef LinkNode<ServiceRead_t> ServiceReadLinkNode;
+    
+    BLEProfileManager();
+    ~BLEProfileManager (void);
+    
+    void serviceDiscoverComplete(const BLEDevice &bledevice);
+    
+    int getDeviceIndex(const bt_addr_le_t* macAddr);
+    int getDeviceIndex(const BLEDevice* device);
+    /**
+     * @brief   Get the unused service header index
+     *
+     * @param   none
+     *
+     * @return  int     The unused BLE profile index
+     *
+     * @note  This object has a buffer to manage all devices profile. 
+     *          The buffer is an array. The different profiles 
+     *          distinguished by BLE address.
+     */
+    int getUnusedIndex();
+    
+    /**
+     * @brief   Get the Service header by BLE device
+     *
+     * @param[in]   bledevice   The BLE device
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    BLEServiceLinkNodeHeader* getServiceHeader(const BLEDevice &bledevice);
+    const BLEServiceLinkNodeHeader* getServiceHeader(const BLEDevice &bledevice) const;
+    
+    /**
+     * @brief   Get the BLE attribute counter based on services, characteristics
+     *           and descriptors.
+     *
+     * @param   none
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    int getAttributeCount(BLEDevice &bledevice);
+    
+    /**
+     * @brief   Discard the profile by BLE device
+     *
+     * @param[in]   bledevice   The BLE device
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    void clearProfile(BLEServiceLinkNodeHeader* serviceHeader);
+    
+    bool readService(const BLEDevice &bledevice, uint16_t handle);
+    bool discovering();
+    void setDiscovering(bool discover);
+    void checkReadService();
+    
+private:
+    // The last header is for local BLE
+    BLEServiceLinkNodeHeader _service_header_array[BLE_MAX_CONN_CFG + 1]; // The connected devices' service and self service
+    bt_addr_le_t  _addresses[BLE_MAX_CONN_CFG + 1]; // The BLE devices' address
+    bt_addr_le_t  _discovering_ble_addresses;
+    
+    bool _start_discover;
+
+    bool _discovering;
+    uint64_t _discover_rsp_timestamp;
+    bt_gatt_discover_params_t _discover_params[BLE_MAX_CONN_CFG];
+    bt_uuid_128_t _discover_uuid[BLE_MAX_CONN_CFG];
+    BLEServiceImp* _cur_discover_service;
+    bool _discover_one_service;
+    bt_gatt_read_params_t _read_params;
+    bool _reading;
+    ServiceReadLinkNodeHeader _read_service_header;
+    
+    bt_gatt_attr_t *_attr_base; // Allocate the memory for BLE stack
+    int _attr_index;
+    
+    static BLEProfileManager* _instance; // The profile manager instance
+    bool _profile_registered;
+    uint8_t _disconnect_bitmap;
+};
+
+#endif
+
diff --git a/libraries/CurieBLE/src/internal/BLEServiceImp.cpp b/libraries/CurieBLE/src/internal/BLEServiceImp.cpp
new file mode 100644
index 00000000..d5d7afe6
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEServiceImp.cpp
@@ -0,0 +1,535 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+#include <errno.h>
+
+#include "internal/ble_client.h"
+
+#include "BLEServiceImp.h"
+#include "BLECallbacks.h"
+#include "BLEUtils.h"
+#include "BLECharacteristicImp.h"
+
+bt_uuid_16_t BLEServiceImp::_gatt_primary_uuid = {BT_UUID_TYPE_16, BT_UUID_GATT_PRIMARY_VAL};
+bt_gatt_read_params_t BLEServiceImp::_read_params;
+
+bt_uuid_t *BLEServiceImp::getPrimayUuid(void)
+{
+    return (bt_uuid_t *)&_gatt_primary_uuid;
+}
+
+BLEServiceImp::BLEServiceImp(BLEService& service): 
+    BLEAttribute(service.uuid(), BLETypeService), 
+    _start_handle(0),
+    _end_handle(0xFFFF),
+    _reading(false),
+    _cur_discover_chrc(NULL)
+{
+    memset(&_characteristics_header, 0, sizeof(_characteristics_header));
+    service.setServiceImp(this);
+}
+
+BLEServiceImp::BLEServiceImp(const bt_uuid_t* uuid): 
+    BLEAttribute(uuid, BLETypeService),
+    _start_handle(0),
+    _end_handle(0xFFFF),
+    _reading(false),
+    _cur_discover_chrc(NULL)
+{
+    memset(&_characteristics_header, 0, sizeof(_characteristics_header));
+}
+
+BLEServiceImp::~BLEServiceImp()
+{
+    releaseCharacteristic();
+}
+
+
+int BLEServiceImp::addCharacteristic(BLEDevice& bledevice, BLECharacteristic& characteristic)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    
+    characteristicImp = characteristic.fetchCharacteristicImp();
+    if (NULL == characteristicImp)
+    {
+        characteristicImp = new BLECharacteristicImp(characteristic, bledevice);
+        pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+        if (NULL == characteristicImp)
+        {
+            return BLE_STATUS_NO_MEMORY;
+        }
+    }
+    
+    BLECharacteristicNodePtr node = link_node_create(characteristicImp);
+    if (NULL == node)
+    {
+        delete characteristicImp;
+        return BLE_STATUS_NO_MEMORY;
+    }
+    link_node_insert_last(&_characteristics_header, node);
+    pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+    return BLE_STATUS_SUCCESS;
+}
+
+int BLEServiceImp::addCharacteristic(BLEDevice& bledevice, 
+                                     const bt_uuid_t* uuid, 
+                                     uint16_t handle, 
+                                     unsigned char properties)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d:handle-%d",__FUNCTION__, __LINE__,handle);
+    characteristicImp = new BLECharacteristicImp(uuid, 
+                                                 properties, 
+                                                 handle, 
+                                                 bledevice);
+    if (NULL == characteristicImp)
+    {
+        return BLE_STATUS_NO_MEMORY;
+    }
+    
+    BLECharacteristicNodePtr node = link_node_create(characteristicImp);
+    if (NULL == node)
+    {
+        delete characteristicImp;
+        return BLE_STATUS_NO_MEMORY;
+    }
+    link_node_insert_last(&_characteristics_header, node);
+    pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+    return BLE_STATUS_SUCCESS;
+}
+
+int BLEServiceImp::updateProfile(bt_gatt_attr_t *attr_start, int& index)
+{
+    bt_gatt_attr_t *start = attr_start;
+    int base_index = index;
+    int offset = 0;
+    int counter = 0;
+    start->uuid = BLEServiceImp::getPrimayUuid();
+    start->perm = BT_GATT_PERM_READ;
+    start->read = bt_gatt_attr_read_service;
+    start->user_data = (void *)bt_uuid();
+
+    pr_debug(LOG_MODULE_BLE, "service-%p", start);
+    start++;
+    index++;
+    counter++;
+    
+    BLECharacteristicNodePtr node = _characteristics_header.next;
+    while (NULL != node)
+    {
+        BLECharacteristicImp *characteristicImp = node->value;
+        start = attr_start + index - base_index;
+        offset = characteristicImp->updateProfile(start, index);
+        counter += offset;
+        node = node->next;
+    }
+    return counter;
+}
+
+int BLEServiceImp::getAttributeCount()
+{
+    int counter = 1;  // Service itself
+    
+    BLECharacteristicNodePtr node = _characteristics_header.next;
+    while (NULL != node)
+    {
+        BLECharacteristicImp *characteristicImp = node->value;
+        
+        counter += characteristicImp->getAttributeCount();
+        node = node->next;
+    }
+    return counter;
+}
+
+int BLEServiceImp::getCharacteristicCount()
+{
+    return link_list_size(&_characteristics_header);
+}
+
+void BLEServiceImp::releaseCharacteristic()
+{
+    BLECharacteristicNodePtr node = link_node_get_first(&_characteristics_header);
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    while (NULL != node)
+    {
+        BLECharacteristicImp* characteristicImp = node->value;
+        delete characteristicImp;
+        link_node_remove_first(&_characteristics_header);
+        node = link_node_get_first(&_characteristics_header);
+    }
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+}
+
+
+BLEDescriptorImp* BLEServiceImp::descriptor(uint16_t handle)
+{
+    BLEDescriptorImp* descriptorImp = NULL;
+    
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLECharacteristicNodePtr node = link_node_get_first(&_characteristics_header);
+    while (NULL != node)
+    {
+        characteristicImp = node->value;
+        descriptorImp = characteristicImp->descriptor(handle);
+        if (descriptorImp != NULL)
+        {
+            break;
+        }
+        node = node->next;
+    }
+    if (NULL == node)
+    {
+        descriptorImp = NULL;
+    }
+    return descriptorImp;
+}
+
+BLECharacteristicImp* BLEServiceImp::characteristic(int index)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLECharacteristicNodePtr node = link_node_get_first(&_characteristics_header);
+    while (NULL != node)
+    {
+        if (0 >= index)
+        {
+            characteristicImp = node->value;
+            break;
+        }
+        index--;
+        node = node->next;
+    }
+    return characteristicImp;
+}
+
+BLECharacteristicImp* BLEServiceImp::characteristic(uint16_t handle)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLECharacteristicNodePtr node = link_node_get_first(&_characteristics_header);
+    while (NULL != node)
+    {
+        characteristicImp = node->value;
+        if (handle == characteristicImp->valueHandle())
+        {
+            break;
+        }
+        node = node->next;
+    }
+    if (NULL == node)
+    {
+        characteristicImp = NULL;
+    }
+    return characteristicImp;
+}
+
+BLECharacteristicImp* BLEServiceImp::characteristic(const bt_uuid_t* uuid)
+{
+    BLECharacteristicImp* characteristicImp = NULL;
+    BLECharacteristicNodePtr node = link_node_get_first(&_characteristics_header);
+    
+    while (NULL != node)
+    {
+        characteristicImp = node->value;
+        if (true == characteristicImp->compareUuid(uuid))
+        {
+            break;
+        }
+        node = node->next;
+    }
+    
+    if (NULL == node)
+    {
+        characteristicImp = NULL;
+    }
+    return characteristicImp;
+}
+
+BLECharacteristicImp* BLEServiceImp::characteristic(const char* uuid)
+{
+    bt_uuid_128_t uuid_tmp;
+    BLEUtils::uuidString2BT(uuid, (bt_uuid_t *)&uuid_tmp);
+    return characteristic((const bt_uuid_t *)&uuid_tmp);
+}
+
+bool BLEServiceImp::discovering()
+{
+    return (_cur_discover_chrc != NULL || _reading);
+}
+
+bool BLEServiceImp::discoverAttributes(BLEDevice* device)
+{
+    return discoverAttributes(device, _start_handle, _end_handle);
+}
+
+bool BLEServiceImp::discoverAttributes(BLEDevice* device, 
+                                       uint16_t start_handle, 
+                                       uint16_t end_handle)
+{
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    int err;
+    bt_conn_t* conn;
+    bt_gatt_discover_params_t* temp = NULL;
+    const bt_uuid_t* service_uuid = bt_uuid();
+    
+    if (service_uuid->type == BT_UUID_TYPE_16)
+    {
+        uint16_t uuid_tmp;// = ((bt_uuid_16_t*)service_uuid)->val;
+        
+        memcpy(&uuid_tmp, &((bt_uuid_16_t*)service_uuid)->val, sizeof(uuid_tmp));
+        if (BT_UUID_GAP_VAL == uuid_tmp ||
+            BT_UUID_GATT_VAL == uuid_tmp)
+        {
+            return false;
+        }
+    }
+
+    conn = bt_conn_lookup_addr_le(device->bt_le_address());
+    if (NULL == conn)
+    {
+        // Link lost
+        pr_debug(LOG_MODULE_BLE, "Can't find connection\n");
+        return false;
+    }
+    temp = &_discover_params;
+    temp->start_handle = start_handle;
+    temp->end_handle = end_handle;
+    temp->uuid = NULL;
+    temp->type = BT_GATT_DISCOVER_CHARACTERISTIC;
+    temp->func = profile_discover_process;
+    
+    err = bt_gatt_discover(conn, temp);
+    bt_conn_unref(conn);
+    if (err)
+    {
+        pr_debug(LOG_MODULE_BLE, "Discover failed(err %d)\n", err);
+        return false;
+    }
+    return true;
+}
+
+uint8_t BLEServiceImp::discoverResponseProc(bt_conn_t *conn,
+                                            const bt_gatt_attr_t *attr,
+                                            bt_gatt_discover_params_t *params)
+{
+    const bt_addr_le_t* dst_addr = bt_conn_get_dst(conn);
+    BLEDevice device(dst_addr);
+    uint8_t retVal = BT_GATT_ITER_STOP;
+    
+    //pr_debug(LOG_MODULE_BLE, "%s-%d: type-%d", __FUNCTION__, __LINE__, params->type);
+
+    // Process the service
+    switch (params->type)
+    {
+        case BT_GATT_DISCOVER_CHARACTERISTIC:
+        {
+            if (NULL != attr)
+            {
+                //const bt_uuid_t* chrc_uuid = attr->uuid;
+                uint16_t chrc_handle = attr->handle + 1;
+                struct bt_gatt_chrc* psttemp = (struct bt_gatt_chrc*)attr->user_data;
+                const bt_uuid_t* chrc_uuid = psttemp->uuid;
+                
+                uint16_t le16;
+                memcpy(&le16, &BT_UUID_16(chrc_uuid)->val, sizeof(le16));
+                if (chrc_uuid->type == BT_UUID_TYPE_16 && 
+                    le16 == 0)
+                {
+                    // Read the UUID
+                    readCharacteristic(device, chrc_handle);
+                    retVal = BT_GATT_ITER_CONTINUE;
+                }
+                else
+                {
+                    int retval = (int)addCharacteristic(device, 
+                                                        psttemp->uuid,
+                                                        chrc_handle,
+                                                        psttemp->properties);
+                    
+                    if (BLE_STATUS_SUCCESS != retval)
+                    {
+                        pr_error(LOG_MODULE_BLE, "%s-%d: Error-%d", 
+                                 __FUNCTION__, __LINE__, retval);
+                        errno = ENOMEM;
+                    }
+                    else
+                    {
+                        retVal = BT_GATT_ITER_CONTINUE;
+                    }
+                }
+            }
+            break;
+        }
+        case BT_GATT_DISCOVER_DESCRIPTOR:
+        {
+            // 
+            
+            if (NULL != _cur_discover_chrc)
+            {
+                retVal = _cur_discover_chrc->discoverResponseProc(conn, 
+                                                                  attr, 
+                                                                  params);
+            }
+            break;
+        }
+        default:
+        {
+            //attribute_tmp->discover(attr, &_discover_params);
+            break;
+        }
+    }
+    
+    //pr_debug(LOG_MODULE_BLE, "%s-%d:ret-%d",__FUNCTION__, __LINE__, retVal);
+    if (retVal == BT_GATT_ITER_STOP)
+    {
+        if (errno == ENOMEM)
+        {
+            _cur_discover_chrc = NULL;
+            return retVal;
+        }
+        
+        if (false == _reading)
+        {
+            discoverNextCharacteristic(device);
+        }
+    }
+    return retVal;
+}
+
+void BLEServiceImp::discoverNextCharacteristic(BLEDevice &bledevice)
+{
+    const BLECharacteristicLinkNodeHeader* chrcHeader = &_characteristics_header;
+    BLECharacteristicImp* chrcCurImp = NULL;
+    BLECharacteristicNodePtr node = chrcHeader->next;
+    
+    //pr_debug(LOG_MODULE_BLE, "%s-%d: node-%p",__FUNCTION__, __LINE__, node);
+    // Discover next service
+    while (node != NULL)
+    {
+        chrcCurImp = node->value;
+        
+        if (NULL == _cur_discover_chrc)
+        {
+            bool result = chrcCurImp->discoverAttributes(&bledevice);
+            pr_debug(LOG_MODULE_BLE, "%s-%d",__FUNCTION__, __LINE__);
+            if (result == true)
+            {
+                // Record the current discovering service
+                _cur_discover_chrc = chrcCurImp;
+                break;
+            }
+        }
+        else if (_cur_discover_chrc == chrcCurImp)
+        {
+            // Find next discoverable service
+            _cur_discover_chrc = NULL;
+        }
+        node = node->next;
+    }
+}
+
+bool BLEServiceImp::readCharacteristic(const BLEDevice &bledevice, uint16_t handle)
+{
+    int retval = 0;
+    bt_conn_t* conn = NULL;
+    
+    if (true == BLEUtils::isLocalBLE(bledevice))
+    {
+        // GATT server can't write
+        return false;
+    }
+    
+    if (_reading)
+    {
+        return false;
+    }
+    
+    _read_params.func = profile_characteristic_read_rsp_process;
+    _read_params.handle_count = 1;
+    _read_params.single.handle = handle - 1;
+    _read_params.single.offset = 0;
+    
+    if (0 == _read_params.single.handle)
+    {
+        // Discover not complete
+        return false;
+    }
+    
+    conn = bt_conn_lookup_addr_le(bledevice.bt_le_address());
+    if (NULL == conn)
+    {
+        return false;
+    }
+    // Send read request
+    retval = bt_gatt_read(conn, &_read_params);
+    bt_conn_unref(conn);
+    if (0 == retval)
+    {
+        _reading = true;
+    }
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    return _reading;
+}
+
+uint8_t BLEServiceImp::characteristicReadRspProc(bt_conn_t *conn, 
+                                           int err,
+                                           bt_gatt_read_params_t *params,
+                                           const void *data, 
+                                           uint16_t length)
+{
+    _reading = false;
+    if (NULL == data)
+    {
+        return BT_GATT_ITER_STOP;
+    }
+    BLEDevice bleDevice(bt_conn_get_dst(conn));
+    
+    pr_debug(LOG_MODULE_BLE, "%s-%d:length-%d", __FUNCTION__, __LINE__, length);
+    if (length == UUID_SIZE_128 + 3)
+    {
+        const uint8_t* rspdata = (const uint8_t*) data;
+        bt_uuid_128_t uuid_tmp;
+        uint16_t chrc_handle = rspdata[1] | (rspdata[2] << 8);
+        uuid_tmp.uuid.type = BT_UUID_TYPE_128;
+        memcpy(uuid_tmp.val, &rspdata[3], UUID_SIZE_128);
+        int retval = (int)addCharacteristic(bleDevice, 
+                                            (const bt_uuid_t*)&uuid_tmp,
+                                            chrc_handle,
+                                            rspdata[0]);
+        
+        if (BLE_STATUS_SUCCESS != retval)
+        {
+            pr_error(LOG_MODULE_BLE, "%s-%d: Error-%d", 
+                     __FUNCTION__, __LINE__, retval);
+            errno = ENOMEM;
+        }
+        else
+        {
+            if (false == discovering())
+            {
+                if (false == discoverAttributes(&bleDevice, chrc_handle + 1, _end_handle))
+                {
+                    discoverNextCharacteristic(bleDevice);
+                }
+            }
+        }
+    }
+    pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+    
+    return BT_GATT_ITER_STOP;
+}
+
diff --git a/libraries/CurieBLE/src/internal/BLEServiceImp.h b/libraries/CurieBLE/src/internal/BLEServiceImp.h
new file mode 100644
index 00000000..f40ffd36
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEServiceImp.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef _BLE_SERVICE_IMP_H_INCLUDED
+#define _BLE_SERVICE_IMP_H_INCLUDED
+
+#include "CurieBLE.h"
+
+#include "BLEAttribute.h"
+
+#include "LinkList.h"
+
+/**
+ * BLE GATT Service
+ */
+class BLEServiceImp: public BLEAttribute{
+public:
+    /**
+     * Constructor for BLE Service
+     *
+     * @param[in] uuid    16-bit or 128-bit UUID (in string form) defined by BLE standard
+     */
+    BLEServiceImp(BLEService& service);
+    BLEServiceImp(const bt_uuid_t* uuid);
+    ~BLEServiceImp();
+
+    /**
+     * @brief   Add a characteristic in service
+     *
+     * @param[in]   bledevice       The BLE device want to add the characteristic
+     *
+     * @param[in]   characteristic  The characteristic want to be added to service
+     *
+     * @return  none
+     *
+     * @note  none
+     */
+    int addCharacteristic(BLEDevice& bledevice, BLECharacteristic& characteristic);
+    int addCharacteristic(BLEDevice& bledevice, 
+                          const bt_uuid_t* uuid, 
+                          uint16_t handle, 
+                          unsigned char properties);
+    int getCharacteristicCount();
+    
+    BLEDescriptorImp* descriptor(uint16_t handle);
+
+    BLECharacteristicImp* characteristic(const bt_uuid_t* uuid);
+    BLECharacteristicImp* characteristic(const char* uuid);
+    BLECharacteristicImp* characteristic(int index);
+    BLECharacteristicImp* characteristic(uint16_t handle);
+    inline void setHandle(uint16_t handle){_start_handle = handle;}
+    inline void setEndHandle(uint16_t handle){_end_handle = handle;}
+    inline uint16_t endHandle(){return _end_handle;}
+    inline uint16_t startHandle(){return _start_handle;}
+    
+    bool discoverAttributes(BLEDevice* device);
+    uint8_t discoverResponseProc(bt_conn_t *conn,
+                                 const bt_gatt_attr_t *attr,
+                                 bt_gatt_discover_params_t *params);
+    
+    uint8_t characteristicReadRspProc(bt_conn_t *conn, 
+                                      int err,
+                                      bt_gatt_read_params_t *params,
+                                      const void *data, 
+                                      uint16_t length);
+    bool discovering();
+    
+    static bt_uuid_t *getPrimayUuid(void);
+protected:
+    friend class BLEProfileManager;
+    
+    int getAttributeCount();
+    
+    
+    int updateProfile(bt_gatt_attr_t *attr_start, int& index);
+    
+private:
+    void discoverNextCharacteristic(BLEDevice &bledevice);
+    bool readCharacteristic(const BLEDevice &bledevice, uint16_t handle);
+    bool discoverAttributes(BLEDevice* device, 
+                            uint16_t start_handle, 
+                            uint16_t end_handle);
+private:
+    typedef LinkNode<BLECharacteristicImp *>  BLECharacteristicLinkNodeHeader;
+    typedef LinkNode<BLECharacteristicImp *>* BLECharacteristicNodePtr;
+    typedef LinkNode<BLECharacteristicImp *>  BLECharacteristicNode;
+    
+    uint16_t _start_handle;
+    uint16_t _end_handle;
+    
+    static bt_gatt_read_params_t _read_params;
+    bool _reading;
+    
+    void releaseCharacteristic();
+    BLECharacteristicImp *_cur_discover_chrc;
+
+    static bt_uuid_16_t _gatt_primary_uuid;
+    bt_gatt_discover_params_t _discover_params;
+    
+    BLECharacteristicLinkNodeHeader _characteristics_header; // The characteristic link list
+};
+
+#endif // _BLE_SERVICE_H_INCLUDED
diff --git a/libraries/CurieBLE/src/internal/BLEUtils.cpp b/libraries/CurieBLE/src/internal/BLEUtils.cpp
new file mode 100644
index 00000000..0142db07
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/BLEUtils.cpp
@@ -0,0 +1,206 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+#include "CurieBLE.h"
+#include "BLEUtils.h"
+#include "internal/ble_client.h"
+
+String BLEUtils::macAddressBT2String(const bt_addr_le_t &bd_addr)
+{
+    char mac_string[BT_ADDR_STR_LEN];
+    snprintf(mac_string, BT_ADDR_STR_LEN, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
+            bd_addr.val[5], bd_addr.val[4], bd_addr.val[3],
+            bd_addr.val[2], bd_addr.val[1], bd_addr.val[0]);
+    String temp(mac_string);
+    return temp;
+}
+
+void BLEUtils::macAddressString2BT(const char* mac_str, bt_addr_le_t &bd_addr)
+{
+    char temp[] = {0, 0, 0};
+    int strLength = strlen(mac_str);
+    int length = 0;
+    
+    bd_addr.type = BT_ADDR_LE_PUBLIC;
+
+    for (int i = strLength - 1; i >= 0 && length < BLE_ADDR_LEN; i -= 2)
+    {
+        if (mac_str[i] == ':') 
+        {
+            i++;
+            continue;
+        }
+
+        temp[0] = mac_str[i - 1];
+        temp[1] = mac_str[i];
+
+        bd_addr.val[length] = strtoul(temp, NULL, 16);
+
+        length++;
+    }
+    
+}
+
+bool BLEUtils::macAddressSame(const bt_addr_le_t &bd_addr1, 
+                              const bt_addr_le_t &bd_addr2)
+{
+    bool temp = true;//(memcmp(bd_addr1.val, bd_addr2.val, 6) != 0);//
+    #if 1
+    for (int i = 0; i < 6; i++)
+    {
+        if (bd_addr1.val[i] != bd_addr2.val[i])
+        {
+            temp = false;
+            break;
+        }
+    }
+    #endif
+    return temp;
+    
+}
+
+bool BLEUtils::macAddressValid(const bt_addr_le_t &bd_addr)
+{
+    bool temp = false;
+#if 0
+    static const bt_addr_le_t zero = {0,{0,0,0,0,0,0}};
+    temp = (memcmp(bd_addr.val, zero.val, 6) != 0);
+#else
+    for (int i = 0; i < 6; i++)
+    {
+        if (bd_addr.val[i] != 0)
+        {      
+//pr_info(LOG_MODULE_BLE, "%s-idx %d-%.2x:%.2x", __FUNCTION__, i ,bd_addr.val[i], zero.val[i]);
+//pr_info(LOG_MODULE_BLE,"%s",BLEUtils::macAddressBT2String(zero).c_str());
+            temp = true;
+            break;
+        }
+    }
+#endif
+    return temp;
+}
+
+
+bt_addr_le_t* BLEUtils::bleGetLoalAddress()
+{
+    static bt_addr_le_t board_addr;
+    if (false == macAddressValid(board_addr))
+        ble_client_get_mac_address(&board_addr);
+    return &board_addr;
+}
+
+
+
+void BLEUtils::uuidString2BT(const char* uuid, bt_uuid_t* pstuuid)
+{
+    char temp[] = {0, 0, 0};
+    int strLength = strlen(uuid);
+    int length = 0;
+    bt_uuid_128_t uuid_tmp;
+    
+    memset (&uuid_tmp, 0x00, sizeof(uuid_tmp));
+
+    for (int i = strLength - 1; i >= 0 && length < MAX_UUID_SIZE; i -= 2)
+    {
+        if (uuid[i] == '-') 
+        {
+            i++;
+            continue;
+        }
+
+        temp[0] = uuid[i - 1];
+        temp[1] = uuid[i];
+
+        uuid_tmp.val[length] = strtoul(temp, NULL, 16);
+
+        length++;
+    }
+
+    if (length == 2)
+    {
+        uint16_t temp = (uuid_tmp.val[1] << 8)| uuid_tmp.val[0];
+        uint8_t* uuid16_val = (uint8_t*)&((bt_uuid_16_t*)(&uuid_tmp.uuid))->val;
+        uuid_tmp.uuid.type = BT_UUID_TYPE_16;
+        memcpy(uuid16_val, &temp, sizeof (uint16_t));
+    }
+    else
+    {
+        uuid_tmp.uuid.type = BT_UUID_TYPE_128;
+    }
+    memcpy(pstuuid, &uuid_tmp, sizeof (uuid_tmp));
+}
+
+void BLEUtils::uuidBT2String(const bt_uuid_t* pstuuid, char* uuid)
+{
+    unsigned int tmp1, tmp5;
+    uint16_t tmp0, tmp2, tmp3, tmp4;
+    // TODO: Change the magic number 37
+    switch (pstuuid->type) {
+    case BT_UUID_TYPE_16:
+        memcpy(&tmp0, &BT_UUID_16(pstuuid)->val, sizeof(tmp0));
+        snprintf(uuid, 37, "%.4x", tmp0);
+        break;
+    case BT_UUID_TYPE_128:
+        memcpy(&tmp0, &BT_UUID_128(pstuuid)->val[0], sizeof(tmp0));
+        memcpy(&tmp1, &BT_UUID_128(pstuuid)->val[2], sizeof(tmp1));
+        memcpy(&tmp2, &BT_UUID_128(pstuuid)->val[6], sizeof(tmp2));
+        memcpy(&tmp3, &BT_UUID_128(pstuuid)->val[8], sizeof(tmp3));
+        memcpy(&tmp4, &BT_UUID_128(pstuuid)->val[10], sizeof(tmp4));
+        memcpy(&tmp5, &BT_UUID_128(pstuuid)->val[12], sizeof(tmp5));
+        snprintf(uuid, 37, "%.8x-%.4x-%.4x-%.4x-%.8x%.4x",
+             tmp5, tmp4, tmp3, tmp2, tmp1, tmp0);
+        break;
+    default:
+        memset(uuid, 0, 37);
+        return;
+    }
+}
+
+bool BLEUtils::uuidBTSame(const bt_uuid_t* pstuuid1,
+                          const bt_uuid_t* pstuuid2)
+{
+    bool temp = (pstuuid1->type == pstuuid2->type);
+    if (true == temp)
+    {
+        if (pstuuid1->type == BT_UUID_TYPE_16)
+        {
+            temp = (0 == memcmp(&BT_UUID_16(pstuuid1)->val, &BT_UUID_16(pstuuid2)->val, 2));
+        }
+        else
+        {
+            temp = (0 == memcmp(BT_UUID_128(pstuuid1)->val, BT_UUID_128(pstuuid2)->val, 16));
+        }
+    }
+    return temp;
+    
+}
+
+BLEDevice& BLEUtils::getLoacalBleDevice()
+{
+    return BLE;
+}
+
+bool BLEUtils::isLocalBLE(const BLEDevice& device)
+{
+    return (device == BLE);
+}
+
+
+
diff --git a/libraries/CurieBLE/src/BLEUuid.cpp b/libraries/CurieBLE/src/internal/BLEUtils.h
similarity index 51%
rename from libraries/CurieBLE/src/BLEUuid.cpp
rename to libraries/CurieBLE/src/internal/BLEUtils.h
index f0764383..e756843a 100644
--- a/libraries/CurieBLE/src/BLEUuid.cpp
+++ b/libraries/CurieBLE/src/internal/BLEUtils.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
@@ -17,38 +17,20 @@
  *
  */
 
-#include "BLEUuid.h"
 
-BLEUuid::BLEUuid(const char * str)
+namespace BLEUtils
 {
-    char temp[] = {0, 0, 0};
-    int strLength = strlen(str);
-    int length = 0;
-
-    memset(&_uuid, 0x00, sizeof(_uuid));
-
-    for (int i = strLength - 1; i >= 0 && length < MAX_UUID_SIZE; i -= 2) {
-        if (str[i] == '-') {
-            i++;
-            continue;
-        }
-
-        temp[0] = str[i - 1];
-        temp[1] = str[i];
-
-        _uuid.uuid128[length] = strtoul(temp, NULL, 16);
-
-        length++;
-    }
-
-    if (length == 2) {
-        _uuid.type = BT_UUID16;
-    } else {
-        _uuid.type = BT_UUID128;
-    }
+    String macAddressBT2String(const bt_addr_le_t &bd_addr);
+    void macAddressString2BT(const char* mac_str, bt_addr_le_t &bd_addr);
+    bool macAddressValid(const bt_addr_le_t &bd_addr);
+    bool macAddressSame(const bt_addr_le_t &bd_addr1, const bt_addr_le_t &bd_addr2);
+    bt_addr_le_t* bleGetLoalAddress();
+    void uuidString2BT(const char* uuid, bt_uuid_t* pstuuid);
+    void uuidBT2String(const bt_uuid_t* pstuuid, char* uuid);
+    bool uuidBTSame(const bt_uuid_t* pstuuid1,
+                    const bt_uuid_t* pstuuid2);
+    
+    BLEDevice& getLoacalBleDevice();
+    bool isLocalBLE(const BLEDevice& device);
 }
 
-bt_uuid BLEUuid::uuid() const
-{
-    return _uuid;
-}
diff --git a/libraries/CurieBLE/src/internal/LinkList.h b/libraries/CurieBLE/src/internal/LinkList.h
new file mode 100644
index 00000000..180741c1
--- /dev/null
+++ b/libraries/CurieBLE/src/internal/LinkList.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef _LINKLIST_H_
+#define _LINKLIST_H_
+
+template<typename T> struct LinkNode {
+    LinkNode<T> *next;
+    T value;
+};
+
+template<typename T> LinkNode<T>* link_node_create(T value)
+{
+    LinkNode<T>* node = (LinkNode<T>*)malloc(sizeof(LinkNode<T>));
+
+    if (node) {
+      node->value = value;
+      node->next = NULL;
+    }
+    return node;
+}
+
+template<typename T> void link_node_insert_last(LinkNode<T> *root, LinkNode<T> *node)
+{
+    while(root->next != 0)
+    {
+        root = root->next;
+    }
+    root->next = node;
+}
+
+template<typename T> void link_node_remove_last(LinkNode<T> *root)
+{
+    LinkNode<T> *temp1, *temp2;
+    if (root->next != NULL)
+    {
+        temp1 = root->next;
+        while(temp1->next != NULL)
+        {
+            temp2 = temp1;
+            temp1 = temp1->next;
+        }
+        
+        free(temp1);
+        temp2->next = NULL;
+    }
+}
+
+template<typename T> void link_node_remove_first(LinkNode<T> *root)
+{
+    LinkNode<T> *temp1;
+    if (root->next != NULL)
+    {
+        temp1 = root->next;
+        root->next = temp1->next;
+        free(temp1);
+    }
+}
+
+template<typename T> LinkNode<T> * link_node_get_first(LinkNode<T> *root)
+{
+    return root->next;
+}
+
+template<typename T> void link_node_insert_first(LinkNode<T> *root, LinkNode<T> *node)
+{
+    LinkNode<T>* temp = root->next;
+    root->next = node;
+    node->next = temp;
+}
+
+template<typename T> int link_list_size(const LinkNode<T> *root)
+{
+    int counter = 0;
+    while(root->next != 0)
+    {
+        root = root->next;
+        counter++;
+    }
+    return counter;
+}
+
+#endif
+
diff --git a/libraries/CurieBLE/src/internal/ble_client.c b/libraries/CurieBLE/src/internal/ble_client.c
index a4f4cbc6..5a9e6164 100644
--- a/libraries/CurieBLE/src/internal/ble_client.c
+++ b/libraries/CurieBLE/src/internal/ble_client.c
@@ -27,6 +27,8 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+     
+#include <errno.h>
 
 #include <string.h>
 #include "cfw/cfw.h"
@@ -48,327 +50,69 @@
 #include "ble_client.h"
 #include "platform.h"
 
-/* Advertising parameters */
-#define BLE_GAP_ADV_TYPE_ADV_IND          0x00   /**< Connectable undirected. */
-#define BLE_GAP_ADV_FP_ANY                0x00   /**< Allow scan requests and connect requests from any device. */
-/** options see \ref BLE_ADV_OPTIONS */
-/* options: BLE_NO_ADV_OPT */
-#define APP_ULTRA_FAST_ADV_INTERVAL             32
-#define APP_ULTRA_FAST_ADV_TIMEOUT_IN_SECONDS   180
-/* options: BLE_SLOW_ADV */
-#define APP_DISC_ADV_INTERVAL           160
-#define APP_DISC_ADV_TIMEOUT_IN_SECONDS 180
-/* options: BLE_NON_DISC_ADV */
-#define APP_NON_DISC_ADV_FAST_INTERVAL              160
-#define APP_NON_DISC_ADV_FAST_TIMEOUT_IN_SECONDS    30
-/* options: BLE_SLOW_ADV | BLE_NON_DISC_ADV */
-#define APP_NON_DISC_ADV_SLOW_INTERVAL              2056
-#define APP_NON_DISC_ADV_SLOW_TIMEOUT_IN_SECONDS    0
+#include "infra/log.h"
 
-struct cfw_msg_rsp_sync {
-    volatile unsigned response;
-    volatile ble_status_t status;
-    void *param;
-};
-
-#define TIMEOUT_TICKS_1SEC 32768 /* ~1 second in RTC timer ticks */
-#define TIMEOUT_TICKS_1MS  32    /* ~1 millisecond in RTC timer ticks */
-#define wait_for_condition(cond, status) \
-do { \
-    unsigned timeout = get_uptime_32k() + TIMEOUT_TICKS_1SEC;  \
-    status = BLE_STATUS_SUCCESS; \
-    while (!(cond)) { \
-        if (get_uptime_32k() > timeout) { \
-            status = BLE_STATUS_TIMEOUT; \
-            break; \
-        } \
-    } \
-} while(0)
-
-static cfw_handle_t        client_handle;
-static svc_client_handle_t *service_handle;
-static uint16_t            conn_handle;
-static bool                connected;
-
-static ble_client_gap_event_cb_t ble_client_gap_event_cb;
-static void *ble_client_gap_event_param;
-
-static ble_client_gatts_event_cb_t ble_client_gatts_event_cb;
-static void *ble_client_gatts_event_param;
-
-volatile struct cfw_msg_rsp_sync sync;
-
-
-static void handle_msg_id_cfw_svc_avail_evt(cfw_svc_available_evt_msg_t *evt, void *param)
-{
-    if (evt->service_id == BLE_CORE_SERVICE_ID) {
-        sync.status = BLE_STATUS_SUCCESS;
-        sync.response = 1;
-    }
-}
-
-static void handle_msg_id_cfw_open_svc(cfw_open_conn_rsp_msg_t *rsp, void *param)
-{
-    service_handle = (svc_client_handle_t *)(rsp->client_handle);
-
-    sync.status = BLE_STATUS_SUCCESS;
-    sync.response = 1;
-}
-
-static void handle_msg_id_ble_gap_wr_conf_rsp(struct ble_rsp *rsp, void *param)
-{
-    sync.status = rsp->status;
-    sync.response = 1;
-}
-
-static void handle_msg_id_ble_gap_rd_bda_rsp(ble_bda_rd_rsp_t *rsp, void *param)
-{
-    ble_addr_t *p_bda = (ble_addr_t *)sync.param;
-
-    if (p_bda && BLE_STATUS_SUCCESS == rsp->status)
-        memcpy(p_bda, &rsp->bd, sizeof(*p_bda));
-
-    sync.status = rsp->status;
-    sync.response = 1;
-}
-
-static void handle_msg_id_ble_gap_sm_config_rsp(struct ble_rsp *rsp, void *param)
-{
-    sync.status = rsp->status;
-    sync.response = 1;
-}
-
-static void handle_msg_id_ble_gap_wr_adv_data_rsp(struct ble_rsp *rsp, void *param)
-{
-    sync.status = rsp->status;
-    sync.response = 1;
-}
-
-static void handle_msg_id_ble_gap_enable_adv_rsp(struct ble_rsp *rsp, void *param)
-{
-    /* No waiting for this response, so nothing to do here */
-}
-
-static void handle_msg_id_ble_gap_disable_adv_rsp(struct ble_rsp *rsp, void *param)
-{
-    /* No waiting for this response, so nothing to do here */
-}
+// APP callback
+static ble_client_connect_event_cb_t ble_client_connect_event_cb = NULL;
+static void *ble_client_connect_event_param;
 
-static void handle_msg_id_gatts_add_service_rsp(struct ble_gatts_add_svc_rsp *rsp, void *param)
-{
-    uint16_t *p_svc_handle = (uint16_t *)sync.param;
+static ble_client_disconnect_event_cb_t ble_client_disconnect_event_cb = NULL;
+static void *ble_client_disconnect_event_param;
 
-    if (p_svc_handle && BLE_STATUS_SUCCESS == rsp->status)
-        *p_svc_handle = rsp->svc_handle;
-
-    sync.status = rsp->status;
-    sync.response = 1;
-}
+static ble_client_update_param_event_cb_t ble_client_update_param_event_cb = NULL;
+static void *ble_client_update_param_event_param;
 
-static void handle_msg_id_gatts_add_characteristic_rsp(struct ble_gatts_add_char_rsp *rsp, void *param)
-{
-    struct ble_gatts_char_handles *p_handles = (struct ble_gatts_char_handles *)sync.param;
-
-    if (p_handles && BLE_STATUS_SUCCESS == rsp->status)
-        memcpy(p_handles, &rsp->char_h, sizeof(*p_handles));
-
-    sync.status = rsp->status;
-    sync.response = 1;
-}
-
-static void handle_msg_id_gatts_add_desc_rsp(struct ble_gatts_add_desc_rsp *rsp, void *param)
-{
-    uint16_t *p_handle = (uint16_t *)sync.param;
-
-    if (p_handle && BLE_STATUS_SUCCESS == rsp->status)
-        *p_handle = rsp->handle;
-
-    sync.status = rsp->status;
-    sync.response = 1;
-}
-
-static void handle_msg_id_ble_gatts_set_attribute_value_rsp(struct ble_gatts_set_attr_rsp_msg *rsp, void *param)
-{
-    sync.status = rsp->status;
-    sync.response = 1;
-}
 
-static void handle_msg_id_ble_gap_connect_evt_msg(struct ble_gap_event *evt, void *param)
-{
-    conn_handle = evt->conn_handle;
-    connected = true;
-
-    if (ble_client_gap_event_cb)
-        ble_client_gap_event_cb(BLE_CLIENT_GAP_EVENT_CONNECTED, evt, ble_client_gap_event_param);
-}
-
-static void handle_msg_id_ble_gap_disconnect_evt_msg(struct ble_gap_event *evt, void *param)
-{
-    connected = false;
-
-    if (ble_client_gap_event_cb)
-        ble_client_gap_event_cb(BLE_CLIENT_GAP_EVENT_DISCONNECTED, evt, ble_client_gap_event_param);
-}
-
-static void handle_msg_id_ble_gap_timeout_evt_msg(struct ble_gap_event *evt, void *param)
-{
-    connected = false;
-
-    if (!ble_client_gap_event_cb)
-        return;
-
-    switch (evt->timeout.reason) {
-    case BLE_SVC_GAP_TO_ADV:
-        ble_client_gap_event_cb(BLE_CLIENT_GAP_EVENT_ADV_TIMEOUT, evt, ble_client_gap_event_param);
-        break;
-    case BLE_SVC_GAP_TO_CONN:
-        ble_client_gap_event_cb(BLE_CLIENT_GAP_EVENT_CONN_TIMEOUT, evt, ble_client_gap_event_param);
-        break;
-    };
-}
-
-static void handle_msg_id_ble_gap_rssi_evt_msg(struct ble_gap_event *evt, void *param)
-{
-    if (ble_client_gap_event_cb)
-        ble_client_gap_event_cb(BLE_CLIENT_GAP_EVENT_RSSI, evt, ble_client_gap_event_param);
-}
+#define NIBBLE_TO_CHAR(n) \
+    ((n) >= 0xA ? ('A' + (n) - 0xA) : ('0' + (n)))
 
-static void handle_msg_id_ble_gatts_write_evt_msg(struct ble_gatts_evt_msg *evt, void *param)
-{
-    if (ble_client_gatts_event_cb)
-        ble_client_gatts_event_cb(BLE_CLIENT_GATTS_EVENT_WRITE, evt, ble_client_gatts_event_param);
-}
+#define BYTE_TO_STR(s, byte)               \
+    do {                                   \
+        *s++ = NIBBLE_TO_CHAR(byte >> 4);  \
+        *s++ = NIBBLE_TO_CHAR(byte & 0xF); \
+    }while(0)
 
-static void handle_msg_id_ble_gatts_send_notif_ind_rsp(ble_gatts_rsp_t *rsp, void *param)
-{
-    sync.status = rsp->status;
-    sync.response = 1;
-}
 
-static void handle_msg_id_ble_gap_disconnect_rsp(struct ble_rsp *rsp, void *param)
-{
-    sync.status = rsp->status;
-    sync.response = 1;
-}
+#ifdef __cplusplus
+extern "C" {
+#endif
 
-static void handle_msg_id_ble_gap_set_rssi_report_rsp(struct ble_rsp *rsp, void *param)
+static void on_connected(bt_conn_t *conn, uint8_t err)
 {
-    sync.status = rsp->status;
-    sync.response = 1;
+    if (ble_client_connect_event_cb)
+    {
+        ble_client_connect_event_cb(conn, err, ble_client_connect_event_param);
+    }
 }
 
-static void handle_msg_id_ble_gap_dtm_init_rsp(struct ble_generic_msg *rsp, void *param)
+static void on_disconnected(bt_conn_t *conn, uint8_t reason)
 {
-    sync.status = rsp->status;
-    sync.response = 1;
+    if (ble_client_disconnect_event_cb)
+    {
+        ble_client_disconnect_event_cb(conn, reason, ble_client_disconnect_event_param);
+    }
 }
 
-static void ble_core_client_handle_message(struct cfw_message *msg, void *param)
+static void on_le_param_updated(bt_conn_t *conn, uint16_t interval,
+                uint16_t latency, uint16_t timeout)
 {
-    switch (CFW_MESSAGE_ID(msg)) {
-
-    case MSG_ID_CFW_SVC_AVAIL_EVT:
-        handle_msg_id_cfw_svc_avail_evt((cfw_svc_available_evt_msg_t *)msg, param);
-        break;
-
-    case MSG_ID_CFW_OPEN_SERVICE:
-        handle_msg_id_cfw_open_svc((cfw_open_conn_rsp_msg_t *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_WR_CONF_RSP:
-        handle_msg_id_ble_gap_wr_conf_rsp((struct ble_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_RD_BDA_RSP:
-        handle_msg_id_ble_gap_rd_bda_rsp((ble_bda_rd_rsp_t *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_SM_CONFIG_RSP:
-        handle_msg_id_ble_gap_sm_config_rsp((struct ble_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_WR_ADV_DATA_RSP:
-        handle_msg_id_ble_gap_wr_adv_data_rsp((struct ble_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_ENABLE_ADV_RSP:
-        handle_msg_id_ble_gap_enable_adv_rsp((struct ble_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_DISABLE_ADV_RSP:
-        handle_msg_id_ble_gap_disable_adv_rsp((struct ble_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GATTS_ADD_SERVICE_RSP:
-        handle_msg_id_gatts_add_service_rsp((struct ble_gatts_add_svc_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GATTS_ADD_CHARACTERISTIC_RSP:
-        handle_msg_id_gatts_add_characteristic_rsp((struct ble_gatts_add_char_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GATTS_ADD_DESCRIPTOR_RSP:
-        handle_msg_id_gatts_add_desc_rsp((struct ble_gatts_add_desc_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GATTS_SET_ATTRIBUTE_VALUE_RSP:
-        handle_msg_id_ble_gatts_set_attribute_value_rsp((struct ble_gatts_set_attr_rsp_msg *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GATTS_SEND_NOTIF_RSP:
-    case MSG_ID_BLE_GATTS_SEND_IND_RSP:
-        handle_msg_id_ble_gatts_send_notif_ind_rsp((ble_gatts_rsp_t *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_CONNECT_EVT:
-        handle_msg_id_ble_gap_connect_evt_msg((struct ble_gap_event *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_DISCONNECT_EVT:
-        handle_msg_id_ble_gap_disconnect_evt_msg((struct ble_gap_event *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_TO_EVT:
-        handle_msg_id_ble_gap_timeout_evt_msg((struct ble_gap_event *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_RSSI_EVT:
-        handle_msg_id_ble_gap_rssi_evt_msg((struct ble_gap_event *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GATTS_WRITE_EVT:
-        handle_msg_id_ble_gatts_write_evt_msg((struct ble_gatts_evt_msg *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_DISCONNECT_RSP:
-        handle_msg_id_ble_gap_disconnect_rsp((struct ble_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_SET_RSSI_REPORT_RSP:
-        handle_msg_id_ble_gap_set_rssi_report_rsp((struct ble_rsp *)msg, param);
-        break;
-
-    case MSG_ID_BLE_GAP_DTM_INIT_RSP:
-        handle_msg_id_ble_gap_dtm_init_rsp((struct ble_generic_msg *)msg, param);
-        break;
+    if (ble_client_update_param_event_cb)
+    {
+        ble_client_update_param_event_cb (conn, 
+                                          interval,
+                                          latency, 
+                                          timeout, 
+                                          ble_client_update_param_event_param);
     }
-    cfw_msg_free(msg);
 }
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#define NIBBLE_TO_CHAR(n) \
-    ((n) >= 0xA ? ('A' + (n) - 0xA) : ('0' + (n)))
-
-#define BYTE_TO_STR(s, byte)               \
-    do {                                   \
-        *s++ = NIBBLE_TO_CHAR(byte >> 4);  \
-        *s++ = NIBBLE_TO_CHAR(byte & 0xF); \
-    }while(0)
+static struct bt_conn_cb conn_callbacks = {
+    .connected = on_connected,
+    .disconnected = on_disconnected,
+    .le_param_updated = on_le_param_updated
+};
 
-void ble_client_get_factory_config(ble_addr_t *bda, char *name)
+void ble_client_get_mac_address(bt_addr_le_t *bda)
 {
     struct curie_oem_data *p_oem = NULL;
     unsigned i;
@@ -382,10 +126,17 @@ void ble_client_get_factory_config(ble_addr_t *bda, char *name)
             if (p_oem->bt_mac_address_type < 2) {
                 bda->type = p_oem->bt_mac_address_type;
                 for (i = 0; i < BLE_ADDR_LEN; i++)
-                    bda->addr[i] = p_oem->bt_address[BLE_ADDR_LEN - 1 - i];
+                    bda->val[i] = p_oem->bt_address[BLE_ADDR_LEN - 1 - i];
             }
         }
     }
+}
+
+void ble_client_get_factory_config(bt_addr_le_t *bda, char *name)
+{
+    struct curie_oem_data *p_oem = NULL;
+    
+    ble_client_get_mac_address(bda);
 
     /* Set a default name if one has not been specified */
     if (name) {
@@ -419,466 +170,73 @@ void ble_client_get_factory_config(ble_addr_t *bda, char *name)
         if (bda && bda->type != BLE_DEVICE_ADDR_INVALID) 
         {
             *suffix++ = '-';
-             BYTE_TO_STR(suffix, p_oem->bt_address[4]);
-             BYTE_TO_STR(suffix, p_oem->bt_address[5]);
-	     *suffix = 0; /* NULL-terminate the string. Note the macro BYTE_TO_STR 
+            p_oem = (struct curie_oem_data *) &global_factory_data->oem_data.project_data;
+            BYTE_TO_STR(suffix, p_oem->bt_address[4]);
+            BYTE_TO_STR(suffix, p_oem->bt_address[5]);
+            *suffix = 0; /* NULL-terminate the string. Note the macro BYTE_TO_STR 
                              automatically move the pointer */
         }
         else
         {
-	     /* This code segment will be only reached if Curie module was not 
+         /* This code segment will be only reached if Curie module was not 
                 provisioned properly with a BLE MAC address*/
              *suffix++ = 0; /* NULL-terminate the string */
         }
     }
 }
 
-BleStatus ble_client_init(ble_client_gap_event_cb_t gap_event_cb, void *gap_event_param,
-                          ble_client_gatts_event_cb_t gatts_event_cb, void *gatts_event_param)
-{
-    BleStatus status;
-    uint32_t delay_until;
-
-    cfw_platform_nordic_init();
-
-    client_handle = cfw_init(cfw_get_service_queue(),
-                             ble_core_client_handle_message,
-                             NULL);
-
-    sync.response = 0;
-    if (cfw_register_svc_available(client_handle,
-                                   BLE_CORE_SERVICE_ID,
-                                   NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    /* We need to wait for ~1 ms before continuing */
-    delay_until = get_uptime_32k() + TIMEOUT_TICKS_1MS;
-    while (get_uptime_32k() < delay_until);
-
-    sync.response = 0;
-    cfw_open_service(client_handle,
-                     BLE_CORE_SERVICE_ID,
-                     NULL);
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    ble_client_gap_event_cb = gap_event_cb;
-    ble_client_gap_event_param = gap_event_param;
-
-    ble_client_gatts_event_cb = gatts_event_cb;
-    ble_client_gatts_event_param = gatts_event_param;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gap_set_enable_config(const char *name,
-                                           const ble_addr_t *bda,
-                                           const uint16_t appearance,
-                                           const int8_t tx_power,
-                                           const uint16_t min_conn_interval,
-                                           const uint16_t max_conn_interval)
-{
-    struct ble_wr_config config;
-    BleStatus status;
-
-    config.p_bda = (bda && bda->type != BLE_DEVICE_ADDR_INVALID) ? (ble_addr_t *)bda : NULL;
-    config.p_name = (uint8_t *)name;
-    config.appearance = appearance;
-    config.tx_power = tx_power;
-    config.peripheral_conn_params.interval_min = min_conn_interval;
-    config.peripheral_conn_params.interval_max = max_conn_interval;
-    config.peripheral_conn_params.slave_latency = SLAVE_LATENCY;
-    config.peripheral_conn_params.link_sup_to = CONN_SUP_TIMEOUT;
-    config.central_conn_params.interval_min = min_conn_interval;
-    config.central_conn_params.interval_max = max_conn_interval;
-    config.central_conn_params.slave_latency = SLAVE_LATENCY;
-    config.central_conn_params.link_sup_to = CONN_SUP_TIMEOUT;
-
-    sync.response = 0;
-    if (ble_gap_set_enable_config(service_handle, &config, NULL))
-        return BLE_STATUS_ERROR;
-    /* Wait for response message */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-    if (sync.status)
-        return sync.status;
-
-    struct ble_gap_sm_config_params sm_params = {
-        .options = BLE_GAP_BONDING,
-        .io_caps = BLE_GAP_IO_NO_INPUT_NO_OUTPUT,
-        .key_size = 16,
-    };
-    sync.response = 0;
-    if (ble_gap_sm_config(service_handle, &sm_params, NULL))
-        return BLE_STATUS_ERROR;
-    /* Wait for response message */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gap_get_bda(ble_addr_t *p_bda)
-{
-    BleStatus status;
-
-    sync.response = 0;
-    sync.param = (void *)p_bda;
-    if (ble_gap_read_bda(service_handle, NULL))
-        return BLE_STATUS_ERROR;
-    /* Wait for response message */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gap_wr_adv_data(uint8_t *adv_data, const uint8_t adv_data_len)
-{
-    BleStatus status;
-
-    struct ble_gap_adv_rsp_data adv_rsp_data = {
-        .p_data = adv_data,
-        .len = adv_data_len,
-    };
-
-    /* write advertisement data */
-    sync.response = 0;
-    if (ble_gap_wr_adv_data(service_handle, &adv_rsp_data, NULL, NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gap_start_advertise(uint16_t timeout)
-{
-    /* Hard-coding these advertising parameters for now
-     * Could be changed to support advanced features such as:
-     * - slow advertising
-     * - directed advertising
-     * - whitelist filtering
-     * - etc.
-     */
-    ble_gap_adv_param_t adv_params = {
-        .timeout = timeout,
-        .interval_min = APP_ULTRA_FAST_ADV_INTERVAL,
-        .interval_max = APP_ULTRA_FAST_ADV_INTERVAL,
-        .type = BLE_GAP_ADV_TYPE_ADV_IND,
-        .filter_policy = BLE_GAP_ADV_FP_ANY,
-        .p_peer_bda = NULL,
-        .options = BLE_GAP_OPT_ADV_DEFAULT,
-    };
-
-    /* For this message, we don't wait for the response, just fire
-     * and forget.  This allows us to invoke it within the
-     * disconnect event handler to restart the connection
-     */
-    return ble_gap_start_advertise(service_handle, &adv_params, NULL);
-}
-
-BleStatus ble_client_gap_stop_advertise(void)
+void ble_client_init(ble_client_connect_event_cb_t connect_cb, void* connect_param,
+                     ble_client_disconnect_event_cb_t disconnect_cb, void* disconnect_param,
+                     ble_client_update_param_event_cb_t update_param_cb, void* update_param_param)
 {
-    /* For this message, we don't wait for the response, just fire
-     * and forget.
-     */
-    return ble_gap_stop_advertise(service_handle, NULL);
+    //uint32_t delay_until;
+    pr_info(LOG_MODULE_BLE, "%s", __FUNCTION__);
+    ble_client_connect_event_cb = connect_cb;
+    ble_client_connect_event_param = connect_param;
+    
+    ble_client_disconnect_event_cb = disconnect_cb;
+    ble_client_disconnect_event_param = disconnect_param;
+    
+    ble_client_update_param_event_cb = update_param_cb;
+    ble_client_update_param_event_param = update_param_param;
+    
+    bt_conn_cb_register(&conn_callbacks);
+    return;
 }
 
-BleStatus ble_client_gatts_add_service(const struct bt_uuid *uuid,
-				       const uint8_t type,
-				       uint16_t *svc_handle)
+BLE_STATUS_T errorno_to_ble_status(int err)
 {
-    BleStatus status;
-
-    sync.response = 0;
-    sync.param = (void *)svc_handle;
-    if (ble_gatts_add_service(service_handle, uuid, type, NULL, NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gatts_include_service(const uint16_t primary_svc_handle,
-					   const uint16_t included_svc_handle)
-{
-    BleStatus status;
-
-    sync.response = 0;
-    if (ble_gatts_add_included_svc(service_handle,
-				   primary_svc_handle,
-				   included_svc_handle,
-				   NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gatts_add_characteristic(const uint16_t svc_handle,
-                                              struct ble_gatts_characteristic *char_data,
-                                              struct ble_gatts_char_handles *handles)
-{
-    BleStatus status;
-
-    sync.response = 0;
-    sync.param = (void *)handles;
-
-    if (ble_gatts_add_characteristic(service_handle, svc_handle, char_data,
-                                     NULL, NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gatts_add_descriptor(const uint16_t svc_handle,
-                                          struct ble_gatts_descriptor *desc,
-                                          uint16_t *handle)
-{
-    BleStatus status;
-
-    sync.response = 0;
-    sync.param = (void *)handle;
-
-    if (ble_gatts_add_descriptor(service_handle, desc, NULL, NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gatts_set_attribute_value(const uint16_t value_handle,
-					       const uint16_t len, const uint8_t * p_value,
-					       const uint16_t offset)
-{
-    BleStatus status;
-
-    sync.response = 0;
-    if (ble_gatts_set_attribute_value(service_handle, value_handle,
-				      len, p_value, offset, NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gatts_send_notif_ind(const uint16_t value_handle,
-                                          const uint16_t len, uint8_t * p_value,
-                                          const uint16_t offset,
-                                          const bool indication)
-{
-    BleStatus status;
-
-    ble_gatts_ind_params_t ind_params = {
-        .val_handle = value_handle,
-        .len = len,
-        .p_data = p_value,
-        .offset = offset,
-    };
-
-    sync.response = 0;
-    if (indication)
-        status = ble_gatts_send_ind(service_handle, conn_handle, &ind_params, NULL, NULL);
-    else
-        status = ble_gatts_send_notif(service_handle, conn_handle, &ind_params, NULL, NULL);
-
-    if (status)
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gap_disconnect(const uint8_t reason)
-{
-    BleStatus status;
-
-    if (!connected)
-        return BLE_STATUS_WRONG_STATE;
-
-    sync.response = 0;
-    if (ble_gap_disconnect(service_handle, conn_handle, reason, NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_gap_set_rssi_report(boolean_t enable)
-{
-    BleStatus status;
-    struct rssi_report_params params;
-
-    if (!connected)
-        return BLE_STATUS_WRONG_STATE;
-
-    params.conn_hdl = conn_handle;
-    params.op = enable ? BLE_GAP_RSSI_ENABLE_REPORT : BLE_GAP_RSSI_DISABLE_REPORT;
-    /* TODO - pick sensible defaults for these and/or allow user to specify */
-    params.delta_dBm = 5;
-    params.min_count = 3;
-
-    sync.response = 0;
-    if (ble_gap_set_rssi_report(service_handle, &params, NULL))
-        return BLE_STATUS_ERROR;
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    return sync.status;
-}
-
-BleStatus ble_client_dtm_init(void)
-{
-    BleStatus status;
-
-    /* Ensure that the ble_client_init() has been called already */
-    if (!service_handle)
-        return BLE_STATUS_WRONG_STATE;
-
-    /* Instruct the Nordic to enter Direct Test Mode */
-    sync.response = 0;
-    ble_gap_dtm_init_req(service_handle, NULL);
-
-    /* Wait for response messages */
-    wait_for_condition(sync.response, status);
-    if (status != BLE_STATUS_SUCCESS)
-        return status;
-
-    /* DTM is active. Detach UART IPC driver to allow direct access */
-	if (BLE_STATUS_SUCCESS == sync.status)
-        uart_ipc_disable(IPC_UART);
-
-    return sync.status;
-}
-
-static int uart_raw_ble_core_tx_rx(uint8_t * send_data, uint8_t send_no,
-			       uint8_t * rcv_data, uint8_t rcv_no)
-{
-	int i;
-	uint8_t rx_byte;
-	int res;
-	/* send command */
-	for (i = 0; i < send_no; i++)
-		uart_poll_out(IPC_UART, send_data[i]);
-	/* answer */
-	i = 0;
-	do {
-		res = uart_poll_in(IPC_UART, &rx_byte);
-		if (res == 0) {
-			rcv_data[i++] = rx_byte;
-		}
-	} while (i < rcv_no);
-	return i;
+    BLE_STATUS_T err_code;
+    err = 0 - err;
+    
+    switch(err) {
+    case 0:
+        err_code = BLE_STATUS_SUCCESS;
+        break;
+    case EIO:
+        err_code = BLE_STATUS_WRONG_STATE;
+        break;
+    case EBUSY:
+        err_code = BLE_STATUS_TIMEOUT;
+        break;
+    case EFBIG:
+    case ENOTSUP:
+        err_code = BLE_STATUS_NOT_SUPPORTED;
+        break;
+    case EPERM:
+    case EACCES:
+        err_code = BLE_STATUS_NOT_ALLOWED;
+        break;
+    case ENOMEM: // No memeory
+        err_code = BLE_STATUS_NO_MEMORY;
+        break;
+    default:
+        err_code = BLE_STATUS_ERROR;
+        break;
+    }
+    return err_code;
 }
 
-BleStatus ble_client_dtm_cmd(const struct ble_test_cmd *test_cmd,
-                             struct ble_dtm_test_result *test_result)
-{
-    BleStatus status;
-
-	uint8_t send_data[7];
-	uint8_t rcv_data[9] = {};
-	int send_no;
-	int rcv_no;
-
-	send_data[0] = DTM_HCI_CMD;
-	send_data[1] = test_cmd->mode;
-	send_data[2] = DTM_HCI_OPCODE2;
-
-	switch (test_cmd->mode) {
-	case BLE_TEST_START_DTM_RX:
-		send_data[3] = 1;	/* length */
-		send_data[4] = test_cmd->rx.freq;
-		send_no = 5;
-		rcv_no = 7;
-		break;
-	case BLE_TEST_START_DTM_TX:
-		send_data[3] = 3;	/* length */
-		send_data[4] = test_cmd->tx.freq;
-		send_data[5] = test_cmd->tx.len;
-		send_data[6] = test_cmd->tx.pattern;
-		send_no = 7;
-		rcv_no = 7;
-		break;
-	case BLE_TEST_SET_TXPOWER:
-		send_data[3] = 1;	/* length */
-		send_data[4] = test_cmd->tx_pwr.dbm;
-		send_no = 5;
-		rcv_no = 7;
-		break;
-	case BLE_TEST_END_DTM:
-		send_data[3] = 0;	/* length */
-		send_no = 4;
-		rcv_no = 9;
-		break;
-	default:
-		return BLE_STATUS_NOT_SUPPORTED;
-	}
-
-	uart_raw_ble_core_tx_rx(send_data, send_no, rcv_data, rcv_no);
-
-	status = rcv_data[DTM_HCI_STATUS_IDX];
-
-    test_result->mode = test_cmd->mode;
-
-	uint8_t *p;
-	switch (test_cmd->mode) {
-	case BLE_TEST_END_DTM:
-		p = &rcv_data[DTM_HCI_LE_END_IDX];
-		LESTREAM_TO_UINT16(p, test_result->nb);
-		break;
-	}
-
-    return status;
-}
 
 #ifdef __cplusplus
 }
diff --git a/libraries/CurieBLE/src/internal/ble_client.h b/libraries/CurieBLE/src/internal/ble_client.h
index 3fe47bd4..75c3d59f 100644
--- a/libraries/CurieBLE/src/internal/ble_client.h
+++ b/libraries/CurieBLE/src/internal/ble_client.h
@@ -40,12 +40,13 @@ enum {
 };
 
 #define MSEC_TO_UNITS(TIME, RESOLUTION) (((TIME) * 1000) / (RESOLUTION))
+#define UNITS_TO_MSEC(TIME, RESOLUTION) (((TIME) * RESOLUTION) / 1000)
 
 /* Connection parameters used for Peripheral Preferred Connection Parameterss (PPCP) and update request */
 #define DEFAULT_MIN_CONN_INTERVAL MSEC_TO_UNITS(80, UNIT_1_25_MS)
 #define DEFAULT_MAX_CONN_INTERVAL MSEC_TO_UNITS(150, UNIT_1_25_MS)
 #define MIN_CONN_INTERVAL 0x0006
-#define MAX_CONN_INTERVAL 0x095f
+#define MAX_CONN_INTERVAL 0x0C80
 #define SLAVE_LATENCY 0
 #define CONN_SUP_TIMEOUT MSEC_TO_UNITS(6000, UNIT_10_MS)
 
@@ -87,63 +88,28 @@ enum {
         uuid.type = BT_UUID128;                                         \
     } while(0)
 
-typedef enum {
-    BLE_CLIENT_GAP_EVENT_CONNECTED = 0,
-    BLE_CLIENT_GAP_EVENT_DISCONNECTED,
-    BLE_CLIENT_GAP_EVENT_ADV_TIMEOUT,
-    BLE_CLIENT_GAP_EVENT_CONN_TIMEOUT,
-    BLE_CLIENT_GAP_EVENT_RSSI,
-} ble_client_gap_event_t;
 
-typedef enum {
-    BLE_CLIENT_GATTS_EVENT_WRITE = 0,
-} ble_client_gatts_event_t;
+typedef void (*ble_client_connect_event_cb_t)(struct bt_conn *conn, uint8_t err, void *param);
+typedef void (*ble_client_disconnect_event_cb_t)(struct bt_conn *conn, uint8_t reason, void *param);
+typedef void (*ble_client_update_param_event_cb_t)(struct bt_conn *conn, 
+                                                   uint16_t interval,
+                                                   uint16_t latency, 
+                                                   uint16_t timeout, 
+                                                   void *param);
 
-typedef void (*ble_client_gap_event_cb_t)(ble_client_gap_event_t event, struct ble_gap_event *event_data, void *param);
-typedef void (*ble_client_gatts_event_cb_t)(ble_client_gatts_event_t event, struct ble_gatts_evt_msg *event_data, void *param);
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-void ble_client_get_factory_config(ble_addr_t *bda, char *name);
-BleStatus ble_client_init(ble_client_gap_event_cb_t gap_event_cb,
-                          void *gap_event_param,
-                          ble_client_gatts_event_cb_t gatts_event_cb,
-                          void *gatts_event_param);
-BleStatus ble_client_gap_set_enable_config(const char *name,
-                                           const ble_addr_t *bda,
-                                           const uint16_t appearance,
-                                           const int8_t tx_power,
-                                           const uint16_t min_conn_interval,
-                                           const uint16_t max_conn_interval);
-BleStatus ble_client_gap_get_bda(ble_addr_t *p_bda);
-BleStatus ble_client_gap_wr_adv_data(uint8_t *adv_data,
-                                     const uint8_t adv_data_len);
-BleStatus ble_client_gap_start_advertise(uint16_t timeout);
-BleStatus ble_client_gap_stop_advertise(void);
-BleStatus ble_client_gatts_add_service(const struct bt_uuid *uuid, const uint8_t type, uint16_t *svc_handle);
-BleStatus ble_client_gatts_include_service(const uint16_t primary_svc_handle, uint16_t included_svc_handle);
-BleStatus ble_client_gatts_add_characteristic(const uint16_t svc_handle,
-                                              struct ble_gatts_characteristic *char_data,
-                                              struct ble_gatts_char_handles *handles);
-BleStatus ble_client_gatts_add_descriptor(const uint16_t svc_handle,
-                                          struct ble_gatts_descriptor *desc,
-                                          uint16_t *handle);
-BleStatus ble_client_gatts_set_attribute_value(const uint16_t value_handle,
-                                               const uint16_t len, const uint8_t *value,
-                                               const uint16_t offset);
-BleStatus ble_client_gatts_send_notif_ind(const uint16_t value_handle,
-                                          const uint16_t len, uint8_t * p_value,
-                                          const uint16_t offset,
-                                          const bool indication);
-BleStatus ble_client_gap_disconnect(const uint8_t reason);
-BleStatus ble_client_gap_set_rssi_report(boolean_t enable);
-
-/* Direct Test Mode (DTM) API - for internal use only */
-BleStatus ble_client_dtm_init(void);
-BleStatus ble_client_dtm_cmd(const struct ble_test_cmd *test_cmd,
-                             struct ble_dtm_test_result *test_result);
+void ble_client_init(ble_client_connect_event_cb_t connect_cb, void* connect_param,
+                     ble_client_disconnect_event_cb_t disconnect_cb, void* disconnect_param,
+                     ble_client_update_param_event_cb_t update_param_cb, void* update_param_param);
+void ble_client_get_factory_config(bt_addr_le_t *bda, char *name);
+void ble_gap_set_tx_power(int8_t tx_power);
+BLE_STATUS_T errorno_to_ble_status(int err);
+
+void ble_client_get_mac_address(bt_addr_le_t *bda);
 
 #ifdef __cplusplus
 }
diff --git a/libraries/CurieI2S/examples/I2SDMA_RXCallBack/I2SDMA_RXCallBack.ino b/libraries/CurieI2S/examples/I2SDMA_RXCallBack/I2SDMA_RXCallBack.ino
index ed099fa1..188800a1 100644
--- a/libraries/CurieI2S/examples/I2SDMA_RXCallBack/I2SDMA_RXCallBack.ino
+++ b/libraries/CurieI2S/examples/I2SDMA_RXCallBack/I2SDMA_RXCallBack.ino
@@ -22,8 +22,8 @@
 **/
 #include <CurieI2SDMA.h>
 
-#define BUFF_SIZE 128
-#define OFFSET 2
+const int BUFF_SIZE=64;
+const int OFFSET=2;
 uint32_t dataBuff[BUFF_SIZE+OFFSET]; // extra 2 buffers are for the padding zero
 
 /** 
diff --git a/libraries/CurieI2S/examples/I2SDMA_TXCallBack/I2SDMA_TXCallBack.ino b/libraries/CurieI2S/examples/I2SDMA_TXCallBack/I2SDMA_TXCallBack.ino
index 9ce55501..52b8de4c 100644
--- a/libraries/CurieI2S/examples/I2SDMA_TXCallBack/I2SDMA_TXCallBack.ino
+++ b/libraries/CurieI2S/examples/I2SDMA_TXCallBack/I2SDMA_TXCallBack.ino
@@ -9,8 +9,8 @@
 
 #include <CurieI2SDMA.h>
 
-#define BUFF_SIZE 128
-boolean blinkState = true;          // state of the LED
+const int BUFF_SIZE=64;
+bool blinkState = true;          // state of the LED
 uint32_t dataBuff[BUFF_SIZE];
 uint32_t loop_count = 0;
 void setup() 
diff --git a/libraries/CurieI2S/examples/I2S_RxCallback/I2S_RxCallback.ino b/libraries/CurieI2S/examples/I2S_RxCallback/I2S_RxCallback.ino
index 9550ad1f..06b4db8e 100644
--- a/libraries/CurieI2S/examples/I2S_RxCallback/I2S_RxCallback.ino
+++ b/libraries/CurieI2S/examples/I2S_RxCallback/I2S_RxCallback.ino
@@ -17,7 +17,9 @@
 **/
 #include <CurieI2S.h>
 
-uint32_t dataBuff[256];
+const int BUFF_SIZE=128;
+
+uint32_t dataBuff[BUFF_SIZE];
 volatile int count = 0;
 
 void setup() 
@@ -54,6 +56,6 @@ void rxDataReceived()
   while(CurieI2S.available())
   {
     dataBuff[count++] =  CurieI2S.requestdword();   
-    count %= 256; //prevent buffer overflow and just write data in front of the buffer.
+    count %= BUFF_SIZE; //prevent buffer overflow and just write data in front of the buffer.
   }
 }
diff --git a/libraries/CurieI2S/src/CurieI2S.cpp b/libraries/CurieI2S/src/CurieI2S.cpp
index 6179b34e..92785ee6 100644
--- a/libraries/CurieI2S/src/CurieI2S.cpp
+++ b/libraries/CurieI2S/src/CurieI2S.cpp
@@ -419,9 +419,6 @@ void Curie_I2S::muxRX(bool enable)
     SET_PIN_MODE(49, mux_mode); //I2S_RXD
     SET_PIN_MODE(51, mux_mode); //I2S_RWS
     SET_PIN_MODE(50,  mux_mode); //I2S_RSCK
-    g_APinDescription[I2S_RXD].ulPinMode = mux_mode;
-    g_APinDescription[I2S_RWS].ulPinMode = mux_mode;
-    g_APinDescription[I2S_RSCK].ulPinMode  = mux_mode;
 }
 
 void Curie_I2S::muxTX(bool enable)
@@ -436,9 +433,6 @@ void Curie_I2S::muxTX(bool enable)
     SET_PIN_MODE(g_APinDescription[I2S_TXD].ulSocPin, mux_mode);
     SET_PIN_MODE(g_APinDescription[I2S_TWS].ulSocPin, mux_mode);
     SET_PIN_MODE(g_APinDescription[I2S_TSCK].ulSocPin,  mux_mode);
-    g_APinDescription[I2S_TXD].ulPinMode = mux_mode;
-    g_APinDescription[I2S_TWS].ulPinMode = mux_mode;
-    g_APinDescription[I2S_TSCK].ulPinMode  = mux_mode;
 }
 
 void Curie_I2S::initRX()
diff --git a/libraries/CurieI2S/src/CurieI2SDMA.cpp b/libraries/CurieI2S/src/CurieI2SDMA.cpp
index 9ac8de50..3840ecde 100644
--- a/libraries/CurieI2S/src/CurieI2SDMA.cpp
+++ b/libraries/CurieI2S/src/CurieI2SDMA.cpp
@@ -110,9 +110,6 @@ void Curie_I2SDMA::muxTX(bool enable)
 	SET_PIN_MODE(g_APinDescription[I2S_TXD].ulSocPin, mux_mode);
 	SET_PIN_MODE(g_APinDescription[I2S_TWS].ulSocPin, mux_mode);
 	SET_PIN_MODE(g_APinDescription[I2S_TSCK].ulSocPin, mux_mode);
-	g_APinDescription[I2S_TXD].ulPinMode = mux_mode;
-	g_APinDescription[I2S_TWS].ulPinMode = mux_mode;
-	g_APinDescription[I2S_TSCK].ulPinMode  = mux_mode;
 }
 
 void Curie_I2SDMA::muxRX(bool enable)
@@ -127,9 +124,6 @@ void Curie_I2SDMA::muxRX(bool enable)
 	SET_PIN_MODE(49, mux_mode); //I2S_RXD
 	SET_PIN_MODE(51, mux_mode); //I2S_RWS
 	SET_PIN_MODE(50,  mux_mode); //I2S_RSCK
-	g_APinDescription[I2S_RXD].ulPinMode = mux_mode;
-	g_APinDescription[I2S_RWS].ulPinMode = mux_mode;
-	g_APinDescription[I2S_RSCK].ulPinMode  = mux_mode;
 }
 
 int Curie_I2SDMA::beginTX(uint16_t sample_rate,uint8_t resolution,uint8_t master,uint8_t mode)
diff --git a/libraries/CurieIMU/examples/FreeFallDetect/FreeFallDetect.ino b/libraries/CurieIMU/examples/FreeFallDetect/FreeFallDetect.ino
index ca88e457..d964270a 100644
--- a/libraries/CurieIMU/examples/FreeFallDetect/FreeFallDetect.ino
+++ b/libraries/CurieIMU/examples/FreeFallDetect/FreeFallDetect.ino
@@ -10,7 +10,7 @@
 
 #include "CurieIMU.h"
 
-boolean blinkState = false;          // state of the LED
+bool blinkState = false;          // state of the LED
 unsigned long loopTime = 0;          // get the time since program started
 unsigned long interruptsTime = 0;    // get the time when free fall event is detected
 
diff --git a/libraries/CurieIMU/examples/MotionDetect/MotionDetect.ino b/libraries/CurieIMU/examples/MotionDetect/MotionDetect.ino
index 400ad461..cb94d85a 100644
--- a/libraries/CurieIMU/examples/MotionDetect/MotionDetect.ino
+++ b/libraries/CurieIMU/examples/MotionDetect/MotionDetect.ino
@@ -10,7 +10,7 @@
 
 #include "CurieIMU.h"
 
-boolean blinkState = false;          // state of the LED
+bool blinkState = false;          // state of the LED
 unsigned long loopTime = 0;          // get the time since program started
 unsigned long interruptsTime = 0;    // get the time when motion event is detected
 
diff --git a/libraries/CurieIMU/examples/RawImuDataSerial/RawImuDataSerial.ino b/libraries/CurieIMU/examples/RawImuDataSerial/RawImuDataSerial.ino
index d61da508..08eed73b 100644
--- a/libraries/CurieIMU/examples/RawImuDataSerial/RawImuDataSerial.ino
+++ b/libraries/CurieIMU/examples/RawImuDataSerial/RawImuDataSerial.ino
@@ -35,7 +35,7 @@ int ax, ay, az;         // accelerometer values
 int gx, gy, gz;         // gyrometer values
 
 const int ledPin = 13;      // activity LED pin
-boolean blinkState = false; // state of the LED
+bool blinkState = false; // state of the LED
 
 int calibrateOffsets = 1; // int to determine whether calibration takes place or not
 
diff --git a/libraries/CurieIMU/examples/ShockDetect/ShockDetect.ino b/libraries/CurieIMU/examples/ShockDetect/ShockDetect.ino
index 467299a2..9f2d7a9e 100644
--- a/libraries/CurieIMU/examples/ShockDetect/ShockDetect.ino
+++ b/libraries/CurieIMU/examples/ShockDetect/ShockDetect.ino
@@ -10,7 +10,7 @@
 
 #include "CurieIMU.h"
 
-boolean blinkState = false;          // state of the LED
+bool blinkState = false;          // state of the LED
 
 void setup() {
   Serial.begin(9600); // initialize Serial communication
diff --git a/libraries/CurieIMU/examples/StepCount/StepCount.ino b/libraries/CurieIMU/examples/StepCount/StepCount.ino
index aafc9483..9ebcadb0 100644
--- a/libraries/CurieIMU/examples/StepCount/StepCount.ino
+++ b/libraries/CurieIMU/examples/StepCount/StepCount.ino
@@ -20,9 +20,9 @@
 */
 const int ledPin = 13;
 
-boolean stepEventsEnabeled = true;   // whether you're polling or using events
+bool stepEventsEnabeled = true;   // whether you're polling or using events
 long lastStepCount = 0;              // step count on previous polling check
-boolean blinkState = false;          // state of the LED
+bool blinkState = false;          // state of the LED
 
 void setup() {
   Serial.begin(9600); // initialize Serial communication
diff --git a/libraries/CurieIMU/examples/ZeroMotionDetect/ZeroMotionDetect.ino b/libraries/CurieIMU/examples/ZeroMotionDetect/ZeroMotionDetect.ino
index 56fc3a9d..1f7d11fe 100644
--- a/libraries/CurieIMU/examples/ZeroMotionDetect/ZeroMotionDetect.ino
+++ b/libraries/CurieIMU/examples/ZeroMotionDetect/ZeroMotionDetect.ino
@@ -9,7 +9,7 @@
 */
 #include "CurieIMU.h"
 
-boolean ledState = false;          // state of the LED
+bool ledState = false;          // state of the LED
 void setup() {
   Serial.begin(9600); // initialize Serial communication
   while(!Serial) ;    // wait for serial port to connect.
diff --git a/libraries/CurieIMU/keywords.txt b/libraries/CurieIMU/keywords.txt
index 1c64688b..11bd0056 100644
--- a/libraries/CurieIMU/keywords.txt
+++ b/libraries/CurieIMU/keywords.txt
@@ -14,6 +14,7 @@ CurieIMUClass	KEYWORD1
 
 begin	KEYWORD1
 
+dataReady	KEYWORD1
 getGyroRate	KEYWORD1
 setGyroRate	KEYWORD1
 getAccelerometerRate	KEYWORD1
@@ -64,7 +65,7 @@ readAcceleration	KEYWORD1
 readRotation	KEYWORD1
 
 readAccelerometer	KEYWORD1
-readAccelerometerScaled KEYWORD1
+readAccelerometerScaled	KEYWORD1
 readGyro	KEYWORD1
 readGyroScaled	KEYWORD1
 readTemperature	KEYWORD1
@@ -84,6 +85,9 @@ CurieIMU	KEYWORD2
 # Constants (LITERAL1)
 #######################################
 
+ACCEL	LITERAL1
+GYRO	LITERAL1
+
 X_AXIS	LITERAL1
 Y_AXIS	LITERAL1
 Z_AXIS	LITERAL1
diff --git a/libraries/CurieIMU/src/BMI160.cpp b/libraries/CurieIMU/src/BMI160.cpp
index 746e8123..c8945b38 100644
--- a/libraries/CurieIMU/src/BMI160.cpp
+++ b/libraries/CurieIMU/src/BMI160.cpp
@@ -78,6 +78,11 @@ uint8_t BMI160Class::reg_read_bits(uint8_t reg, unsigned pos, unsigned len)
     return b;
 }
 
+int BMI160Class::isBitSet(uint8_t value, unsigned bit)
+{
+    return value & (1 << bit);
+}
+
 /******************************************************************************/
 
 /** Power on and prepare for general usage.
@@ -85,8 +90,10 @@ uint8_t BMI160Class::reg_read_bits(uint8_t reg, unsigned pos, unsigned len)
  * after start-up). This function also sets both the accelerometer and the gyroscope
  * to default range settings, namely +/- 2g and +/- 250 degrees/sec.
  */
-void BMI160Class::initialize()
+void BMI160Class::initialize(unsigned int flags)
 {
+    sensors_enabled = 0;
+
     /* Issue a soft-reset to bring the device into a clean state */
     reg_write(BMI160_RA_CMD, BMI160_CMD_SOFT_RESET);
     delay(1);
@@ -95,26 +102,36 @@ void BMI160Class::initialize()
     reg_read(0x7F);
     delay(1);
 
-    /* Power up the accelerometer */
-    reg_write(BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL);
-    delay(1);
-    /* Wait for power-up to complete */
-    while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
+    if (flags & ACCEL) {
+        /* Power up the accelerometer */
+        reg_write(BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL);
+        delay(1);
+
+        /* Wait for power-up to complete */
+        while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
                                 BMI160_ACC_PMU_STATUS_BIT,
                                 BMI160_ACC_PMU_STATUS_LEN))
+            delay(1);
+
+        sensors_enabled |= ACCEL;
+    }
+
+    if (flags & GYRO) {
+        /* Power up the gyroscope */
+        reg_write(BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL);
         delay(1);
 
-    /* Power up the gyroscope */
-    reg_write(BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL);
-    delay(1);
-    /* Wait for power-up to complete */
-    while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
+        /* Wait for power-up to complete */
+        while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
                                 BMI160_GYR_PMU_STATUS_BIT,
                                 BMI160_GYR_PMU_STATUS_LEN))
-        delay(1);
+            delay(1);
+
+        sensors_enabled |= GYRO;
+    }
 
-    setFullScaleGyroRange(BMI160_GYRO_RANGE_250);
-    setFullScaleAccelRange(BMI160_ACCEL_RANGE_2G);
+    setFullScaleGyroRange(BMI160_GYRO_RANGE_250, 250.0f);
+    setFullScaleAccelRange(BMI160_ACCEL_RANGE_2G, 2.0f);
 
     /* Only PIN1 interrupts currently supported - map all interrupts to PIN1 */
     reg_write(BMI160_RA_INT_MAP_0, 0xFF);
@@ -131,6 +148,17 @@ uint8_t BMI160Class::getDeviceID() {
     return reg_read(BMI160_RA_CHIP_ID);
 }
 
+/* Checks if the specified sensors are enabled (sensors are specified using
+ * bit flags defined by CurieIMUSensor enum). If 0 is passed, checks if *any*
+ * sensors are enabled */
+bool BMI160Class::isEnabled(unsigned int sensors)
+{
+    if (sensors == 0)
+        return sensors_enabled > 0;
+
+   return (sensors_enabled & sensors) > 0;
+}
+
 /** Verify the SPI connection.
  * Make sure the device is connected and responds as expected.
  * @return True if connection is valid, false otherwise
@@ -330,10 +358,11 @@ uint8_t BMI160Class::getFullScaleGyroRange() {
  * @param range New full-scale gyroscope range value
  * @see getFullScaleGyroRange()
  */
-void BMI160Class::setFullScaleGyroRange(uint8_t range) {
+void BMI160Class::setFullScaleGyroRange(uint8_t range, float real) {
     reg_write_bits(BMI160_RA_GYRO_RANGE, range,
                    BMI160_GYRO_RANGE_SEL_BIT,
                    BMI160_GYRO_RANGE_SEL_LEN);
+    gyro_range = real;
 }
 
 /** Get full-scale accelerometer range.
@@ -362,10 +391,11 @@ uint8_t BMI160Class::getFullScaleAccelRange() {
  * @see getFullScaleAccelRange()
  * @see BMI160AccelRange
  */
-void BMI160Class::setFullScaleAccelRange(uint8_t range) {
+void BMI160Class::setFullScaleAccelRange(uint8_t range, float real) {
     reg_write_bits(BMI160_RA_ACCEL_RANGE, range,
                    BMI160_ACCEL_RANGE_SEL_BIT,
                    BMI160_ACCEL_RANGE_SEL_LEN);
+    accel_range = real;
 }
 
 /** Get accelerometer offset compensation enabled value.
diff --git a/libraries/CurieIMU/src/BMI160.h b/libraries/CurieIMU/src/BMI160.h
index b452965a..dfdc7531 100644
--- a/libraries/CurieIMU/src/BMI160.h
+++ b/libraries/CurieIMU/src/BMI160.h
@@ -267,6 +267,12 @@ THE SOFTWARE.
 
 #define BMI160_RA_CMD               0x7E
 
+/* Bit flags for selecting individual sensors */
+typedef enum {
+    GYRO = 0x1,
+    ACCEL = 0x2
+} CurieIMUSensor;
+
 /**
  * Interrupt Latch Mode options
  * @see setInterruptLatch()
@@ -471,9 +477,10 @@ typedef enum {
 
 class BMI160Class {
     public:
-        void initialize();
+        void initialize(unsigned int flags);
         bool testConnection();
 
+        bool isEnabled(unsigned int sensors);
         uint8_t getGyroRate();
         void setGyroRate(uint8_t rate);
 
@@ -487,9 +494,9 @@ class BMI160Class {
         void setAccelDLPFMode(uint8_t bandwidth);
 
         uint8_t getFullScaleGyroRange();
-        void setFullScaleGyroRange(uint8_t range);
+        void setFullScaleGyroRange(uint8_t range, float real);
         uint8_t getFullScaleAccelRange();
-        void setFullScaleAccelRange(uint8_t range);
+        void setFullScaleAccelRange(uint8_t range, float real);
 
         void autoCalibrateGyroOffset();
         bool getGyroOffsetEnabled();
@@ -640,6 +647,7 @@ class BMI160Class {
 
         uint8_t getDeviceID();
 
+        int isBitSet(uint8_t value, unsigned bit);
         uint8_t getRegister(uint8_t reg);
         void setRegister(uint8_t reg, uint8_t data);
 
@@ -653,8 +661,13 @@ class BMI160Class {
         void setInterruptLatch(uint8_t latch);
         void resetInterrupt();
 
+        /* Use a bitmask to track which sensors are enabled */
+        unsigned sensors_enabled;
+        float accel_range;
+        float gyro_range;
+
     protected:
-        virtual int serial_buffer_transfer(uint8_t *buf, unsigned tx_cnt, unsigned rx_cnt);
+        virtual int serial_buffer_transfer(uint8_t *buf, unsigned tx_cnt, unsigned rx_cnt) = 0;
 
     private:
         uint8_t reg_read (uint8_t reg);
diff --git a/libraries/CurieIMU/src/CurieIMU.cpp b/libraries/CurieIMU/src/CurieIMU.cpp
index 94dc4f80..2b2d5466 100644
--- a/libraries/CurieIMU/src/CurieIMU.cpp
+++ b/libraries/CurieIMU/src/CurieIMU.cpp
@@ -32,7 +32,8 @@
  * on the Curie module, before calling BMI160::initialize() to activate the
  * BMI160 accelerometer and gyroscpoe with default settings.
  */
-bool CurieIMUClass::begin()
+
+bool CurieIMUClass::configure_imu(unsigned int sensors)
 {
     ss_spi_init(SPI_SENSING_1, 2000, SPI_BUSMODE_0, SPI_8_BIT, SPI_SE_1);
 
@@ -41,13 +42,26 @@ bool CurieIMUClass::begin()
     serial_buffer_transfer(&dummy_reg, 1, 1);
 
     /* The SPI interface is ready - now invoke the base class initialization */
-    BMI160Class::initialize();
+    initialize(sensors);
 
     /** Verify the SPI connection.
      * MakgetGyroRatee sure the device is connected and responds as expected.
      * @return True if connection is valid, false otherwise
      */
-    return (CURIE_IMU_CHIP_ID == getDeviceID());
+    if (CURIE_IMU_CHIP_ID != getDeviceID())
+        return false;
+
+    return true;
+}
+
+bool CurieIMUClass::begin()
+{
+    return configure_imu(GYRO | ACCEL);
+}
+
+bool CurieIMUClass::begin(unsigned int sensors)
+{
+    return configure_imu(sensors);
 }
 
 void CurieIMUClass::end()
@@ -55,6 +69,48 @@ void CurieIMUClass::end()
     ss_spi_disable(SPI_SENSING_1);
 }
 
+bool CurieIMUClass::dataReady()
+{
+    uint8_t stat;
+
+    /* If no sensors are enabled */
+    if (!isEnabled(0))
+        return false;
+
+    /* Read status register */
+    stat = getRegister(BMI160_RA_STATUS);
+
+    if (isEnabled(GYRO) && !isBitSet(stat, BMI160_STATUS_DRDY_GYR))
+        return false;
+
+    if (isEnabled(ACCEL) && !isBitSet(stat, BMI160_STATUS_DRDY_ACC))
+        return false;
+
+    return true;
+}
+
+bool CurieIMUClass::dataReady(unsigned int sensors)
+{
+    uint8_t stat;
+
+    /* If no sensors enabled, or no data requested */
+    if (sensors == 0 || !isEnabled(0))
+        return false;
+
+    /* Read status register */
+    stat = getRegister(BMI160_RA_STATUS);
+
+    if ((sensors & GYRO) && isEnabled(GYRO) &&
+        !isBitSet(stat, BMI160_STATUS_DRDY_GYR))
+        return false;
+
+    if ((sensors & ACCEL) && isEnabled(ACCEL) &&
+        !isBitSet(stat, BMI160_STATUS_DRDY_ACC))
+        return false;
+
+    return true;
+}
+
 int CurieIMUClass::getGyroRate()
 {
     int rate;
@@ -222,25 +278,26 @@ int CurieIMUClass::getGyroRange()
 void CurieIMUClass::setGyroRange(int range)
 {
     BMI160GyroRange bmiRange;
+    float real;
 
     if (range >= 2000) {
         bmiRange = BMI160_GYRO_RANGE_2000;
-        gyro_range = 2000.0f;
+        real = 2000.0f;
     } else if (range >= 1000) {
         bmiRange = BMI160_GYRO_RANGE_1000;
-        gyro_range = 1000.0f;
+        real = 1000.0f;
     } else if (range >= 500) {
         bmiRange = BMI160_GYRO_RANGE_500;
-        gyro_range = 500.0f;
+        real = 500.0f;
     } else if (range >= 250) {
         bmiRange = BMI160_GYRO_RANGE_250;
-        gyro_range = 250.0f;
+        real = 250.0f;
     } else {
         bmiRange = BMI160_GYRO_RANGE_125;
-        gyro_range = 125.0f;
+        real = 125.0f;
     }
 
-    setFullScaleGyroRange(bmiRange);
+    setFullScaleGyroRange(bmiRange, real);
 }
 
 int CurieIMUClass::getAccelerometerRange()
@@ -272,22 +329,23 @@ int CurieIMUClass::getAccelerometerRange()
 void CurieIMUClass::setAccelerometerRange(int range)
 {
     BMI160AccelRange bmiRange;
+    float real;
 
     if (range <= 2) {
         bmiRange = BMI160_ACCEL_RANGE_2G;
-        accel_range = 2.0f;
+        real = 2.0f;
     } else if (range <= 4) {
         bmiRange = BMI160_ACCEL_RANGE_4G;
-        accel_range = 4.0f;
+        real = 4.0f;
     } else if (range <= 8) {
         bmiRange = BMI160_ACCEL_RANGE_8G;
-        accel_range = 8.0f;
+        real = 8.0f;
     } else {
         bmiRange = BMI160_ACCEL_RANGE_16G;
-        accel_range = 16.0f;
+        real = 16.0f;
     }
 
-    setFullScaleAccelRange(bmiRange);
+    setFullScaleAccelRange(bmiRange, real);
 }
 
 void CurieIMUClass::autoCalibrateGyroOffset()
@@ -760,7 +818,7 @@ void CurieIMUClass::setTapDetectionThreshold(float threshold)
 
         case 16:
         default:
-            bmiThreshold = (threshold - 2500) / 500.0;
+            bmiThreshold = (threshold - 250) / 500.0;
             break;
     }
 
@@ -1778,20 +1836,10 @@ bool CurieIMUClass::stepsDetected()
 int CurieIMUClass::serial_buffer_transfer(uint8_t *buf, unsigned tx_cnt,
                                           unsigned rx_cnt)
 {
-    int flags, status;
-
     if (rx_cnt) /* For read transfers, assume 1st byte contains register address */
         buf[0] |= (1 << BMI160_SPI_READ_BIT);
 
-    /* Lock interrupts here to
-     * - avoid concurrent access to the SPI bus
-     * - avoid delays in SPI transfer due to unrelated interrupts
-     */
-    flags = interrupt_lock();
-    status = ss_spi_xfer(SPI_SENSING_1, buf, tx_cnt, rx_cnt);
-    interrupt_unlock(flags);
-
-    return status;
+    return ss_spi_xfer(SPI_SENSING_1, buf, tx_cnt, rx_cnt);
 }
 
 /** Interrupt handler for interrupts from PIN1 on the BMI160
diff --git a/libraries/CurieIMU/src/CurieIMU.h b/libraries/CurieIMU/src/CurieIMU.h
index 8056b135..3a7a0dc4 100644
--- a/libraries/CurieIMU/src/CurieIMU.h
+++ b/libraries/CurieIMU/src/CurieIMU.h
@@ -94,9 +94,13 @@ class CurieIMUClass : public BMI160Class {
     friend void bmi160_pin1_isr(void);
 
     public:
+        bool begin(unsigned int sensors);
         bool begin(void);
         void end(void);
 
+        bool dataReady();
+        bool dataReady(unsigned int sensors);
+
         // supported values: 25, 50, 100, 200, 400, 800, 1600, 3200 (Hz)
         int getGyroRate();
         void setGyroRate(int rate);
@@ -207,11 +211,9 @@ class CurieIMUClass : public BMI160Class {
         void detachInterrupt(void);
 
     private:
+        bool configure_imu(unsigned int sensors);
         int serial_buffer_transfer(uint8_t *buf, unsigned tx_cnt, unsigned rx_cnt);
 
-        float accel_range;
-        float gyro_range;
-
         float getFreefallDetectionThreshold();
         void setFreefallDetectionThreshold(float threshold);
         float getShockDetectionThreshold();
diff --git a/libraries/CuriePowerManagement/README.md b/libraries/CuriePowerManagement/README.md
new file mode 100644
index 00000000..8b472632
--- /dev/null
+++ b/libraries/CuriePowerManagement/README.md
@@ -0,0 +1,296 @@
+Table of Contents
+=================
+
+   * [CuriePower](#curiepower)
+   * [CuriePower API reference](#curiepower-api-reference)
+      * [Functions](#functions)
+         * [CuriePower.doze()](#curiepowerdoze)
+         * [CuriePower.doze(int duration)](#curiepowerdozeint-duration)
+         * [CuriePower.idle()](#curiepoweridle)
+         * [CuriePower.idle(int duration)](#curiepoweridleint-duration)
+         * [CuriePower.sleep()](#curiepowersleep)
+         * [CuriePower.sleep(int duration)](#curiepowersleepint-duration)
+         * [CuriePower.deepSleep()](#curiepowerdeepsleep)
+         * [CuriePower.deepSleep(int duration)](#curiepowerdeepsleepint-duration)
+         * [CuriePower.attachInterruptWakeup(uint32_t pin, voidFuncPtr callback, uint32_t mode)](#curiepowerattachinterruptwakeupuint32_t-pin-voidfuncptr-callback-uint32_t-mode)
+         * [CuriePower.detachInterruptWakeup(uint32_t pin)](#curiepowerdetachinterruptwakeupuint32_t-pin)
+   * [Tutorials](#tutorials)
+      * [Tutorial #1: TimedWakeup Example](#tutorial-1-timedwakeup-example)
+      * [Tutorial #2: WakeFromIMU Example](#tutorial-2-wakefromimu-example)
+
+# CuriePower
+CuriePower is a Power Management library for Curie based boards such as the Arduino101/Genuino101 and tinyTILE
+
+# CuriePower API reference
+
+## Functions
+
+### ``CuriePower.doze()``
+
+```
+void CuriePower.doze()
+```
+
+Places the SoC in "doze" mode which switches the system clock to the internal 32.768 kHz RTC oscillator.
+
+*Parameters*
+
+none
+
+*Return value*
+
+none
+
+### ``CuriePower.doze(int duration)``
+
+```
+void CuriePower.doze(int duration)
+```
+
+
+Places the SoC in "doze" mode which switches the system clock to the internal 32.768 kHz RTC oscillator for `duration` milliseconds
+
+
+*Parameters*
+
+1. `int duration` : number in milliseconds to doze
+
+*Return value*
+
+none
+
+### ``CuriePower.idle()``
+
+```
+void CuriePower.idle()
+```
+
+Places the SoC into "doze" mode then enters an infinite loop, effectively stopping all operations until a wake interrupt is generated.
+
+*Parameters*
+
+none
+
+*Return value*
+
+none
+
+### ``CuriePower.idle(int duration)``
+
+```
+void CuriePower.idle(int duration)
+```
+
+
+Places the SoC in "doze" mode and enters an infinite loop for `duration` milliseconds
+
+
+*Parameters*
+
+1. `int duration` : number in milliseconds to idle
+
+*Return value*
+
+none
+
+### ``CuriePower.sleep()``
+
+```
+void CuriePower.sleep()
+```
+
+Places the SoC into a sleep state, stopping all operations, until a wake interrupt is generated
+
+*Parameters*
+
+none
+
+*Return value*
+
+none
+
+### ``CuriePower.sleep(int duration)``
+
+```
+void CuriePower.sleep(int duration)
+```
+
+
+Places the SoC into a sleep state for `duration` milliseconds
+
+
+*Parameters*
+
+1. `int duration` : number in milliseconds to sleep
+
+*Return value*
+
+none
+
+### ``CuriePower.deepSleep()``
+
+```
+void CuriePower.deepSleep()
+```
+
+Places the SoC into a deep sleep state, stopping all operations, until a wake interrupt is generated
+
+*Parameters*
+
+none
+
+*Return value*
+
+none
+
+### ``CuriePower.deepSleep(int duration)``
+
+```
+void CuriePower.deepSleep(int duration)
+```
+
+
+Places the SoC into a deep sleep state for `duration` milliseconds
+
+
+*Parameters*
+
+1. `int duration` : number in milliseconds to deep sleep
+
+*Return value*
+
+none
+
+### ``CuriePower.attachInterruptWakeup(uint32_t pin, voidFuncPtr callback, uint32_t mode)``
+
+```
+void CuriePower.attachInterruptWakeup(uint32_t pin, voidFuncPtr callback, uint32_t mode)
+```
+
+Attaches a wakeup interrupt to digital pin `pin`. `callback` is the function to be called when the wakeup interrupt occurs. 
+
+*Parameters*
+
+none
+
+*Return value*
+
+none
+
+### ``CuriePower.detachInterruptWakeup(uint32_t pin)``
+
+```
+void CuriePower.detachInterruptWakeup(uint32_t pin)
+```
+
+Removes any wakeup interrupt attached to digital pin `pin`. 
+
+*Parameters*
+
+none
+
+*Return value*
+
+none
+
+# Tutorials
+
+## Tutorial #1: TimedWakeup Example
+
+This sketch demonstrates the simplest way to use the CuriePower library. It blinks the LED a few times, goes to sleep for a certain amount of time then goes back at the start of loop()
+
+```cpp
+#include <Power.h>
+
+void setup() {
+  pinMode(LED_BUILTIN, OUTPUT);
+}
+
+void loop() {
+  for(int i = 0; i < 5; i++)
+  { 
+    digitalWrite(LED_BUILTIN, HIGH);
+    delay(100);
+    digitalWrite(LED_BUILTIN, LOW);
+    delay(100);
+  }
+  PM.sleep(1000);
+}
+```
+
+The line of code that is most interesting here is:
+```cpp
+PM.sleep(1000);
+```
+This puts the SoC into sleep, drawing significantly less power, for 1000ms or 1s.
+
+In what situations is this most useful?
+
+Let's says you have a battery powered project that reads a sensor value once every second and saves it to an SD card.
+Simply using delay() will work but you will notice that you will run out of battery pretty fast. That is because even though the code is not doing much inside delay, it is still running everything at full clock speed and all peripherals are turned on.
+When we put the SoC to sleep, several things are turned off. This includes most of the peripherals. voltage rails, and some clocks. Basically, it draws much less power and no code is running until a wake interrupt is generated.
+
+Many Arduino projects typically have a loop where you read a sensor, do something with that reading, and then delay for a set amount of time.
+In most cases, reading the sensor and doing something with that reading takes very little time, much smaller than the delay duration. This means that we are wasting a lot of power inside the delay doing nothing.
+By placing the SoC to sleep instead of just waiting inside the delay, we can save a considerable amount of power in most applications.
+
+
+## Tutorial #2: WakeFromIMU Example
+
+This sketch uses CuriePower library and the CurieIMU library together and demonstrates the use of an interrupt to wake the Curie SoC from sleep.
+
+```cpp
+#include <Power.h>
+#include "CurieIMU.h"
+
+
+void setup() {
+  pinMode(LED_BUILTIN, OUTPUT);
+  CurieIMU.begin();
+  CurieIMU.attachInterrupt(wakeup);
+  CurieIMU.setDetectionThreshold(CURIE_IMU_MOTION, 20);      // 100mg
+  CurieIMU.setDetectionDuration(CURIE_IMU_MOTION, 10);       // trigger times of consecutive slope data points
+  CurieIMU.interrupts(CURIE_IMU_MOTION);
+  
+}
+
+void loop() {
+  PM.sleep();
+  digitalWrite(LED_BUILTIN, HIGH);
+  delay(3000);
+  digitalWrite(LED_BUILTIN, LOW);
+}
+
+void wakeup()
+{
+  PM.wakeFromDoze();
+  // This function will be called once on device wakeup
+  // You can do some little operations here (like changing variables which will be used in the loop)
+  // Remember to avoid calling delay() and long running functions since this functions executes in interrupt context
+}
+```
+
+If you look at the beginning of loop() you will see:
+```cpp
+PM.sleep();
+```
+This line of code puts the SoC into a sleep state. The SoC will remain in a sleep state until it is woken by an interrupt.
+If no interrupt triggers the SoC to wake up, it will stay in a sleep state forever(or until the battery runs out).
+This is specifically useful in applications that are not periodic, like in this example sketch where the SoC stays at a sleep state until the Arduino101 detects motion, or more precisely the Bosch BMI160 [6-Axis Accelerometer/Gyroscope] sensor detects the motion and triggers an interrupt to wake the Curie Soc from sleep.
+
+Inside setup() we have:
+```cpp
+CurieIMU.attachInterrupt(wakeup);
+CurieIMU.setDetectionThreshold(CURIE_IMU_MOTION, 20);      // 100mg
+CurieIMU.setDetectionDuration(CURIE_IMU_MOTION, 10);       // trigger times of consecutive slope data points
+CurieIMU.interrupts(CURIE_IMU_MOTION);
+```
+These lines of code attaches a method called wakeup() which is called whenever motion is detected. Since the interrupt signal generated by the Bosch BMI160 sensor is already internally connected to AON(Always On) interrupt of the SoC, it automatically wakes the SoC from sleep. 
+However, since we are using the attachInterrupt() method of the CurieIMU Library instead of the one from the CuriePower Library we still need to do one more thing after the SoC is taken out of a sleep state.
+
+Inside the wakeup() method:
+```cpp
+PM.wakeFromDoze();
+```
+This line of code simply takes the SoC out of the Doze state, switching it from using the internal RTC 32.768 KHz as the main clock, back to the 32Mhz oscillator.
+At this point the SoC runs back at full speed ready to do stuff quickly and then go back to sleep to save power.
diff --git a/libraries/CuriePowerManagement/examples/TimedWakeup/TimedWakeup.ino b/libraries/CuriePowerManagement/examples/TimedWakeup/TimedWakeup.ino
new file mode 100644
index 00000000..baeb4dec
--- /dev/null
+++ b/libraries/CuriePowerManagement/examples/TimedWakeup/TimedWakeup.ino
@@ -0,0 +1,16 @@
+#include <Power.h>
+
+void setup() {
+  pinMode(LED_BUILTIN, OUTPUT);
+}
+
+void loop() {
+  for(int i = 0; i < 5; i++)
+  { 
+    digitalWrite(LED_BUILTIN, HIGH);
+    delay(100);
+    digitalWrite(LED_BUILTIN, LOW);
+    delay(100);
+  }
+  PM.sleep(1000);
+}
\ No newline at end of file
diff --git a/libraries/CuriePowerManagement/examples/WakeFromIMU/WakeFromIMU.ino b/libraries/CuriePowerManagement/examples/WakeFromIMU/WakeFromIMU.ino
new file mode 100644
index 00000000..7cf2015c
--- /dev/null
+++ b/libraries/CuriePowerManagement/examples/WakeFromIMU/WakeFromIMU.ino
@@ -0,0 +1,28 @@
+#include <Power.h>
+#include "CurieIMU.h"
+
+
+void setup() {
+  pinMode(LED_BUILTIN, OUTPUT);
+  CurieIMU.begin();
+  CurieIMU.attachInterrupt(wakeup);
+  CurieIMU.setDetectionThreshold(CURIE_IMU_MOTION, 20);      // 100mg
+  CurieIMU.setDetectionDuration(CURIE_IMU_MOTION, 10);       // trigger times of consecutive slope data points
+  CurieIMU.interrupts(CURIE_IMU_MOTION);
+  
+}
+
+void loop() {
+  PM.sleep();
+  digitalWrite(LED_BUILTIN, HIGH);
+  delay(3000);
+  digitalWrite(LED_BUILTIN, LOW);
+}
+
+void wakeup()
+{
+  PM.wakeFromDoze();
+  // This function will be called once on device wakeup
+  // You can do some little operations here (like changing variables which will be used in the loop)
+  // Remember to avoid calling delay() and long running functions since this functions executes in interrupt context
+}
diff --git a/libraries/CuriePowerManagement/keywords.txt b/libraries/CuriePowerManagement/keywords.txt
new file mode 100644
index 00000000..7051dfac
--- /dev/null
+++ b/libraries/CuriePowerManagement/keywords.txt
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map For Curie Power Library
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+sleep	KEYWORD2
+deepSleep	KEYWORD2
+idle	KEYWORD2
+doze	KEYWORD2
+wakeFromDoze	KEYWORD2
+
+#######################################
+# Instances (KEYWORD2)
+#######################################
+
+#######################################
+# Constants (LITERAL1)
+#######################################
diff --git a/libraries/CuriePowerManagement/library.properties b/libraries/CuriePowerManagement/library.properties
new file mode 100644
index 00000000..83a2661e
--- /dev/null
+++ b/libraries/CuriePowerManagement/library.properties
@@ -0,0 +1,9 @@
+name=CuriePowerManagement
+version=1.0
+author=Intel
+maintainer=Intel <dino.tinitigan@intel.com>
+sentence=Curie Power Management library for Curie based boards.
+paragraph=Allows to manage the power states of Curie based boards.
+category=Device Control
+url=
+architectures=arc32
diff --git a/libraries/CuriePowerManagement/src/Power.cpp b/libraries/CuriePowerManagement/src/Power.cpp
new file mode 100644
index 00000000..450c2ad7
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/Power.cpp
@@ -0,0 +1,478 @@
+#include "Power.h"
+
+Power PM;
+uint32_t arc_restore_addr;
+uint32_t cpu_context[33];
+
+typedef void (*user_cb)(void);
+
+static void sleepInterruptHandler(void)
+{
+    unsigned int flags = interrupt_lock();
+    PM.wakeFromDoze();
+    PM.wakeFromSleepCallback();
+    interrupt_unlock(flags);
+}
+
+static void dozeInterruptHandler(void)
+{
+    unsigned int flags = interrupt_lock();
+    PM.wakeFromDoze();
+    interrupt_unlock(flags);
+}
+
+static void soc_gpio_sleep_isr()
+{
+    if(soc_sleeping || soc_dozing)
+    {
+        unsigned int flags = interrupt_lock();
+        PM.wakeFromDoze();
+        wsrc_t wsrc;
+        while (wsrc_get_newest_attached(&wsrc)){
+            PinDescription *p = &g_APinDescription[wsrc.id];
+            uint32_t mask = 0x1 << p->ulGPIOId;
+            if(p->ulGPIOPort == SOC_GPIO_32){
+                uint32_t status = shared_data->pm_int_status;
+                if((status>>p->ulGPIOId)&0x1){
+                    //call user callback
+                    user_cb cb = (user_cb)wsrc.callback;
+                    cb();
+                }
+            }
+        }
+        interrupt_unlock(flags);
+    }
+    soc_sleeping = false;
+}
+
+static void soc_aongpio_sleep_isr()
+{
+    if(soc_sleeping || soc_dozing){
+        unsigned int flags = interrupt_lock();
+        PM.wakeFromDoze();
+        wsrc_t wsrc;
+        while (wsrc_get_newest_attached(&wsrc)){
+            PinDescription *p = &g_APinDescription[wsrc.id];
+            uint32_t mask = 0x1 << p->ulGPIOId;
+            if(p->ulGPIOPort == SOC_GPIO_32){
+                // Save interrupt status
+                uint32_t status = MMIO_REG_VAL_FROM_BASE(SOC_GPIO_AON_BASE_ADDR, SOC_GPIO_INTSTATUS);
+                // Mask the pending interrupts
+                MMIO_REG_VAL_FROM_BASE(SOC_GPIO_AON_BASE_ADDR, SOC_GPIO_INTMASK) |= status;
+                shared_data->pm_int_status = status;
+                // Clear interrupt flag (write 1 to clear)
+                MMIO_REG_VAL_FROM_BASE(SOC_GPIO_AON_BASE_ADDR, SOC_GPIO_PORTA_EOI) = status;
+                if((status>>p->ulGPIOId)&0x1){
+                    //call user callback
+                    user_cb cb = (user_cb)wsrc.callback;
+                    cb();
+                }
+            }
+        }
+        interrupt_unlock(flags);
+    }
+    soc_sleeping = false;
+}
+
+static void ss_gpio0_sleep_isr()
+{
+    if(soc_sleeping || soc_dozing){
+        unsigned int flags = interrupt_lock();
+        PM.wakeFromDoze();
+        wsrc_t wsrc;
+        while (wsrc_get_newest_attached(&wsrc)) {
+            PinDescription *p = &g_APinDescription[wsrc.id];
+            uint32_t mask = 0x1 << p->ulGPIOId;
+            if(p->ulGPIOPort == SS_GPIO_8B0){
+                uint32_t status = shared_data->pm_int_status;
+                if((status>>p->ulGPIOId)&0x1){
+                    //call user callback
+                    user_cb cb = (user_cb)wsrc.callback;
+                    cb();
+                }
+            }
+        }
+        interrupt_unlock(flags);
+    }
+    soc_sleeping = false;
+}
+
+static void ss_gpio1_sleep_isr()
+{
+    if(soc_sleeping || soc_dozing){
+        unsigned int flags = interrupt_lock();
+        PM.wakeFromDoze();
+        wsrc_t wsrc;
+        while (wsrc_get_newest_attached(&wsrc)) {
+            PinDescription *p = &g_APinDescription[wsrc.id];
+            uint32_t mask = 0x1 << p->ulGPIOId;
+            if(p->ulGPIOPort == SS_GPIO_8B1){
+                uint32_t status = shared_data->pm_int_status;
+                if((status>>p->ulGPIOId)&0x1){
+                    //call user callback
+                    user_cb cb = (user_cb)wsrc.callback;
+                    cb();
+                }
+            }
+        }
+        interrupt_unlock(flags);
+    }
+    soc_sleeping = false;
+}
+
+static void aontimer_isr()
+{
+    if(soc_sleeping || soc_dozing){
+        unsigned int flags = interrupt_lock();
+        interrupt_disable(IRQ_ALWAYS_ON_TMR);
+        *AONPT_CFG = 0;
+        PM.wakeFromDoze();
+        interrupt_unlock(flags);
+        //wait until quark core is running
+        volatile uint32_t psts = (MMIO_REG_VAL(P_STS))&0x7;
+        while(psts){
+          psts = (MMIO_REG_VAL(P_STS))&0x7;
+        }
+        PM.wakeFromSleepCallback();
+    }
+    soc_sleeping = false;
+}
+
+Power::Power()
+{
+    
+}
+
+void Power::doze()
+{
+    //actually attach the interrupts
+    enableWakeInterrupts();
+    turnOffUSB();
+    soc_dozing = true;
+    //switch from external crystal oscillator to internal hybrid oscillator
+    switchToHybridOscillator();
+
+    //Set system clock to the RTC Crystal Oscillator
+    uint32_t current_val = MMIO_REG_VAL(CCU_SYS_CLK_CTL);
+    MMIO_REG_VAL(CCU_SYS_CLK_CTL) = current_val & 0xFFFFFFFE;
+
+    //Powerdown hybrid oscillator
+    current_val = MMIO_REG_VAL(OSC0_CFG1);
+    MMIO_REG_VAL(OSC0_CFG1) = current_val | 0x00000004; 
+}
+
+void Power::doze(int duration)
+{
+    doze();
+    delayTicks(millisToRTCTicks(duration));
+    wakeFromDoze();
+}
+
+void Power::idle()
+{
+    doze();
+    while(soc_dozing);
+}
+
+void Power::idle(int duration)
+{
+    enableAONPTimerInterrrupt(duration);
+    idle();
+}
+
+void Power::wakeFromDoze()
+{
+    //Powerup hybrid oscillator
+    uint32_t current_val = MMIO_REG_VAL(OSC0_CFG1);
+    MMIO_REG_VAL(OSC0_CFG1) = current_val & 0xFFFFFFFB;
+   
+    //Set system clock to the Hybrid Oscillator
+    current_val = MMIO_REG_VAL(CCU_SYS_CLK_CTL);
+    MMIO_REG_VAL(CCU_SYS_CLK_CTL) = current_val | 0x00000001;
+
+    //switch back to the external crystal oscillator
+    void switchToCrystalOscillator();
+  
+    turnOnUSB();
+    soc_dozing = false;
+}
+
+void Power::sleep()
+{
+    soc_sleeping = true;
+    //disable low power mode on OPM_2P6 regulator
+    MMIO_REG_VAL(SLP_CFG) &= ~(1 << LPMODE_EN);
+    
+    uint32_t creg_mst0_ctrl = 0;
+    creg_mst0_ctrl = READ_ARC_REG(QM_SS_CREG_BASE);
+    
+    /*
+	  * Clock gate the sensor peripherals at CREG level.
+	  * This clock gating is independent of the peripheral-specific clock
+	  * gating provided in ss_clk.h .
+    */
+    creg_mst0_ctrl |= (QM_SS_IO_CREG_MST0_CTRL_ADC_CLK_GATE |
+        QM_SS_IO_CREG_MST0_CTRL_I2C1_CLK_GATE |
+	    QM_SS_IO_CREG_MST0_CTRL_I2C0_CLK_GATE |
+	    QM_SS_IO_CREG_MST0_CTRL_SPI1_CLK_GATE |
+	    QM_SS_IO_CREG_MST0_CTRL_SPI0_CLK_GATE);
+    
+    WRITE_ARC_REG(creg_mst0_ctrl, QM_SS_CREG_BASE);
+    
+    //Send request to put quark core to sleep
+    //x86_C2LPRequest(); //can only wake Quark core using AON_GPIO, RTC, AON_Timer, and AON_Comparators
+    x86_C2Request();
+    doze();
+   
+    __asm__ __volatile__(
+		"sleep %0"
+		:
+		: "i"(QM_SS_SLEEP_MODE_CORE_TIMERS_RTC_OFF));
+    
+    creg_mst0_ctrl &= ~(QM_SS_IO_CREG_MST0_CTRL_ADC_CLK_GATE |
+		QM_SS_IO_CREG_MST0_CTRL_I2C1_CLK_GATE |
+		QM_SS_IO_CREG_MST0_CTRL_I2C0_CLK_GATE |
+		QM_SS_IO_CREG_MST0_CTRL_SPI1_CLK_GATE |
+		QM_SS_IO_CREG_MST0_CTRL_SPI0_CLK_GATE);
+                
+    WRITE_ARC_REG(creg_mst0_ctrl, QM_SS_CREG_BASE);
+    soc_sleeping = false;
+}
+
+void Power::sleep(int duration)
+{
+    enableAONPTimerInterrrupt(duration);
+    sleep();
+}
+
+void Power::deepSleep()
+{
+   sleep();
+}
+
+void Power::deepSleep(int duration)
+{
+    sleep(duration);
+}
+
+inline void Power::wakeFromSleepCallback(void)
+{
+    //ToDo: check table and call apprpriate CBs
+    if(pmCB != NULL)
+        pmCB();
+    //ToDo: unregister all sleep IRQs
+}
+
+inline void Power::wakeFromDozeCallback(void)
+{
+    //ToDo: check table and call apprpriate CBs
+    if(pmCB != NULL)
+        pmCB();
+}
+
+void Power::attachInterruptWakeup(uint32_t pin, voidFuncPtr callback, uint32_t mode)
+{
+    if (pin > NUM_WAKEUP) {
+        return;
+    }
+
+    if (pin <= GPIO_END) {
+        wsrc_register_gpio(pin, callback, mode);
+    }
+    else{
+        wsrc_register_id(pin, callback);
+    }
+}
+
+void Power::detachInterruptWakeup(uint32_t pin)
+{
+    wsrc_unregister(pin);
+    if (pin <= GPIO_END) {
+        detachInterrupt(pin);
+    }
+}
+
+//Privates
+
+void Power::turnOffUSB()
+{
+    MMIO_REG_VAL(USB_PHY_CFG0) |= 0x00000001; 
+}
+
+void Power::turnOnUSB()
+{
+    MMIO_REG_VAL(USB_PHY_CFG0) &= 0xFFFFFFFE;
+}
+
+void Power::switchToHybridOscillator()
+{
+    //read trim value from OTP
+    uint32_t trimMask = *(uint16_t*)OSCTRIM_ADDR << 20;
+    MMIO_REG_VAL(OSC0_CFG1) = 0x00000002 | trimMask;  //switch to internal hybrid oscillator using trim value from OTP
+    //ToDo: wait for hybrid oscillator to stabilize
+}
+
+void Power::switchToCrystalOscillator()
+{
+    MMIO_REG_VAL(OSC0_CFG1) = 0x00070009;
+    while(!(MMIO_REG_VAL(OSC0_STAT) & 0x00000002));   //wait till crystal oscillator is stable
+}
+
+void Power::setRTCCMR(int seconds)
+{
+    MMIO_REG_VAL(RTC_CMR) = readRTC_CCVR() + seconds;
+}
+
+uint32_t Power::readRTC_CCVR()
+{
+    return *RTC_CCVR;
+}
+
+uint32_t Power::millisToRTCTicks(int milliseconds)
+{
+    return (uint32_t)((double)milliseconds*32.768);
+}
+
+void Power::enableRTCInterrupt(int seconds)
+{
+    setRTCCMR(seconds);
+    MMIO_REG_VAL(RTC_MASK_INT) &= 0xFFFFFEFE;
+    MMIO_REG_VAL(RTC_CCR) |= 0x00000001;
+    MMIO_REG_VAL(RTC_CCR) &= 0xFFFFFFFD;
+    volatile uint32_t read = MMIO_REG_VAL(RTC_EOI);
+    
+    pmCB = &wakeFromRTC;
+    interrupt_disable(IRQ_RTC_INTR);
+    interrupt_connect(IRQ_RTC_INTR , &sleepInterruptHandler);
+    delayTicks(6400);   //2ms
+    interrupt_enable(IRQ_RTC_INTR);
+}
+
+void Power::enableAONGPIOInterrupt(int aon_gpio, int mode)
+{
+    switch(mode){
+        case CHANGE:    //not supported just do the same as FALLING
+            MMIO_REG_VAL(AON_GPIO_INTTYPE_LEVEL) |= 1 << aon_gpio;
+            MMIO_REG_VAL(AON_GPIO_INT_POL) &= ~(1 << aon_gpio);
+            break;
+        case RISING:
+            MMIO_REG_VAL(AON_GPIO_INTTYPE_LEVEL) |= 1 << aon_gpio;
+            MMIO_REG_VAL(AON_GPIO_INT_POL) |= 1 << aon_gpio;
+            break;
+        case FALLING:
+            MMIO_REG_VAL(AON_GPIO_INTTYPE_LEVEL) |= 1 << aon_gpio;
+            MMIO_REG_VAL(AON_GPIO_INT_POL) &= ~(1 << aon_gpio);
+            break;
+        case HIGH:
+            MMIO_REG_VAL(AON_GPIO_INTTYPE_LEVEL) &= ~(1 << aon_gpio);
+            MMIO_REG_VAL(AON_GPIO_INT_POL) |= 1 << aon_gpio;
+            break;
+        case LOW:
+            MMIO_REG_VAL(AON_GPIO_INTTYPE_LEVEL) &= ~(1 << aon_gpio);
+            MMIO_REG_VAL(AON_GPIO_INT_POL) &= ~(1 << aon_gpio);
+            break;
+        default:
+            MMIO_REG_VAL(AON_GPIO_INTTYPE_LEVEL) &= ~(1 << aon_gpio);
+            MMIO_REG_VAL(AON_GPIO_INT_POL) &= ~(1 << aon_gpio);
+            break;
+    };
+    
+    MMIO_REG_VAL(AON_GPIO_SWPORTA_DDR) &= ~(1 << aon_gpio);
+    MMIO_REG_VAL(AON_GPIO_INTMASK) &= ~(1 << aon_gpio);
+    MMIO_REG_VAL(AON_GPIO_INTEN) |= 1 << aon_gpio;
+    
+    *AON_GPIO_MASK_INT &= 0xFFFFFEFE;
+    interrupt_disable(IRQ_ALWAYS_ON_GPIO);
+    interrupt_connect(IRQ_ALWAYS_ON_GPIO , &soc_aongpio_sleep_isr);
+    interrupt_enable(IRQ_ALWAYS_ON_GPIO);
+}
+
+void Power::enableAONPTimerInterrrupt(int millis)
+{
+    WRITE_ARC_REG(QM_IRQ_AONPT_0_INT_VECTOR, QM_SS_AUX_IRQ_SELECT);
+	WRITE_ARC_REG(QM_SS_IRQ_LEVEL_SENSITIVE, QM_SS_AUX_IRQ_TRIGGER);
+    
+    interrupt_disable(IRQ_ALWAYS_ON_TMR);
+    pmCB = resetAONPTimer;
+    *AONPT_CFG = millisToRTCTicks(millis);
+    interrupt_connect(IRQ_ALWAYS_ON_TMR , &aontimer_isr);
+    *AON_TIMER_MASK_INT &= 0xFFFFFEFE;
+    *AONPT_CTRL |= 0x00000002;
+    *AONPT_CTRL |= 0x00000001;  
+    volatile uint32_t aonpt_stat = *AONPT_STAT;
+    while(aonpt_stat){
+       *AONPT_CTRL &= 0xFFFFFFFE;
+       *AONPT_CTRL |= 0x00000001;
+       aonpt_stat = *AONPT_STAT;
+       //delayTicks(1000); 
+    }
+    interrupt_enable(IRQ_ALWAYS_ON_TMR);
+}
+
+void Power::enableWakeInterrupts()
+{
+    void (*fptr)(void);
+    wsrc_t wsrc;
+ 
+    while (wsrc_get_newest_attached(&wsrc)) {
+        switch(wsrc.irq){
+        case IRQ_ALWAYS_ON_GPIO:
+            enableAONGPIOInterrupt((wsrc.id-GPIO_END), wsrc_gpio_mode(wsrc.status));
+            break;
+        case IRQ_ALWAYS_ON_TMR:
+            break;
+        case IRQ_RTC_INTR:
+            break;
+        case IRQ_GPIO0_INTR:
+            attachInterrupt(wsrc.id, &ss_gpio0_sleep_isr, wsrc_gpio_mode(wsrc.status));
+            interrupt_enable(wsrc.irq);
+            break;
+        case IRQ_GPIO1_INTR:
+            attachInterrupt(wsrc.id, &ss_gpio1_sleep_isr, wsrc_gpio_mode(wsrc.status));
+            interrupt_enable(wsrc.irq);
+            break;
+        case IRQ_GPIO_INTR:
+            attachInterrupt(wsrc.id, &soc_gpio_sleep_isr, wsrc_gpio_mode(wsrc.status));
+            interrupt_enable(wsrc.irq);
+            break;
+        case IRQ_TIMER1:
+            break;
+        default:
+            break;
+        }
+    }   
+}
+
+void Power::resetAONPTimer()
+{
+    interrupt_disable(IRQ_ALWAYS_ON_TMR);
+    *AON_TIMER_MASK_INT |= 0xFFFFF1F1;
+    *AONPT_CFG = 0;
+    *AONPT_CTRL |= 0x00000001;
+    WRITE_ARC_REG(QM_IRQ_AONPT_0_INT_VECTOR, QM_SS_AUX_IRQ_SELECT);
+	WRITE_ARC_REG(QM_SS_IRQ_EDGE_SENSITIVE, QM_SS_AUX_IRQ_TRIGGER);
+}
+
+void Power::wakeFromRTC()
+{
+    MMIO_REG_VAL(RTC_MASK_INT) |= 0x00000101;
+    interrupt_disable(IRQ_RTC_INTR);
+    volatile uint32_t read = MMIO_REG_VAL(RTC_EOI);
+}
+
+void Power::x86_C2Request()
+{
+    switchToHybridOscillator();
+    //request for the x86 core go into C2 sleep
+    MMIO_REG_VAL(CCU_LP_CLK_CTL) &= 0xFFFFFFFC;
+    volatile uint32_t c2 = MMIO_REG_VAL(P_LVL2);
+}
+
+void Power::x86_C2LPRequest()
+{
+    switchToHybridOscillator();
+    //request for the x86 core go into C2LP sleep
+    MMIO_REG_VAL(CCU_LP_CLK_CTL) &= 0xFFFFFFFE;
+    MMIO_REG_VAL(CCU_LP_CLK_CTL) |= 0x00000002;
+    volatile uint32_t c2lp = MMIO_REG_VAL(P_LVL2);
+}
diff --git a/libraries/CuriePowerManagement/src/Power.h b/libraries/CuriePowerManagement/src/Power.h
new file mode 100644
index 00000000..dfd919b6
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/Power.h
@@ -0,0 +1,133 @@
+#define OSC0_STAT       0xB0800004
+#define OSC0_CFG1       0xB0800008
+
+#define CCU_SS_PERIPH_CLK_GATE_CTL  0xB0800028
+#define CCU_LP_CLK_CTL  0xB080002C
+#define CCU_SYS_CLK_CTL 0xB0800038
+#define P_LVL2          0xB0800504
+#define PM1C            0xB0800518
+#define SLP_CFG         0xB0800550
+#define SS_STS          0xB0800604
+
+#define AONC_CNT        0xB0800700
+#define AONC_CFG        0xB0800704
+#define AONPT_CNT       0xB0800708
+#define AONPT_STAT      (volatile int*)0xB080070C
+#define AONPT_CTRL      (volatile int*)0xB0800710
+#define AONPT_CFG       (volatile int*)0xB0800714
+
+#define USB_PLL_CFG0    0xB0800014
+#define USB_PHY_CFG0    0xB0800800
+
+#define RTC_CCVR        (volatile int*)0xB0000400 // Current Counter Value Register
+#define RTC_CMR         0xB0000404
+#define RTC_CCR         0xB000040C
+#define RTC_EOI         0xB0000418
+
+#define RTC_MASK_INT            0xB0800478
+#define AON_TIMER_MASK_INT      (volatile int*)0xB08004C8
+#define AON_GPIO_MASK_INT       (volatile int*)0xB08004D4
+
+#define AON_GPIO_SWPORTA_DR         0xB0800B00
+#define AON_GPIO_SWPORTA_DDR        0xB0800B04
+#define AON_GPIO_SWPORTA_CTL        0xB0800B08
+#define AON_GPIO_INTEN              0xB0800B30
+#define AON_GPIO_INTMASK            0xB0800B34
+#define AON_GPIO_INTTYPE_LEVEL      0xB0800B38
+#define AON_GPIO_INT_POL            0xB0800B3C
+#define AON_GPIO_DEBOUNCE           0xB0888B48
+#define AON_GPIO_PORTA_EOI          0xB0800B4C
+
+#define OSCTRIM_ADDR    0xffffe1f8
+
+#define QM_SS_SLEEP_MODE_CORE_OFF (0x0)
+#define QM_SS_SLEEP_MODE_CORE_OFF_TIMER_OFF (0x20)
+#define QM_SS_SLEEP_MODE_CORE_TIMERS_RTC_OFF (0x60)
+
+#define P_STS   0xB0800560
+
+#define LPMODE_EN   8
+
+#include <Arduino.h>
+#include <stdint.h>
+#include <interrupt.h>
+#include <board.h>
+#include <portable.h>
+#include "qmsi/qm_sensor_regs.h"
+#include "qmsi/ss_power_states.h"
+#include "wsrc.h"
+
+static volatile bool soc_sleeping = false;
+static volatile bool soc_dozing = false;
+
+class Power
+{
+    public:
+        Power();
+
+        //puts the SoC into "doze" mode which lowers the system clock speed to 32k
+        void doze();
+
+        void doze(int duration);
+        
+        void idle();
+        
+        void idle(int duration);
+
+        void wakeFromDoze();
+
+        void sleep();
+
+        void sleep(int duration);
+
+        void deepSleep();
+
+        void deepSleep(int duration);
+
+        void wakeFromSleepCallback(void);
+        
+        void wakeFromDozeCallback(void);
+
+        void attachInterruptWakeup(uint32_t pin, voidFuncPtr callback, uint32_t mode);
+
+        void detachInterruptWakeup(uint32_t pin);
+
+        uint32_t arc_restore_addr;
+
+    private:
+        void turnOffUSB();
+
+        void turnOnUSB();
+
+        void switchToHybridOscillator();
+
+        void switchToCrystalOscillator();
+
+        void setRTCCMR(int seconds);
+
+        uint32_t readRTC_CCVR();
+
+        bool isSleeping = false;
+
+        uint32_t millisToRTCTicks(int milliseconds);
+
+        void enableRTCInterrupt(int seconds);
+
+        void enableAONGPIOInterrupt(int aon_gpio, int mode);
+
+        void enableAONPTimerInterrrupt(int millis);
+        
+        void enableWakeInterrupts();
+
+        static void resetAONPTimer();
+
+        static void wakeFromRTC();
+
+        void x86_C2Request();
+        
+        void x86_C2LPRequest();
+
+        void (*pmCB)();
+};
+
+extern Power PM;
diff --git a/libraries/CuriePowerManagement/src/qmsi/power_states.h b/libraries/CuriePowerManagement/src/qmsi/power_states.h
new file mode 100644
index 00000000..eb5fecf0
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/qmsi/power_states.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2016, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived from this
+ *    software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE INTEL CORPORATION OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __POWER_STATES_H__
+#define __POWER_STATES_H__
+
+/**
+ * SoC Power mode control for Quark SE Microcontrollers.
+ *
+ * Available SoC states are:
+ *     - Low Power Sensing Standby (LPSS)
+ *     - Sleep
+ *
+ * LPSS can only be enabled from the Sensor core,
+ * refer to @ref ss_power_soc_lpss_enable for further details.
+ *
+ * @defgroup groupSoCPower Quark SE SoC Power states
+ * @{
+ */
+
+/**
+ * Enter SoC sleep state.
+ *
+ * Put the SoC into sleep state until next SoC wake event.
+ *
+ * - Core well is turned off
+ * - Always on well is on
+ * - Hybrid Clock is off
+ * - RTC Clock is on
+ *
+ * Possible SoC wake events are:
+ * 	- Low Power Comparator Interrupt
+ * 	- AON GPIO Interrupt
+ * 	- AON Timer Interrupt
+ * 	- RTC Interrupt
+ */
+void power_soc_sleep(void);
+
+/**
+ * Enter SoC deep sleep state.
+ *
+ * Put the SoC into deep sleep state until next SoC wake event.
+ *
+ * - Core well is turned off
+ * - Always on well is on
+ * - Hybrid Clock is off
+ * - RTC Clock is on
+ *
+ * Possible SoC wake events are:
+ * 	- Low Power Comparator Interrupt
+ * 	- AON GPIO Interrupt
+ * 	- AON Timer Interrupt
+ * 	- RTC Interrupt
+ *
+ * This function puts 1P8V regulators and 3P3V into Linear Mode.
+ */
+void power_soc_deep_sleep(void);
+
+/**
+ * @}
+ */
+
+#endif /* __POWER_STATES_H__ */
diff --git a/libraries/CuriePowerManagement/src/qmsi/qm_interrupt_router_regs.h b/libraries/CuriePowerManagement/src/qmsi/qm_interrupt_router_regs.h
new file mode 100644
index 00000000..216612e9
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/qmsi/qm_interrupt_router_regs.h
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2016, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived from this
+ *    software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE INTEL CORPORATION OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __QM_INTERRUPT_ROUTER_REGS_H__
+#define __QM_INTERRUPT_ROUTER_REGS_H__
+
+/**
+ * Quark SE SoC Event Router registers.
+ *
+ * @defgroup groupQUARKSESEEVENTROUTER SoC Event Router (SE)
+ * @{
+ */
+
+/**
+ * Masks for single source interrupts in the Event Router.
+ * To enable:  reg &= ~(MASK)
+ * To disable: reg |= MASK;
+ */
+#define QM_IR_INT_LMT_MASK BIT(0)
+#define QM_IR_INT_SS_MASK BIT(8)
+
+/* Masks for single source halts in the Event Router. */
+#define QM_IR_INT_LMT_HALT_MASK BIT(16)
+#define QM_IR_INT_SS_HALT_MASK BIT(24)
+
+/* Event Router Unmask interrupts for a peripheral. */
+#define QM_IR_UNMASK_LMT_INTERRUPTS(_peripheral_)                              \
+	(_peripheral_ &= ~(QM_IR_INT_LMT_MASK))
+#define QM_IR_UNMASK_SS_INTERRUPTS(_peripheral_)                               \
+	(_peripheral_ &= ~(QM_IR_INT_SS_MASK))
+
+/* Mask interrupts for a peripheral. */
+#define QM_IR_MASK_LMT_INTERRUPTS(_peripheral_)                                \
+	(_peripheral_ |= QM_IR_INT_LMT_MASK)
+#define QM_IR_MASK_SS_INTERRUPTS(_peripheral_)                                 \
+	(_peripheral_ |= QM_IR_INT_SS_MASK)
+
+/* Unmask halt for a peripheral. */
+#define QM_IR_UNMASK_LMT_HALTS(_peripheral_)                                   \
+	(_peripheral_ &= ~(QM_IR_INT_LMT_HALT_MASK))
+#define QM_IR_UNMASK_SS_HALTS(_peripheral_)                                    \
+	(_peripheral_ &= ~(QM_IR_INT_SS_HALT_MASK))
+
+/* Mask halt for a peripheral. */
+#define QM_IR_MASK_LMT_HALTS(_peripheral_)                                     \
+	(_peripheral_ |= QM_IR_INT_LMT_HALT_MASK)
+#define QM_IR_MASK_SS_HALTS(_peripheral_)                                      \
+	(_peripheral_ |= QM_IR_INT_SS_HALT_MASK)
+
+#define QM_IR_GET_LMT_MASK(_peripheral_) (_peripheral_ & QM_IR_INT_LMT_MASK)
+#define QM_IR_GET_LMT_HALT_MASK(_peripheral_)                                  \
+	(_peripheral_ & QM_IR_INT_LMT_HALT_MASK)
+
+#define QM_IR_GET_SS_MASK(_peripheral_) (_peripheral_ & QM_IR_INT_SS_MASK)
+#define QM_IR_GET_SS_HALT_MASK(_peripheral_)                                   \
+	(_peripheral_ & QM_IR_INT_SS_HALT_MASK)
+
+/* Define macros for use by the active core. */
+#if (QM_LAKEMONT)
+#define QM_IR_UNMASK_INTERRUPTS(_peripheral_)                                  \
+	QM_IR_UNMASK_LMT_INTERRUPTS(_peripheral_)
+#define QM_IR_MASK_INTERRUPTS(_peripheral_)                                    \
+	QM_IR_MASK_LMT_INTERRUPTS(_peripheral_)
+#define QM_IR_UNMASK_HALTS(_peripheral_) QM_IR_UNMASK_LMT_HALTS(_peripheral_)
+#define QM_IR_MASK_HALTS(_peripheral_) QM_IR_MASK_LMT_HALTS(_peripheral_)
+
+#define QM_IR_INT_MASK QM_IR_INT_LMT_MASK
+#define QM_IR_INT_HALT_MASK QM_IR_INT_LMT_HALT_MASK
+#define QM_IR_GET_MASK(_peripheral_) QM_IR_GET_LMT_MASK(_peripheral_)
+#define QM_IR_GET_HALT_MASK(_peripheral_) QM_IR_GET_LMT_HALT_MASK(_peripheral_)
+
+#elif(QM_SENSOR)
+#define QM_IR_UNMASK_INTERRUPTS(_peripheral_)                                  \
+	QM_IR_UNMASK_SS_INTERRUPTS(_peripheral_)
+#define QM_IR_MASK_INTERRUPTS(_peripheral_)                                    \
+	QM_IR_MASK_SS_INTERRUPTS(_peripheral_)
+#define QM_IR_UNMASK_HALTS(_peripheral_) QM_IR_UNMASK_SS_HALTS(_peripheral_)
+#define QM_IR_MASK_HALTS(_peripheral_) QM_IR_MASK_SS_HALTS(_peripheral_)
+
+#define QM_IR_INT_MASK QM_IR_INT_SS_MASK
+#define QM_IR_INT_HALT_MASK QM_IR_INT_SS_HALT_MASK
+#define QM_IR_GET_MASK(_peripheral_) QM_IR_GET_SS_MASK(_peripheral_)
+#define QM_IR_GET_HALT_MASK(_peripheral_) QM_IR_GET_SS_HALT_MASK(_peripheral_)
+#else
+#error "No active core selected."
+#endif
+
+/** SS I2C Interrupt register map. */
+typedef struct {
+	QM_RW uint32_t err_mask;
+	QM_RW uint32_t rx_avail_mask;
+	QM_RW uint32_t tx_req_mask;
+	QM_RW uint32_t stop_det_mask;
+} int_ss_i2c_reg_t;
+
+/** SS SPI Interrupt register map. */
+typedef struct {
+	QM_RW uint32_t err_int_mask;
+	QM_RW uint32_t rx_avail_mask;
+	QM_RW uint32_t tx_req_mask;
+} int_ss_spi_reg_t;
+
+/** Interrupt register map. */
+typedef struct {
+	QM_RW uint32_t ss_adc_0_error_int_mask; /**< Sensor ADC 0 Error. */
+	QM_RW uint32_t ss_adc_0_int_mask;       /**< Sensor ADC 0. */
+	QM_RW uint32_t ss_gpio_0_int_mask;      /**< Sensor GPIO 0. */
+	QM_RW uint32_t ss_gpio_1_int_mask;      /**< Sensor GPIO 1. */
+	int_ss_i2c_reg_t ss_i2c_0_int;		/**< Sensor I2C 0 Masks. */
+	int_ss_i2c_reg_t ss_i2c_1_int;		/**< Sensor I2C 1 Masks. */
+	int_ss_spi_reg_t ss_spi_0_int;		/**< Sensor SPI 0 Masks. */
+	int_ss_spi_reg_t ss_spi_1_int;		/**< Sensor SPI 1 Masks. */
+	QM_RW uint32_t i2c_master_0_int_mask;   /**< I2C Master 0. */
+	QM_RW uint32_t i2c_master_1_int_mask;   /**< I2C Master 1. */
+	QM_R uint32_t reserved;
+	QM_RW uint32_t spi_master_0_int_mask; /**< SPI Master 0. */
+	QM_RW uint32_t spi_master_1_int_mask; /**< SPI Master 1. */
+	QM_RW uint32_t spi_slave_0_int_mask;  /**< SPI Slave 0. */
+	QM_RW uint32_t uart_0_int_mask;       /**< UART 0. */
+	QM_RW uint32_t uart_1_int_mask;       /**< UART 1. */
+	QM_RW uint32_t i2s_0_int_mask;	/**< I2S 0. */
+	QM_RW uint32_t gpio_0_int_mask;       /**< GPIO 0. */
+	QM_RW uint32_t pwm_0_int_mask;	/**< PWM 0. */
+	QM_RW uint32_t usb_0_int_mask;	/**< USB 0. */
+	QM_RW uint32_t rtc_0_int_mask;	/**< RTC 0. */
+	QM_RW uint32_t wdt_0_int_mask;	/**< WDT 0. */
+	QM_RW uint32_t dma_0_int_0_mask;      /**< DMA 0 Ch 0. */
+	QM_RW uint32_t dma_0_int_1_mask;      /**< DMA 0 Ch 1. */
+	QM_RW uint32_t dma_0_int_2_mask;      /**< DMA 0 Ch 2. */
+	QM_RW uint32_t dma_0_int_3_mask;      /**< DMA 0 Ch 3. */
+	QM_RW uint32_t dma_0_int_4_mask;      /**< DMA 0 Ch 4. */
+	QM_RW uint32_t dma_0_int_5_mask;      /**< DMA 0 Ch 5. */
+	QM_RW uint32_t dma_0_int_6_mask;      /**< DMA 0 Ch 6. */
+	QM_RW uint32_t dma_0_int_7_mask;      /**< DMA 0 Ch 7. */
+	/** Mailbox 0 Combined 8 Channel Host and Sensor Masks. */
+	QM_RW uint32_t mailbox_0_int_mask;
+	/** Comparator Sensor Halt Mask. */
+	QM_RW uint32_t comparator_0_ss_halt_int_mask;
+	/** Comparator Host Halt Mask. */
+	QM_RW uint32_t comparator_0_host_halt_int_mask;
+	/** Comparator Sensor Mask. */
+	QM_RW uint32_t comparator_0_ss_int_mask;
+	/** Comparator Host Mask. */
+	QM_RW uint32_t comparator_0_host_int_mask;
+	QM_RW uint32_t host_bus_error_int_mask; /**< Host bus error. */
+	QM_RW uint32_t dma_0_error_int_mask;    /**< DMA 0 Error. */
+	QM_RW uint32_t sram_mpr_0_int_mask;     /**< SRAM MPR 0. */
+	QM_RW uint32_t flash_mpr_0_int_mask;    /**< Flash MPR 0. */
+	QM_RW uint32_t flash_mpr_1_int_mask;    /**< Flash MPR 1. */
+	QM_RW uint32_t aonpt_0_int_mask;	/**< AONPT 0. */
+	QM_RW uint32_t adc_0_pwr_int_mask;      /**< ADC 0 PWR. */
+	QM_RW uint32_t adc_0_cal_int_mask;      /**< ADC 0 CAL. */
+	QM_RW uint32_t aon_gpio_0_int_mask;     /**< AON GPIO 0. */
+	QM_RW uint32_t lock_int_mask_reg; /**< Interrupt Mask Lock Register. */
+} qm_interrupt_router_reg_t;
+
+/* Number of SCSS interrupt mask registers (excluding mask lock register). */
+#define QM_INTERRUPT_ROUTER_MASK_NUMREG                                        \
+	((sizeof(qm_interrupt_router_reg_t) / sizeof(uint32_t)) - 1)
+
+/* Default POR SCSS interrupt mask (all interrupts masked). */
+#define QM_INTERRUPT_ROUTER_MASK_DEFAULT (0xFFFFFFFF)
+
+#if (UNIT_TEST)
+qm_interrupt_router_reg_t test_interrupt_router;
+#define QM_INTERRUPT_ROUTER                                                    \
+	((qm_interrupt_router_reg_t *)(&test_interrupt_router))
+
+#else
+/* System control subsystem interrupt masking register block. */
+#define QM_INTERRUPT_ROUTER_BASE (0xB0800400)
+#define QM_INTERRUPT_ROUTER                                                    \
+	((qm_interrupt_router_reg_t *)QM_INTERRUPT_ROUTER_BASE)
+#endif
+
+#define QM_IR_DMA_ERROR_HOST_MASK (0x000000FF)
+#define QM_IR_DMA_ERROR_SS_MASK (0x0000FF00)
+
+/** @} */
+
+#endif /* __QM_INTERRUPT_ROUTER_REGS_H__ */
diff --git a/libraries/CuriePowerManagement/src/qmsi/qm_sensor_regs.h b/libraries/CuriePowerManagement/src/qmsi/qm_sensor_regs.h
new file mode 100644
index 00000000..324d001e
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/qmsi/qm_sensor_regs.h
@@ -0,0 +1,673 @@
+/*
+ * Copyright (c) 2016, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived from this
+ *    software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE INTEL CORPORATION OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __SENSOR_REGISTERS_H__
+#define __SENSOR_REGISTERS_H__
+
+/**
+ * Quark SE SoC Sensor Subsystem Registers.
+ *
+ * For detailed description please read the SOC datasheet.
+ *
+ * @defgroup groupSSSEREG SoC Registers (Sensor Subsystem)
+ * @{
+ */
+
+#define BIT(x) (1U << (x))
+
+/* Bitwise OR operation macro for registers in the auxiliary memory space. */
+#define QM_SS_REG_AUX_OR(reg, mask)                                            \
+	(__builtin_arc_sr(__builtin_arc_lr(reg) | (mask), reg))
+/* Bitwise NAND operation macro for registers in the auxiliary memory space. */
+#define QM_SS_REG_AUX_NAND(reg, mask)                                          \
+	(__builtin_arc_sr(__builtin_arc_lr(reg) & (~(mask)), reg))
+
+/* Sensor Subsystem status32 register. */
+#define QM_SS_AUX_STATUS32 (0xA)
+/** Interrupt priority threshold. */
+#define QM_SS_STATUS32_E_MASK (0x1E)
+/** Interrupt enable. */
+#define QM_SS_STATUS32_IE_MASK BIT(31)
+/* Sensor Subsystem control register. */
+#define QM_SS_AUX_IC_CTRL (0x11)
+/* Sensor Subsystem cache invalidate register. */
+#define QM_SS_AUX_IC_IVIL (0x19)
+/* Sensor Subsystem vector base register. */
+#define QM_SS_AUX_INT_VECTOR_BASE (0x25)
+
+/**
+ * @name SS Interrupt
+ * @{
+ */
+
+#define QM_SS_EXCEPTION_NUM (16)  /* Exceptions and traps in ARC EM core. */
+#define QM_SS_INT_TIMER_NUM (2)   /* Internal interrupts in ARC EM core. */
+#define QM_SS_IRQ_SENSOR_NUM (18) /* IRQ's from the Sensor Subsystem. */
+#define QM_SS_IRQ_COMMON_NUM (32) /* IRQ's from the common SoC fabric. */
+#define QM_SS_INT_VECTOR_NUM                                                   \
+        (QM_SS_EXCEPTION_NUM + QM_SS_INT_TIMER_NUM + QM_SS_IRQ_SENSOR_NUM +    \
+         QM_SS_IRQ_COMMON_NUM)
+#define QM_SS_IRQ_NUM (QM_SS_IRQ_SENSOR_NUM + QM_SS_IRQ_COMMON_NUM)
+
+/**
+ * SS IRQ context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by
+ * qm_irq_save_context and qm_irq_restore_context functions.
+ */
+typedef struct {
+	uint32_t status32_irq_threshold; /**< STATUS32 Interrupt Threshold. */
+	uint32_t status32_irq_enable;    /**< STATUS32 Interrupt Enable. */
+	uint32_t irq_ctrl; /**< Interrupt Context Saving Control Register. */
+
+	/**
+	 * IRQ configuration:
+	 * - IRQ Priority:BIT(6):BIT(2)
+	 * - IRQ Trigger:BIT(1)
+	 * - IRQ Enable:BIT(0)
+	 */
+	uint8_t irq_config[QM_SS_INT_VECTOR_NUM - 1];
+} qm_irq_context_t;
+
+/** General Purpose register map. */
+typedef struct {
+	volatile uint32_t gps0; /**< General Purpose Sticky Register 0 */
+	volatile uint32_t gps1; /**< General Purpose Sticky Register 1 */
+	volatile uint32_t gps2; /**< General Purpose Sticky Register 2 */
+	volatile uint32_t gps3; /**< General Purpose Sticky Register 3 */
+	volatile uint32_t reserved;
+	volatile uint32_t gp0; /**< General Purpose Scratchpad Register 0 */
+	volatile uint32_t gp1; /**< General Purpose Scratchpad Register 1 */
+	volatile uint32_t gp2; /**< General Purpose Scratchpad Register 2 */
+	volatile uint32_t gp3; /**< General Purpose Scratchpad Register 3 */
+	volatile uint32_t reserved1;
+	volatile uint32_t id;    /**< Identification Register */
+	volatile uint32_t rev;   /**< Revision Register */
+	volatile uint32_t wo_sp; /**< Write-One-to-Set Scratchpad Register */
+	volatile uint32_t
+	    wo_st; /**< Write-One-to-Set Sticky Scratchpad Register */
+} qm_scss_gp_reg_t;
+
+#define QM_SCSS_GP_BASE (0xB0800100)
+#define QM_SCSS_GP ((qm_scss_gp_reg_t *)QM_SCSS_GP_BASE)
+
+/* The GPS0 register usage. */
+#define QM_GPS0_BIT_FM (0)	    /**< Start Firmware Manager. */
+#define QM_GPS0_BIT_X86_WAKEUP (1)    /**< Lakemont core reset type. */
+#define QM_GPS0_BIT_SENSOR_WAKEUP (2) /**< Sensor core reset type. */
+
+/** System Core register map. */
+typedef struct {
+	volatile uint32_t osc0_cfg0;    /**< Hybrid Oscillator Configuration 0. */
+	volatile uint32_t osc0_stat1;   /**< Hybrid Oscillator status 1. */
+	volatile uint32_t osc0_cfg1;    /**< Hybrid Oscillator configuration 1. */
+	volatile uint32_t osc1_stat0;   /**< RTC Oscillator status 0. */
+	volatile uint32_t osc1_cfg0;    /**< RTC Oscillator Configuration 0. */
+	volatile uint32_t usb_pll_cfg0; /**< USB Phase lock look configuration. */
+	volatile uint32_t
+	    ccu_periph_clk_gate_ctl; /**< Peripheral Clock Gate Control. */
+	volatile uint32_t
+	    ccu_periph_clk_div_ctl0; /**< Peripheral Clock Divider Control. 0 */
+	volatile uint32_t
+	    ccu_gpio_db_clk_ctl; /**< Peripheral Clock Divider Control 1. */
+	volatile uint32_t
+	    ccu_ext_clock_ctl; /**< External Clock Control Register. */
+	/** Sensor Subsystem peripheral clock gate control. */
+	volatile uint32_t ccu_ss_periph_clk_gate_ctl;
+	volatile uint32_t ccu_lp_clk_ctl; /**< System Low Power Clock Control. */
+	volatile uint32_t reserved;
+	volatile uint32_t ccu_mlayer_ahb_ctl; /**< AHB Control Register. */
+	volatile uint32_t ccu_sys_clk_ctl; /**< System Clock Control Register. */
+	volatile uint32_t osc_lock_0;      /**< Clocks Lock Register. */
+} qm_scss_ccu_reg_t;
+
+#define QM_SCSS_CCU ((qm_scss_ccu_reg_t *)SCSS_REGISTER_BASE)
+
+/** Power Management register map. */
+typedef struct {
+	volatile uint32_t p_lvl2; /**< Processor level 2 */
+	volatile uint32_t reserved[4];
+	volatile uint32_t pm1c; /**< Power management 1 control */
+	volatile uint32_t reserved1[9];
+	volatile uint32_t aon_vr;     /**< AON Voltage Regulator */
+	volatile uint32_t plat3p3_vr; /**< Platform 3p3 voltage regulator */
+	volatile uint32_t plat1p8_vr; /**< Platform 1p8 voltage regulator */
+	volatile uint32_t host_vr;    /**< Host Voltage Regulator */
+	volatile uint32_t slp_cfg;    /**< Sleeping Configuration */
+	/** Power Management Network (PMNet) Control and Status */
+	volatile uint32_t pmnetcs;
+	volatile uint32_t pm_wait; /**< Power Management Wait */
+	volatile uint32_t reserved2;
+	volatile uint32_t p_sts; /**< Processor Status */
+	volatile uint32_t reserved3[3];
+	volatile uint32_t rstc; /**< Reset Control */
+	volatile uint32_t rsts; /**< Reset Status */
+	volatile uint32_t reserved4[6];
+	volatile uint32_t vr_lock; /**< Voltage regulator lock */
+	volatile uint32_t pm_lock; /**< Power Management Lock */
+} qm_scss_pmu_reg_t;
+
+#define QM_SCSS_PMU_BASE (0xB0800504)
+#define QM_SCSS_PMU ((qm_scss_pmu_reg_t *)QM_SCSS_PMU_BASE)
+
+#define QM_SS_CFG_ARC_RUN_REQ_A BIT(24)
+#define QM_P_STS_HALT_INTERRUPT_REDIRECTION BIT(26)
+#define QM_P_STS_ARC_HALT BIT(14)
+
+#define QM_AON_VR_VSEL_MASK (0xFFE0)
+#define QM_AON_VR_VSEL_1V2 (0x8)
+#define QM_AON_VR_VSEL_1V35 (0xB)
+#define QM_AON_VR_VSEL_1V8 (0x10)
+#define QM_AON_VR_EN BIT(7)
+#define QM_AON_VR_VSTRB BIT(5)
+
+#define QM_SCSS_SLP_CFG_LPMODE_EN BIT(8)
+#define QM_SCSS_SLP_CFG_RTC_DIS BIT(7)
+#define QM_SCSS_PM1C_SLPEN BIT(13)
+#define QM_SCSS_HOST_VR_EN BIT(7)
+#define QM_SCSS_PLAT3P3_VR_EN BIT(7)
+#define QM_SCSS_PLAT1P8_VR_EN BIT(7)
+#define QM_SCSS_HOST_VR_VREG_SEL BIT(6)
+#define QM_SCSS_PLAT3P3_VR_VREG_SEL BIT(6)
+#define QM_SCSS_PLAT1P8_VR_VREG_SEL BIT(6)
+#define QM_SCSS_VR_ROK BIT(10)
+#define QM_SCSS_VR_EN BIT(7)
+#define QM_SCSS_VR_VREG_SEL BIT(6)
+
+#define QM_SCSS_CCU_SS_LPS_EN BIT(0)
+
+typedef enum {
+        QM_SS_IRQ_LEVEL_SENSITIVE = 0,
+        QM_SS_IRQ_EDGE_SENSITIVE = 1
+} qm_ss_irq_trigger_t;
+
+#define QM_SS_AUX_IRQ_CTRL (0xE)
+#define QM_SS_AUX_IRQ_HINT (0x201)
+#define QM_SS_AUX_IRQ_PRIORITY (0x206)
+#define QM_SS_AUX_IRQ_STATUS (0x406)
+#define QM_SS_AUX_IRQ_SELECT (0x40B)
+#define QM_SS_AUX_IRQ_ENABLE (0x40C)
+#define QM_SS_AUX_IRQ_TRIGGER (0x40D)
+
+/** Always-On Timer Interrupt. */
+#define QM_IRQ_AONPT_0_INT 28
+#define QM_IRQ_AONPT_0_INT_MASK_OFFSET 32
+#define QM_IRQ_AONPT_0_INT_VECTOR 64
+
+/** RTC Single Interrupt. */
+#define QM_IRQ_RTC_0_INT 11
+#define QM_IRQ_RTC_0_INT_MASK_OFFSET 12
+#define QM_IRQ_RTC_0_INT_VECTOR 47
+
+/** @} */
+
+/**
+ * @name SS Timer
+ * @{
+ */
+
+typedef enum {
+	QM_SS_TIMER_COUNT = 0,
+	QM_SS_TIMER_CONTROL,
+	QM_SS_TIMER_LIMIT
+} qm_ss_timer_reg_t;
+
+/**
+ * Sensor Subsystem Timers.
+ */
+typedef enum { QM_SS_TIMER_0 = 0, QM_SS_TIMER_NUM } qm_ss_timer_t;
+
+/*
+ * SS TIMER context type.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_ss_timer_save_context
+ * and qm_ss_timer_restore_context functions.
+ */
+typedef struct {
+	uint32_t timer_count;   /**< Timer count. */
+	uint32_t timer_control; /**< Timer control. */
+	uint32_t timer_limit;   /**< Timer limit. */
+} qm_ss_timer_context_t;
+
+#define QM_SS_TIMER_0_BASE (0x21)
+#define QM_SS_TIMER_1_BASE (0x100)
+#define QM_SS_TSC_BASE QM_SS_TIMER_1_BASE
+
+#define QM_SS_TIMER_CONTROL_INT_EN_OFFSET (0)
+#define QM_SS_TIMER_CONTROL_NON_HALTED_OFFSET (1)
+#define QM_SS_TIMER_CONTROL_WATCHDOG_OFFSET (2)
+#define QM_SS_TIMER_CONTROL_INT_PENDING_OFFSET (3)
+/** @} */
+
+/**
+ * GPIO registers and definitions.
+ *
+ * @name SS GPIO
+ * @{
+ */
+
+/** Sensor Subsystem GPIO register block type. */
+typedef enum {
+	QM_SS_GPIO_SWPORTA_DR = 0,
+	QM_SS_GPIO_SWPORTA_DDR,
+	QM_SS_GPIO_INTEN = 3,
+	QM_SS_GPIO_INTMASK,
+	QM_SS_GPIO_INTTYPE_LEVEL,
+	QM_SS_GPIO_INT_POLARITY,
+	QM_SS_GPIO_INTSTATUS,
+	QM_SS_GPIO_DEBOUNCE,
+	QM_SS_GPIO_PORTA_EOI,
+	QM_SS_GPIO_EXT_PORTA,
+	QM_SS_GPIO_LS_SYNC
+} qm_ss_gpio_reg_t;
+
+/**
+ * SS GPIO context type.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_ss_gpio_save_context and
+ * qm_ss_gpio_restore_context functions.
+ */
+typedef struct {
+	uint32_t gpio_swporta_dr;    /**< Port A Data. */
+	uint32_t gpio_swporta_ddr;   /**< Port A Data Direction. */
+	uint32_t gpio_inten;	 /**< Interrupt Enable. */
+	uint32_t gpio_intmask;       /**< Interrupt Mask. */
+	uint32_t gpio_inttype_level; /**< Interrupt Type. */
+	uint32_t gpio_int_polarity;  /**< Interrupt Polarity. */
+	uint32_t gpio_debounce;      /**< Debounce Enable. */
+	uint32_t gpio_ls_sync;       /**< Synchronization Level. */
+} qm_ss_gpio_context_t;
+
+#define QM_SS_GPIO_NUM_PINS (16)
+#define QM_SS_GPIO_LS_SYNC_CLK_EN BIT(31)
+#define QM_SS_GPIO_LS_SYNC_SYNC_LVL BIT(0)
+
+/** Sensor Subsystem GPIO. */
+typedef enum { QM_SS_GPIO_0 = 0, QM_SS_GPIO_1, QM_SS_GPIO_NUM } qm_ss_gpio_t;
+
+#define QM_SS_GPIO_0_BASE (0x80017800)
+#define QM_SS_GPIO_1_BASE (0x80017900)
+
+/** @} */
+
+/**
+ * I2C registers and definitions.
+ *
+ * @name SS I2C
+ * @{
+ */
+
+/** Sensor Subsystem I2C register block type. */
+typedef enum {
+	QM_SS_I2C_CON = 0,
+	QM_SS_I2C_DATA_CMD,
+	QM_SS_I2C_SS_SCL_CNT,
+	QM_SS_I2C_FS_SCL_CNT = 0x04,
+	QM_SS_I2C_INTR_STAT = 0x06,
+	QM_SS_I2C_INTR_MASK,
+	QM_SS_I2C_TL,
+	QM_SS_I2C_INTR_CLR = 0x0A,
+	QM_SS_I2C_STATUS,
+	QM_SS_I2C_TXFLR,
+	QM_SS_I2C_RXFLR,
+	QM_SS_I2C_SDA_CONFIG,
+	QM_SS_I2C_TX_ABRT_SOURCE,
+	QM_SS_I2C_ENABLE_STATUS = 0x11
+} qm_ss_i2c_reg_t;
+
+/**
+ * SS I2C context type.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_ss_gpio_save_context and
+ * qm_ss_gpio_restore_context functions.
+ */
+typedef struct {
+	uint32_t i2c_con;
+	uint32_t i2c_ss_scl_cnt;
+	uint32_t i2c_fs_scl_cnt;
+} qm_ss_i2c_context_t;
+
+#define QM_SS_I2C_CON_ENABLE BIT(0)
+#define QM_SS_I2C_CON_ABORT BIT(1)
+#define QM_SS_I2C_CON_SPEED_SS BIT(3)
+#define QM_SS_I2C_CON_SPEED_FS BIT(4)
+#define QM_SS_I2C_CON_SPEED_MASK (0x18)
+#define QM_SS_I2C_CON_IC_10BITADDR BIT(5)
+#define QM_SS_I2C_CON_IC_10BITADDR_OFFSET (5)
+#define QM_SS_I2C_CON_IC_10BITADDR_MASK (5)
+#define QM_SS_I2C_CON_RESTART_EN BIT(7)
+#define QM_SS_I2C_CON_TAR_SAR_OFFSET (9)
+#define QM_SS_I2C_CON_TAR_SAR_MASK (0x7FE00)
+#define QM_SS_I2C_CON_TAR_SAR_10_BIT_MASK (0x3FF)
+#define QM_SS_I2C_CON_SPKLEN_OFFSET (22)
+#define QM_SS_I2C_CON_SPKLEN_MASK (0x3FC00000)
+#define QM_SS_I2C_CON_CLK_ENA BIT(31)
+
+#define QM_SS_I2C_DATA_CMD_CMD BIT(8)
+#define QM_SS_I2C_DATA_CMD_STOP BIT(9)
+#define QM_SS_I2C_DATA_CMD_PUSH (0xC0000000)
+#define QM_SS_I2C_DATA_CMD_POP (0x80000000)
+
+#define QM_SS_I2C_SS_FS_SCL_CNT_HCNT_OFFSET (16)
+#define QM_SS_I2C_SS_FS_SCL_CNT_16BIT_MASK (0xFFFF)
+
+#define QM_SS_I2C_INTR_STAT_RX_UNDER BIT(0)
+#define QM_SS_I2C_INTR_STAT_RX_OVER BIT(1)
+#define QM_SS_I2C_INTR_STAT_RX_FULL BIT(2)
+#define QM_SS_I2C_INTR_STAT_TX_OVER BIT(3)
+#define QM_SS_I2C_INTR_STAT_TX_EMPTY BIT(4)
+#define QM_SS_I2C_INTR_STAT_TX_ABRT BIT(6)
+
+#define QM_SS_I2C_INTR_MASK_ALL (0x0)
+#define QM_SS_I2C_INTR_MASK_RX_UNDER BIT(0)
+#define QM_SS_I2C_INTR_MASK_RX_OVER BIT(1)
+#define QM_SS_I2C_INTR_MASK_RX_FULL BIT(2)
+#define QM_SS_I2C_INTR_MASK_TX_OVER BIT(3)
+#define QM_SS_I2C_INTR_MASK_TX_EMPTY BIT(4)
+#define QM_SS_I2C_INTR_MASK_TX_ABRT BIT(6)
+
+#define QM_SS_I2C_TL_TX_TL_OFFSET (16)
+#define QM_SS_I2C_TL_RX_TL_MASK (0xFF)
+#define QM_SS_I2C_TL_TX_TL_MASK (0xFF0000)
+
+#define QM_SS_I2C_INTR_CLR_ALL (0xFF)
+#define QM_SS_I2C_INTR_CLR_TX_ABRT BIT(6)
+
+#define QM_SS_I2C_TX_ABRT_SOURCE_NAK_MASK (0x09)
+#define QM_SS_I2C_TX_ABRT_SOURCE_ALL_MASK (0x1FFFF)
+#define QM_SS_I2C_TX_ABRT_SBYTE_NORSTRT BIT(9)
+#define QM_SS_I2C_TX_ABRT_SOURCE_ART_LOST BIT(12)
+
+#define QM_SS_I2C_ENABLE_CONTROLLER_EN BIT(0)
+#define QM_SS_I2C_ENABLE_STATUS_IC_EN BIT(0)
+
+#define QM_SS_I2C_STATUS_BUSY_MASK (0x21)
+#define QM_SS_I2C_STATUS_RFNE BIT(3)
+#define QM_SS_I2C_STATUS_TFE BIT(2)
+#define QM_SS_I2C_STATUS_TFNF BIT(1)
+
+#define QM_SS_I2C_IC_LCNT_MAX (65525)
+#define QM_SS_I2C_IC_LCNT_MIN (8)
+#define QM_SS_I2C_IC_HCNT_MAX (65525)
+#define QM_SS_I2C_IC_HCNT_MIN (6)
+
+#define QM_SS_I2C_FIFO_SIZE (8)
+
+/** Sensor Subsystem I2C */
+typedef enum { QM_SS_I2C_0 = 0, QM_SS_I2C_1, QM_SS_I2C_NUM } qm_ss_i2c_t;
+
+#define QM_SS_I2C_0_BASE (0x80012000)
+#define QM_SS_I2C_1_BASE (0x80012100)
+
+/** @} */
+/** Sensor Subsystem ADC @{*/
+
+/** Sensor Subsystem ADC registers */
+typedef enum {
+	QM_SS_ADC_SET = 0,    /**< ADC and sequencer settings register. */
+	QM_SS_ADC_DIVSEQSTAT, /**< ADC clock and sequencer status register. */
+	QM_SS_ADC_SEQ,	/**< ADC sequence entry register. */
+	QM_SS_ADC_CTRL,       /**< ADC control register. */
+	QM_SS_ADC_INTSTAT,    /**< ADC interrupt status register. */
+	QM_SS_ADC_SAMPLE      /**< ADC sample register. */
+} qm_ss_adc_reg_t;
+
+/** Sensor Subsystem ADC */
+typedef enum {
+	QM_SS_ADC_0 = 0, /**< ADC first module. */
+	QM_SS_ADC_NUM
+} qm_ss_adc_t;
+
+/**
+ * SS ADC context type.
+ *
+ * The application should not modify the content of this structure.
+ *
+ * This structure is intented to be used by qm_ss_adc_save_context and
+ * qm_ss_adc_restore_context functions only.
+ */
+typedef struct {
+	uint32_t adc_set;	/**< ADC settings. */
+	uint32_t adc_divseqstat; /**< ADC clock divider and sequencer status. */
+	uint32_t adc_seq;	/**< ADC sequencer entry. */
+	uint32_t adc_ctrl;       /**< ADC control. */
+} qm_ss_adc_context_t;
+
+/* SS ADC register base. */
+#define QM_SS_ADC_BASE (0x80015000)
+
+/* For 1MHz, the max divisor is 7. */
+#define QM_SS_ADC_DIV_MAX (7)
+
+#define QM_SS_ADC_FIFO_LEN (32)
+
+#define QM_SS_ADC_SET_POP_RX BIT(31)
+#define QM_SS_ADC_SET_FLUSH_RX BIT(30)
+#define QM_SS_ADC_SET_THRESHOLD_MASK (0x3F000000)
+#define QM_SS_ADC_SET_THRESHOLD_OFFSET (24)
+#define QM_SS_ADC_SET_SEQ_ENTRIES_MASK (0x3F0000)
+#define QM_SS_ADC_SET_SEQ_ENTRIES_OFFSET (16)
+#define QM_SS_ADC_SET_SEQ_MODE BIT(13)
+#define QM_SS_ADC_SET_SAMPLE_WIDTH_MASK (0x1F)
+
+#define QM_SS_ADC_DIVSEQSTAT_CLK_RATIO_MASK (0x1FFFFF)
+
+#define QM_SS_ADC_CTRL_CLR_SEQERROR BIT(19)
+#define QM_SS_ADC_CTRL_CLR_UNDERFLOW BIT(18)
+#define QM_SS_ADC_CTRL_CLR_OVERFLOW BIT(17)
+#define QM_SS_ADC_CTRL_CLR_DATA_A BIT(16)
+#define QM_SS_ADC_CTRL_MSK_SEQERROR BIT(11)
+#define QM_SS_ADC_CTRL_MSK_UNDERFLOW BIT(10)
+#define QM_SS_ADC_CTRL_MSK_OVERFLOW BIT(9)
+#define QM_SS_ADC_CTRL_MSK_DATA_A BIT(8)
+#define QM_SS_ADC_CTRL_SEQ_TABLE_RST BIT(6)
+#define QM_SS_ADC_CTRL_SEQ_PTR_RST BIT(5)
+#define QM_SS_ADC_CTRL_SEQ_START BIT(4)
+#define QM_SS_ADC_CTRL_CLK_ENA BIT(2)
+#define QM_SS_ADC_CTRL_ADC_ENA BIT(1)
+
+#define QM_SS_ADC_CTRL_MSK_ALL_INT (0xF00)
+#define QM_SS_ADC_CTRL_CLR_ALL_INT (0xF0000)
+
+#define QM_SS_ADC_SEQ_DELAYODD_OFFSET (21)
+#define QM_SS_ADC_SEQ_MUXODD_OFFSET (16)
+#define QM_SS_ADC_SEQ_DELAYEVEN_OFFSET (5)
+
+#define QM_SS_ADC_SEQ_DUMMY (0x480)
+
+#define QM_SS_ADC_INTSTAT_SEQERROR BIT(3)
+#define QM_SS_ADC_INTSTAT_UNDERFLOW BIT(2)
+#define QM_SS_ADC_INTSTAT_OVERFLOW BIT(1)
+#define QM_SS_ADC_INTSTAT_DATA_A BIT(0)
+
+/** End of Sensor Subsystem ADC @}*/
+
+/**
+ * CREG Registers.
+ *
+ * @name SS CREG
+ * @{
+ */
+
+/* Sensor Subsystem CREG */
+typedef enum {
+	QM_SS_IO_CREG_MST0_CTRL = 0x0,  /**< Master control register. */
+	QM_SS_IO_CREG_SLV0_OBSR = 0x80, /**< Slave control register. */
+	QM_SS_IO_CREG_SLV1_OBSR = 0x180 /**< Slave control register. */
+} qm_ss_creg_reg_t;
+
+/* MST0_CTRL fields */
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_PWR_MODE_OFFSET (1)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_PWR_MODE_MASK (0x7)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_DELAY_OFFSET (3)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_DELAY_MASK (0xFFF8)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_CAL_REQ BIT(16)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_CAL_CMD_OFFSET (17)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_CAL_CMD_MASK (0xE0000)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_CAL_VAL_OFFSET (20)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_CAL_VAL_MASK (0x7F00000)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_CAL_VAL_MAX (0x7F)
+#define QM_SS_IO_CREG_MST0_CTRL_SPI1_CLK_GATE BIT(27)
+#define QM_SS_IO_CREG_MST0_CTRL_SPI0_CLK_GATE BIT(28)
+#define QM_SS_IO_CREG_MST0_CTRL_I2C0_CLK_GATE BIT(29)
+#define QM_SS_IO_CREG_MST0_CTRL_I2C1_CLK_GATE BIT(30)
+#define QM_SS_IO_CREG_MST0_CTRL_ADC_CLK_GATE BIT(31)
+/* SLV0_OBSR fields */
+#define QM_SS_IO_CREG_SLV0_OBSR_ADC_CAL_VAL_OFFSET (5)
+#define QM_SS_IO_CREG_SLV0_OBSR_ADC_CAL_VAL_MASK (0xFE0)
+#define QM_SS_IO_CREG_SLV0_OBSR_ADC_CAL_ACK BIT(4)
+#define QM_SS_IO_CREG_SLV0_OBSR_ADC_PWR_MODE_STS BIT(3)
+
+#define SS_CLK_PERIPH_ALL_IN_CREG                                              \
+	(SS_CLK_PERIPH_ADC | SS_CLK_PERIPH_I2C_1 | SS_CLK_PERIPH_I2C_0 |       \
+	 SS_CLK_PERIPH_SPI_1 | SS_CLK_PERIPH_SPI_0)
+
+/* SS CREG base. */
+#define QM_SS_CREG_BASE (0x80018000)
+
+/** @} */
+
+/**
+ * I2C registers and definitions.
+ *
+ * @name SS SPI
+ * @{
+ */
+
+/** Sensor Subsystem SPI register map. */
+typedef enum {
+	QM_SS_SPI_CTRL = 0,   /**< SPI control register. */
+	QM_SS_SPI_SPIEN = 2,  /**< SPI enable register. */
+	QM_SS_SPI_TIMING = 4, /**< SPI serial clock divider value. */
+	QM_SS_SPI_FTLR,       /**< Threshold value for TX/RX FIFO. */
+	QM_SS_SPI_TXFLR = 7,  /**< Number of valid data entries in TX FIFO. */
+	QM_SS_SPI_RXFLR,      /**< Number of valid data entries in RX FIFO. */
+	QM_SS_SPI_SR,	 /**< SPI status register.	*/
+	QM_SS_SPI_INTR_STAT,  /**< Interrupt status register. */
+	QM_SS_SPI_INTR_MASK,  /**< Interrupt mask register. */
+	QM_SS_SPI_CLR_INTR,   /**< Interrupt clear register. */
+	QM_SS_SPI_DR,	 /**< RW buffer for FIFOs. */
+} qm_ss_spi_reg_t;
+
+/**
+ * Sensor Subsystem SPI context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by
+ * the qm_ss_spi_save_context and qm_ss_spi_restore_context functions.
+ */
+typedef struct {
+	uint32_t spi_ctrl;   /**< Control Register. */
+	uint32_t spi_spien;  /**< SPI Enable Register. */
+	uint32_t spi_timing; /**< Timing Register. */
+} qm_ss_spi_context_t;
+
+/** Sensor Subsystem SPI modules. */
+typedef enum {
+	QM_SS_SPI_0 = 0, /**< SPI module 0 */
+	QM_SS_SPI_1,     /**< SPI module 1 */
+	QM_SS_SPI_NUM
+} qm_ss_spi_t;
+
+#define QM_SS_SPI_0_BASE (0x80010000)
+#define QM_SS_SPI_1_BASE (0x80010100)
+
+#define QM_SS_SPI_CTRL_DFS_OFFS (0)
+#define QM_SS_SPI_CTRL_DFS_MASK (0x0000000F)
+#define QM_SS_SPI_CTRL_BMOD_OFFS (6)
+#define QM_SS_SPI_CTRL_BMOD_MASK (0x000000C0)
+#define QM_SS_SPI_CTRL_SCPH BIT(6)
+#define QM_SS_SPI_CTRL_SCPOL BIT(7)
+#define QM_SS_SPI_CTRL_TMOD_OFFS (8)
+#define QM_SS_SPI_CTRL_TMOD_MASK (0x00000300)
+#define QM_SS_SPI_CTRL_SRL BIT(11)
+#define QM_SS_SPI_CTRL_CLK_ENA BIT(15)
+#define QM_SS_SPI_CTRL_NDF_OFFS (16)
+#define QM_SS_SPI_CTRL_NDF_MASK (0xFFFF0000)
+
+#define QM_SS_SPI_SPIEN_EN BIT(0)
+#define QM_SS_SPI_SPIEN_SER_OFFS (4)
+#define QM_SS_SPI_SPIEN_SER_MASK (0x000000F0)
+
+#define QM_SS_SPI_TIMING_SCKDV_OFFS (0)
+#define QM_SS_SPI_TIMING_SCKDV_MASK (0x0000FFFF)
+#define QM_SS_SPI_TIMING_RSD_OFFS (16)
+#define QM_SS_SPI_TIMING_RSD_MASK (0x00FF0000)
+
+#define QM_SS_SPI_FTLR_RFT_OFFS (0)
+#define QM_SS_SPI_FTLR_RFT_MASK (0x0000FFFF)
+#define QM_SS_SPI_FTLR_TFT_OFFS (16)
+#define QM_SS_SPI_FTLR_TFT_MASK (0xFFFF0000)
+
+#define QM_SS_SPI_SR_BUSY BIT(0)
+#define QM_SS_SPI_SR_TFNF BIT(1)
+#define QM_SS_SPI_SR_TFE BIT(2)
+#define QM_SS_SPI_SR_RFNE BIT(3)
+#define QM_SS_SPI_SR_RFF BIT(4)
+
+#define QM_SS_SPI_INTR_TXEI BIT(0)
+#define QM_SS_SPI_INTR_TXOI BIT(1)
+#define QM_SS_SPI_INTR_RXUI BIT(2)
+#define QM_SS_SPI_INTR_RXOI BIT(3)
+#define QM_SS_SPI_INTR_RXFI BIT(4)
+#define QM_SS_SPI_INTR_ALL (0x0000001F)
+
+#define QM_SS_SPI_INTR_STAT_TXEI QM_SS_SPI_INTR_TXEI
+#define QM_SS_SPI_INTR_STAT_TXOI QM_SS_SPI_INTR_TXOI
+#define QM_SS_SPI_INTR_STAT_RXUI QM_SS_SPI_INTR_RXUI
+#define QM_SS_SPI_INTR_STAT_RXOI QM_SS_SPI_INTR_RXOI
+#define QM_SS_SPI_INTR_STAT_RXFI QM_SS_SPI_INTR_RXFI
+
+#define QM_SS_SPI_INTR_MASK_TXEI QM_SS_SPI_INTR_TXEI
+#define QM_SS_SPI_INTR_MASK_TXOI QM_SS_SPI_INTR_TXOI
+#define QM_SS_SPI_INTR_MASK_RXUI QM_SS_SPI_INTR_RXUI
+#define QM_SS_SPI_INTR_MASK_RXOI QM_SS_SPI_INTR_RXOI
+#define QM_SS_SPI_INTR_MASK_RXFI QM_SS_SPI_INTR_RXFI
+
+#define QM_SS_SPI_CLR_INTR_TXEI QM_SS_SPI_INTR_TXEI
+#define QM_SS_SPI_CLR_INTR_TXOI QM_SS_SPI_INTR_TXOI
+#define QM_SS_SPI_CLR_INTR_RXUI QM_SS_SPI_INTR_RXUI
+#define QM_SS_SPI_CLR_INTR_RXOI QM_SS_SPI_INTR_RXOI
+#define QM_SS_SPI_CLR_INTR_RXFI QM_SS_SPI_INTR_RXFI
+
+#define QM_SS_SPI_DR_DR_OFFS (0)
+#define QM_SS_SPI_DR_DR_MASK (0x0000FFFF)
+#define QM_SS_SPI_DR_WR BIT(30)
+#define QM_SS_SPI_DR_STROBE BIT(31)
+#define QM_SS_SPI_DR_W_MASK (0xc0000000)
+#define QM_SS_SPI_DR_R_MASK (0x80000000)
+
+/** @} */
+/** @} */
+
+#endif /* __SENSOR_REGISTERS_H__ */
diff --git a/libraries/CuriePowerManagement/src/qmsi/qm_soc_interrupts.h b/libraries/CuriePowerManagement/src/qmsi/qm_soc_interrupts.h
new file mode 100644
index 00000000..9acd4c35
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/qmsi/qm_soc_interrupts.h
@@ -0,0 +1,410 @@
+/*
+ * Copyright (c) 2016, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived from this
+ *    software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE INTEL CORPORATION OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __QM_SOC_INTERRUPTS_H__
+#define __QM_SOC_INTERRUPTS_H__
+
+/**
+ * Quark SE SoC Interrupts.
+ *
+ * @defgroup groupQUARKSESEINT SoC Interrupts (SE)
+ * @{
+ */
+
+#if (QM_LAKEMONT)
+
+/* x86 internal interrupt vectors. */
+#define QM_X86_DIVIDE_ERROR_INT (0)
+#define QM_X86_DEBUG_EXCEPTION_INT (1)
+#define QM_X86_NMI_INTERRUPT_INT (2)
+#define QM_X86_BREAKPOINT_INT (3)
+#define QM_X86_OVERFLOW_INT (4)
+#define QM_X86_BOUND_RANGE_EXCEEDED_INT (5)
+#define QM_X86_INVALID_OPCODE_INT (6)
+#define QM_X86_DEVICE_NOT_AVAILABLE_INT (7)
+#define QM_X86_DOUBLE_FAULT_INT (8)
+#define QM_X86_INTEL_RESERVED_09_INT (9)
+#define QM_X86_INVALID_TSS_INT (10)
+#define QM_X86_SEGMENT_NOT_PRESENT_INT (11)
+#define QM_X86_STACK_SEGMENT_FAULT_INT (12)
+#define QM_X86_GENERAL_PROTECT_FAULT_INT (13)
+#define QM_X86_PAGE_FAULT_INT (14)
+#define QM_X86_INTEL_RESERVED_15_INT (15)
+#define QM_X86_FLOATING_POINT_ERROR_INT (16)
+#define QM_X86_ALIGNMENT_CHECK_INT (17)
+#define QM_X86_INTEL_RESERVED_18_INT (18)
+#define QM_X86_INTEL_RESERVED_19_INT (19)
+#define QM_X86_INTEL_RESERVED_20_INT (20)
+#define QM_X86_INTEL_RESERVED_21_INT (21)
+#define QM_X86_INTEL_RESERVED_22_INT (22)
+#define QM_X86_INTEL_RESERVED_23_INT (23)
+#define QM_X86_INTEL_RESERVED_24_INT (24)
+#define QM_X86_INTEL_RESERVED_25_INT (25)
+#define QM_X86_INTEL_RESERVED_26_INT (26)
+#define QM_X86_INTEL_RESERVED_27_INT (27)
+#define QM_X86_INTEL_RESERVED_28_INT (28)
+#define QM_X86_INTEL_RESERVED_29_INT (29)
+#define QM_X86_INTEL_RESERVED_30_INT (30)
+#define QM_X86_INTEL_RESERVED_31_INT (31)
+
+#define QM_X86_PIC_TIMER_INT_VECTOR (32)
+
+#endif /* QM_LAKEMONT */
+
+#if (QM_SENSOR)
+
+/* ARC EM processor internal interrupt vector assignments. */
+#define QM_ARC_RESET_INT (0)
+#define QM_ARC_MEMORY_ERROR_INT (1)
+#define QM_ARC_INSTRUCTION_ERROR_INT (2)
+#define QM_ARC_MACHINE_CHECK_EXCEPTION_INT (3)
+#define QM_ARC_INSTRUCTION_TLB_MISS_INT (4)
+#define QM_ARC_DATA_TLB_MISS_INT (5)
+#define QM_ARC_PROTECTION_VIOLATION_INT (6)
+#define QM_ARC_PRIVILEGE_VIOLATION_INT (7)
+#define QM_ARC_SOFTWARE_INTERRUPT_INT (8)
+#define QM_ARC_TRAP_INT (9)
+#define QM_ARC_EXTENSION_INSTRUCTION_EXCEPTION_INT (10)
+#define QM_ARC_DIVIDE_BY_ZERO_INT (11)
+#define QM_ARC_DATA_CACHE_CONSISTENCY_ERROR_INT (12)
+#define QM_ARC_MISALIGNED_DATA_ACCESS_INT (13)
+#define QM_ARC_RESERVED_14_INT (14)
+#define QM_ARC_RESERVED_15_INT (15)
+#define QM_ARC_TIMER_0_INT (16)
+#define QM_ARC_TIMER_1_INT (17)
+
+#endif /* QM_SENSOR */
+
+#if (QM_SENSOR)
+/**
+ * Sensor Sub-System Specific IRQs and interrupt vectors.
+ *
+ * @name SS Interrupt
+ * @{
+ */
+
+#define QM_SS_EXCEPTION_NUM (16)  /* Exceptions and traps in ARC EM core. */
+#define QM_SS_INT_TIMER_NUM (2)   /* Internal interrupts in ARC EM core. */
+#define QM_SS_IRQ_SENSOR_NUM (18) /* IRQ's from the Sensor Subsystem. */
+#define QM_SS_IRQ_COMMON_NUM (32) /* IRQ's from the common SoC fabric. */
+#define QM_SS_INT_VECTOR_NUM                                                   \
+	(QM_SS_EXCEPTION_NUM + QM_SS_INT_TIMER_NUM + QM_SS_IRQ_SENSOR_NUM +    \
+	 QM_SS_IRQ_COMMON_NUM)
+#define QM_SS_IRQ_NUM (QM_SS_IRQ_SENSOR_NUM + QM_SS_IRQ_COMMON_NUM)
+
+/*
+ * The following definitions are Sensor Subsystem interrupt irq and vector
+ * numbers:
+ * #define QM_SS_xxx          - irq number
+ * #define QM_SS_xxx_VECTOR   - vector number
+ */
+
+/** Sensor Subsystem ADC Rx Fifo Error Interrupt. */
+#define QM_SS_IRQ_ADC_0_ERROR_INT 0
+#define QM_SS_IRQ_ADC_0_ERROR_INT_VECTOR 18
+
+/** Sensor Subsystem ADC Data Available Interrupt. */
+#define QM_SS_IRQ_ADC_0_INT 1
+#define QM_SS_IRQ_ADC_0_INT_VECTOR 19
+
+/** Sensor Subsystem GPIO Single Interrupt 0 */
+#define QM_SS_IRQ_GPIO_0_INT 2
+#define QM_SS_IRQ_GPIO_0_INT_VECTOR 20
+
+/** Sensor Subsystem GPIO Single Interrupt 1. */
+#define QM_SS_IRQ_GPIO_1_INT 3
+#define QM_SS_IRQ_GPIO_1_INT_VECTOR 21
+
+/** Sensor Subsystem I2C 0 Error Interrupt. */
+#define QM_SS_IRQ_I2C_0_ERROR_INT 4
+#define QM_SS_IRQ_I2C_0_ERROR_INT_VECTOR 22
+
+/** Sensor Subsystem I2C 0 Data Available Interrupt. */
+#define QM_SS_IRQ_I2C_0_RX_AVAIL_INT 5
+#define QM_SS_IRQ_I2C_0_RX_AVAIL_INT_VECTOR 23
+
+/** Sensor Subsystem I2C 0 Data Required Interrupt. */
+#define QM_SS_IRQ_I2C_0_TX_REQ_INT 6
+#define QM_SS_IRQ_I2C_0_TX_REQ_INT_VECTOR 24
+
+/** Sensor Subsystem I2C 0 Stop Detect Interrupt. */
+#define QM_SS_IRQ_I2C_0_STOP_DET_INT 7
+#define QM_SS_IRQ_I2C_0_STOP_DET_INT_VECTOR 25
+
+/** Sensor Subsystem I2C 1 Error Interrupt. */
+#define QM_SS_IRQ_I2C_1_ERROR_INT 8
+#define QM_SS_IRQ_I2C_1_ERROR_INT_VECTOR 26
+
+/** Sensor Subsystem I2C 1 Data Available Interrupt. */
+#define QM_SS_IRQ_I2C_1_RX_AVAIL_INT 9
+#define QM_SS_IRQ_I2C_1_RX_AVAIL_INT_VECTOR 27
+
+/** Sensor Subsystem I2C 1 Data Required Interrupt. */
+#define QM_SS_IRQ_I2C_1_TX_REQ_INT 10
+#define QM_SS_IRQ_I2C_1_TX_REQ_INT_VECTOR 28
+
+/** Sensor Subsystem I2C 1 Stop Detect Interrupt. */
+#define QM_SS_IRQ_I2C_1_STOP_DET_INT 11
+#define QM_SS_IRQ_I2C_1_STOP_DET_INT_VECTOR 29
+
+/** Sensor Subsystem SPI 0 Error Interrupt. */
+#define QM_SS_IRQ_SPI_0_ERROR_INT 12
+#define QM_SS_IRQ_SPI_0_ERROR_INT_VECTOR 30
+
+/** Sensor Subsystem SPI 0 Data Available Interrupt. */
+#define QM_SS_IRQ_SPI_0_RX_AVAIL_INT 13
+#define QM_SS_IRQ_SPI_0_RX_AVAIL_INT_VECTOR 31
+
+/** Sensor Subsystem SPI 0 Data Required Interrupt. */
+#define QM_SS_IRQ_SPI_0_TX_REQ_INT 14
+#define QM_SS_IRQ_SPI_0_TX_REQ_INT_VECTOR 32
+
+/** Sensor Subsystem SPI 1 Error Interrupt. */
+#define QM_SS_IRQ_SPI_1_ERROR_INT 15
+#define QM_SS_IRQ_SPI_1_ERROR_INT_VECTOR 33
+
+/** Sensor Subsystem SPI 1 Data Available Interrupt. */
+#define QM_SS_IRQ_SPI_1_RX_AVAIL_INT 16
+#define QM_SS_IRQ_SPI_1_RX_AVAIL_INT_VECTOR 34
+
+/** Sensor Subsystem SPI 1 Data Required Interrupt. */
+#define QM_SS_IRQ_SPI_1_TX_REQ_INT 17
+#define QM_SS_IRQ_SPI_1_TX_REQ_INT_VECTOR 35
+
+typedef enum {
+	QM_SS_INT_PRIORITY_0 = 0,
+	QM_SS_INT_PRIORITY_1 = 1,
+	QM_SS_INT_PRIORITY_15 = 15,
+	QM_SS_INT_PRIORITY_NUM
+} qm_ss_irq_priority_t;
+
+typedef enum { QM_SS_INT_DISABLE = 0, QM_SS_INT_ENABLE = 1 } qm_ss_irq_mask_t;
+
+typedef enum {
+	QM_SS_IRQ_LEVEL_SENSITIVE = 0,
+	QM_SS_IRQ_EDGE_SENSITIVE = 1
+} qm_ss_irq_trigger_t;
+
+#define QM_SS_AUX_IRQ_CTRL (0xE)
+#define QM_SS_AUX_IRQ_HINT (0x201)
+#define QM_SS_AUX_IRQ_PRIORITY (0x206)
+#define QM_SS_AUX_IRQ_STATUS (0x406)
+#define QM_SS_AUX_IRQ_SELECT (0x40B)
+#define QM_SS_AUX_IRQ_ENABLE (0x40C)
+#define QM_SS_AUX_IRQ_TRIGGER (0x40D)
+
+/** @} */
+
+#endif /* QM_SENSOR */
+
+/**
+ * @name Common SoC IRQs and Interrupts
+ * @{
+ */
+
+/* IRQs and interrupt vectors.
+ *
+ * Any IRQ > 1 actually has a event router mask register offset of +1.
+ * The vector numbers must be defined without arithmetic expressions nor
+ * parentheses because they are expanded as token concatenation.
+ */
+
+/** I2C Master 0 Single Interrupt. */
+#define QM_IRQ_I2C_0_INT 0
+#define QM_IRQ_I2C_0_INT_MASK_OFFSET 0
+#define QM_IRQ_I2C_0_INT_VECTOR 36
+
+/** I2C Master 1 Single Interrupt. */
+#define QM_IRQ_I2C_1_INT 1
+#define QM_IRQ_I2C_1_INT_MASK_OFFSET 1
+#define QM_IRQ_I2C_1_INT_VECTOR 37
+
+/** SPI Master 0 Single Interrupt. */
+#define QM_IRQ_SPI_MASTER_0_INT 2
+#define QM_IRQ_SPI_MASTER_0_INT_MASK_OFFSET 3
+#define QM_IRQ_SPI_MASTER_0_INT_VECTOR 38
+
+/** SPI Master 1 Single Interrupt. */
+#define QM_IRQ_SPI_MASTER_1_INT 3
+#define QM_IRQ_SPI_MASTER_1_INT_MASK_OFFSET 4
+#define QM_IRQ_SPI_MASTER_1_INT_VECTOR 39
+
+/** SPI Slave Single Interrupt. */
+#define QM_IRQ_SPI_SLAVE_0_INT 4
+#define QM_IRQ_SPI_SLAVE_0_INT_MASK_OFFSET 5
+#define QM_IRQ_SPI_SLAVE_0_INT_VECTOR 40
+
+/** UART 0 Single Interrupt. */
+#define QM_IRQ_UART_0_INT 5
+#define QM_IRQ_UART_0_INT_MASK_OFFSET 6
+#define QM_IRQ_UART_0_INT_VECTOR 41
+
+/** UART 1 Single Interrupt. */
+#define QM_IRQ_UART_1_INT 6
+#define QM_IRQ_UART_1_INT_MASK_OFFSET 7
+#define QM_IRQ_UART_1_INT_VECTOR 42
+
+/** I2S Single Interrupt. */
+#define QM_IRQ_I2S_0_INT 7
+#define QM_IRQ_I2S_0_INT_MASK_OFFSET 8
+#define QM_IRQ_I2S_0_INT_VECTOR 43
+
+/** GPIO Single Interrupt. */
+#define QM_IRQ_GPIO_0_INT 8
+#define QM_IRQ_GPIO_0_INT_MASK_OFFSET 9
+#define QM_IRQ_GPIO_0_INT_VECTOR 44
+
+/** PWM/Timer Single Interrupt. */
+#define QM_IRQ_PWM_0_INT 9
+#define QM_IRQ_PWM_0_INT_MASK_OFFSET 10
+#define QM_IRQ_PWM_0_INT_VECTOR 45
+
+/** USB Single Interrupt. */
+#define QM_IRQ_USB_0_INT (10)
+#define QM_IRQ_USB_0_INT_MASK_OFFSET (11)
+#define QM_IRQ_USB_0_INT_VECTOR 46
+
+/** RTC Single Interrupt. */
+#define QM_IRQ_RTC_0_INT 11
+#define QM_IRQ_RTC_0_INT_MASK_OFFSET 12
+#define QM_IRQ_RTC_0_INT_VECTOR 47
+
+/** WDT Single Interrupt. */
+#define QM_IRQ_WDT_0_INT 12
+#define QM_IRQ_WDT_0_INT_MASK_OFFSET 13
+#define QM_IRQ_WDT_0_INT_VECTOR 48
+
+/** DMA Channel 0 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_0 13
+#define QM_IRQ_DMA_0_INT_0_MASK_OFFSET 14
+#define QM_IRQ_DMA_0_INT_0_VECTOR 49
+
+/** DMA Channel 1 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_1 14
+#define QM_IRQ_DMA_0_INT_1_MASK_OFFSET 15
+#define QM_IRQ_DMA_0_INT_1_VECTOR 50
+
+/** DMA Channel 2 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_2 15
+#define QM_IRQ_DMA_0_INT_2_MASK_OFFSET 16
+#define QM_IRQ_DMA_0_INT_2_VECTOR 51
+
+/** DMA Channel 3 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_3 16
+#define QM_IRQ_DMA_0_INT_3_MASK_OFFSET 17
+#define QM_IRQ_DMA_0_INT_3_VECTOR 52
+
+/** DMA Channel 4 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_4 17
+#define QM_IRQ_DMA_0_INT_4_MASK_OFFSET 18
+#define QM_IRQ_DMA_0_INT_4_VECTOR 53
+
+/** DMA Channel 5 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_5 18
+#define QM_IRQ_DMA_0_INT_5_MASK_OFFSET 19
+#define QM_IRQ_DMA_0_INT_5_VECTOR 54
+
+/** DMA Channel 6 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_6 19
+#define QM_IRQ_DMA_0_INT_6_MASK_OFFSET 20
+#define QM_IRQ_DMA_0_INT_6_VECTOR 55
+
+/** DMA Channel 7 Single Interrupt. */
+#define QM_IRQ_DMA_0_INT_7 20
+#define QM_IRQ_DMA_0_INT_7_MASK_OFFSET 21
+#define QM_IRQ_DMA_0_INT_7_VECTOR 56
+
+/**
+ * 8 Mailbox Channel Interrupts Routed to Single Interrupt
+ * with 8bit Mask per Destination.
+ */
+#define QM_IRQ_MAILBOX_0_INT 21
+#define QM_IRQ_MAILBOX_0_INT_MASK_OFFSET 22
+#define QM_IRQ_MAILBOX_0_INT_VECTOR 57
+
+/**
+ * 19 Comparators Routed to Single Interrupt with 19bit Mask per Destination.
+ */
+#define QM_IRQ_COMPARATOR_0_INT 22
+#define QM_IRQ_COMPARATOR_0_INT_MASK_OFFSET 26
+#define QM_IRQ_COMPARATOR_0_INT_VECTOR 58
+
+/** System and Power Management Single Interrupt. */
+#define QM_IRQ_PMU_0_INT 23
+#define QM_IRQ_PMU_0_INT_MASK_OFFSET 26
+#define QM_IRQ_PMU_0_INT_VECTOR 58
+
+/**
+ * 8 DMA Channel Error Interrupts Routed to Single Interrupt with 8bit Mask
+ * per Destination.
+ */
+#define QM_IRQ_DMA_0_ERROR_INT 24
+#define QM_IRQ_DMA_0_ERROR_INT_MASK_OFFSET 28
+#define QM_IRQ_DMA_0_ERROR_INT_VECTOR 60
+
+/** Internal SRAM Memory Protection Error Single Interrupt. */
+#define QM_IRQ_SRAM_MPR_0_INT 25
+#define QM_IRQ_SRAM_MPR_0_INT_MASK_OFFSET 29
+#define QM_IRQ_SRAM_MPR_0_INT_VECTOR 61
+
+/** Internal Flash Controller 0 Memory Protection Error Single Interrupt. */
+#define QM_IRQ_FLASH_MPR_0_INT 26
+#define QM_IRQ_FLASH_MPR_0_INT_MASK_OFFSET 30
+#define QM_IRQ_FLASH_MPR_0_INT_VECTOR 62
+
+/** Internal Flash Controller 1 Memory Protection Error Single Interrupt. */
+#define QM_IRQ_FLASH_MPR_1_INT 27
+#define QM_IRQ_FLASH_MPR_1_INT_MASK_OFFSET 31
+#define QM_IRQ_FLASH_MPR_1_INT_VECTOR 63
+
+/** Always-On Timer Interrupt. */
+#define QM_IRQ_AONPT_0_INT 28
+#define QM_IRQ_AONPT_0_INT_MASK_OFFSET 32
+#define QM_IRQ_AONPT_0_INT_VECTOR 64
+
+/** ADC power sequence done. */
+#define QM_SS_IRQ_ADC_0_PWR_INT 29
+#define QM_SS_IRQ_ADC_0_PWR_INT_MASK_OFFSET 33
+#define QM_SS_IRQ_ADC_0_PWR_INT_VECTOR 65
+
+/** ADC calibration done. */
+#define QM_SS_IRQ_ADC_0_CAL_INT 30
+#define QM_SS_IRQ_ADC_0_CAL_INT_MASK_OFFSET 34
+#define QM_SS_IRQ_ADC_0_CAL_INT_VECTOR 66
+
+/** Always-On GPIO Interrupt. */
+#define QM_IRQ_AON_GPIO_0_INT 31
+#define QM_IRQ_AON_GPIO_0_INT_MASK_OFFSET 35
+#define QM_IRQ_AON_GPIO_0_INT_VECTOR 67
+
+/** @} */
+
+/** @} */
+
+#endif /* __QM_SOC_INTERRUPTS_H__ */
diff --git a/libraries/CuriePowerManagement/src/qmsi/qm_soc_regs.h b/libraries/CuriePowerManagement/src/qmsi/qm_soc_regs.h
new file mode 100644
index 00000000..54887384
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/qmsi/qm_soc_regs.h
@@ -0,0 +1,2105 @@
+/*
+ * Copyright (c) 2016, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived from this
+ *    software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE INTEL CORPORATION OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __REGISTERS_H__
+#define __REGISTERS_H__
+
+#include "qm_common.h"
+#include "qm_soc_interrupts.h"
+#include "qm_interrupt_router_regs.h"
+
+/**
+ * Quark SE SoC Registers.
+ *
+ * @defgroup groupQUARKSESEREG SoC Registers (SE)
+ * @{
+ */
+
+#define QUARK_SE (1)
+#define HAS_4_TIMERS (1)
+#define HAS_AON_GPIO (1)
+#define HAS_MAILBOX (1)
+#define HAS_USB (1)
+
+#if !defined(QM_SENSOR)
+#define HAS_APIC (1)
+#endif
+
+/**
+ * @name System Core
+ * @{
+ */
+
+/** System Core register map. */
+typedef struct {
+	QM_RW uint32_t osc0_cfg0;    /**< Hybrid Oscillator Configuration 0. */
+	QM_RW uint32_t osc0_stat1;   /**< Hybrid Oscillator status 1. */
+	QM_RW uint32_t osc0_cfg1;    /**< Hybrid Oscillator configuration 1. */
+	QM_RW uint32_t osc1_stat0;   /**< RTC Oscillator status 0. */
+	QM_RW uint32_t osc1_cfg0;    /**< RTC Oscillator Configuration 0. */
+	QM_RW uint32_t usb_pll_cfg0; /**< USB Phase lock look configuration. */
+	QM_RW uint32_t
+	    ccu_periph_clk_gate_ctl; /**< Peripheral Clock Gate Control. */
+	QM_RW uint32_t
+	    ccu_periph_clk_div_ctl0; /**< Peripheral Clock Divider Control. 0 */
+	QM_RW uint32_t
+	    ccu_gpio_db_clk_ctl; /**< Peripheral Clock Divider Control 1. */
+	QM_RW uint32_t
+	    ccu_ext_clock_ctl; /**< External Clock Control Register. */
+	/** Sensor Subsystem peripheral clock gate control. */
+	QM_RW uint32_t ccu_ss_periph_clk_gate_ctl;
+	QM_RW uint32_t ccu_lp_clk_ctl; /**< System Low Power Clock Control. */
+	QM_RW uint32_t reserved;
+	QM_RW uint32_t ccu_mlayer_ahb_ctl; /**< AHB Control Register. */
+	QM_RW uint32_t ccu_sys_clk_ctl; /**< System Clock Control Register. */
+	QM_RW uint32_t osc_lock_0;      /**< Clocks Lock Register. */
+} qm_scss_ccu_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_ccu_reg_t test_scss_ccu;
+#define QM_SCSS_CCU ((qm_scss_ccu_reg_t *)(&test_scss_ccu))
+
+#else
+#define QM_SCSS_CCU_BASE (0xB0800000)
+#define QM_SCSS_CCU ((qm_scss_ccu_reg_t *)QM_SCSS_CCU_BASE)
+#endif
+
+/* Hybrid oscillator output select select (0=Silicon, 1=Crystal) */
+#define QM_OSC0_MODE_SEL BIT(3)
+#define QM_OSC0_PD BIT(2)
+#define QM_OSC1_PD BIT(1)
+
+/* Enable Crystal oscillator. */
+#define QM_OSC0_EN_CRYSTAL BIT(0)
+
+/* Crystal oscillator parameters. */
+#define OSC0_CFG1_OSC0_FADJ_XTAL_MASK (0x000F0000)
+#define OSC0_CFG1_OSC0_FADJ_XTAL_OFFS (16)
+#define OSC0_CFG0_OSC0_XTAL_COUNT_VALUE_MASK (0x00600000)
+#define OSC0_CFG0_OSC0_XTAL_COUNT_VALUE_OFFS (21)
+
+/* Silicon Oscillator parameters. */
+#define OSC0_CFG1_FTRIMOTP_MASK (0x3FF00000)
+#define OSC0_CFG1_FTRIMOTP_OFFS (20)
+#define OSC0_CFG1_SI_FREQ_SEL_MASK (0x00000300)
+#define OSC0_CFG1_SI_FREQ_SEL_OFFS (8)
+
+#define QM_OSC0_MODE_SEL BIT(3)
+#define QM_OSC0_LOCK_SI BIT(0)
+#define QM_OSC0_LOCK_XTAL BIT(1)
+#define QM_OSC0_EN_SI_OSC BIT(1)
+
+#define QM_SI_OSC_1V2_MODE BIT(0)
+
+/* Peripheral clock divider control. */
+#define QM_CCU_PERIPH_PCLK_DIV_OFFSET (1)
+#define QM_CCU_PERIPH_PCLK_DIV_EN BIT(0)
+
+/* Clock enable / disable register. */
+#define QM_CCU_MLAYER_AHB_CTL (REG_VAL(0xB0800034))
+
+/* System clock control */
+#define QM_CCU_SYS_CLK_SEL BIT(0)
+#define QM_SCSS_CCU_SYS_CLK_SEL BIT(0)
+#define QM_SCSS_CCU_C2_LP_EN BIT(1)
+#define QM_SCSS_CCU_SS_LPS_EN BIT(0)
+#define QM_CCU_RTC_CLK_EN BIT(1)
+#define QM_CCU_RTC_CLK_DIV_EN BIT(2)
+#define QM_CCU_SYS_CLK_DIV_EN BIT(7)
+#define QM_CCU_SYS_CLK_DIV_MASK (0x00000300)
+
+#define QM_OSC0_SI_FREQ_SEL_DEF_MASK (0xFFFFFCFF)
+#define QM_CCU_GPIO_DB_DIV_OFFSET (2)
+#define QM_CCU_GPIO_DB_CLK_DIV_EN BIT(1)
+#define QM_CCU_GPIO_DB_CLK_EN BIT(0)
+#define QM_CCU_RTC_CLK_DIV_OFFSET (3)
+#define QM_CCU_SYS_CLK_DIV_OFFSET (8)
+#define QM_CCU_DMA_CLK_EN BIT(6)
+
+/** @} */
+
+/**
+ * @name General Purpose
+ * @{
+ */
+
+/** General Purpose register map. */
+typedef struct {
+	QM_RW uint32_t gps0; /**< General Purpose Sticky Register 0 */
+	QM_RW uint32_t gps1; /**< General Purpose Sticky Register 1 */
+	QM_RW uint32_t gps2; /**< General Purpose Sticky Register 2 */
+	QM_RW uint32_t gps3; /**< General Purpose Sticky Register 3 */
+	QM_RW uint32_t reserved;
+	QM_RW uint32_t gp0; /**< General Purpose Scratchpad Register 0 */
+	QM_RW uint32_t gp1; /**< General Purpose Scratchpad Register 1 */
+	QM_RW uint32_t gp2; /**< General Purpose Scratchpad Register 2 */
+	QM_RW uint32_t gp3; /**< General Purpose Scratchpad Register 3 */
+	QM_RW uint32_t reserved1;
+	QM_RW uint32_t id;    /**< Identification Register */
+	QM_RW uint32_t rev;   /**< Revision Register */
+	QM_RW uint32_t wo_sp; /**< Write-One-to-Set Scratchpad Register */
+	QM_RW uint32_t
+	    wo_st; /**< Write-One-to-Set Sticky Scratchpad Register */
+} qm_scss_gp_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_gp_reg_t test_scss_gp;
+#define QM_SCSS_GP ((qm_scss_gp_reg_t *)(&test_scss_gp))
+
+#else
+#define QM_SCSS_GP_BASE (0xB0800100)
+#define QM_SCSS_GP ((qm_scss_gp_reg_t *)QM_SCSS_GP_BASE)
+#endif
+
+/* The GPS0 register usage. */
+#define QM_GPS0_BIT_FM (0)	    /**< Start Firmware Manager. */
+#define QM_GPS0_BIT_X86_WAKEUP (1)    /**< Lakemont core reset type. */
+#define QM_GPS0_BIT_SENSOR_WAKEUP (2) /**< Sensor core reset type. */
+
+/** @} */
+
+/**
+ * @name Memory Control
+ * @{
+ */
+
+/** Memory Control register map. */
+typedef struct {
+	QM_RW uint32_t mem_ctrl; /**< Memory control */
+} qm_scss_mem_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_mem_reg_t test_scss_mem;
+#define QM_SCSS_MEM ((qm_scss_mem_reg_t *)(&test_scss_mem))
+
+#else
+#define QM_SCSS_MEM_BASE (0xB0800200)
+#define QM_SCSS_MEM ((qm_scss_mem_reg_t *)QM_SCSS_MEM_BASE)
+#endif
+
+/** @} */
+
+/**
+ * @name Comparator
+ * @{
+ */
+
+/** Comparator register map. */
+typedef struct {
+	QM_RW uint32_t cmp_en;      /**< Comparator enable. */
+	QM_RW uint32_t cmp_ref_sel; /**< Comparator reference select. */
+	QM_RW uint32_t
+	    cmp_ref_pol; /**< Comparator reference polarity select register. */
+	QM_RW uint32_t cmp_pwr; /**< Comparator power enable register. */
+	QM_RW uint32_t reserved[6];
+	QM_RW uint32_t cmp_stat_clr; /**< Comparator clear register. */
+} qm_scss_cmp_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_cmp_reg_t test_scss_cmp;
+#define QM_SCSS_CMP ((qm_scss_cmp_reg_t *)(&test_scss_cmp))
+
+#else
+#define QM_SCSS_CMP_BASE (0xB0800300)
+#define QM_SCSS_CMP ((qm_scss_cmp_reg_t *)QM_SCSS_CMP_BASE)
+#endif
+
+#define QM_AC_HP_COMPARATORS_MASK (0x7FFC0)
+
+/** @} */
+
+/**
+ * @name APIC
+ * @{
+ */
+
+typedef struct {
+	QM_RW uint32_t reg;
+	QM_RW uint32_t pad[3];
+} apic_reg_pad_t;
+
+/** APIC register block type. */
+typedef struct {
+	QM_RW apic_reg_pad_t reserved0[2];
+	QM_RW apic_reg_pad_t id;      /**< LAPIC ID */
+	QM_RW apic_reg_pad_t version; /**< LAPIC version*/
+	QM_RW apic_reg_pad_t reserved1[4];
+	QM_RW apic_reg_pad_t tpr;    /**< Task priority*/
+	QM_RW apic_reg_pad_t apr;    /**< Arbitration priority */
+	QM_RW apic_reg_pad_t ppr;    /**< Processor priority */
+	QM_RW apic_reg_pad_t eoi;    /**< End of interrupt */
+	QM_RW apic_reg_pad_t rrd;    /**< Remote read */
+	QM_RW apic_reg_pad_t ldr;    /**< Logical destination */
+	QM_RW apic_reg_pad_t dfr;    /**< Destination format */
+	QM_RW apic_reg_pad_t svr;    /**< Spurious vector */
+	QM_RW apic_reg_pad_t isr[8]; /**< In-service */
+	QM_RW apic_reg_pad_t tmr[8]; /**< Trigger mode */
+	QM_RW apic_reg_pad_t irr[8]; /**< Interrupt request */
+	QM_RW apic_reg_pad_t esr;    /**< Error status */
+	QM_RW apic_reg_pad_t reserved2[6];
+	QM_RW apic_reg_pad_t lvtcmci;   /**< Corrected Machine Check vector */
+	QM_RW apic_reg_pad_t icr[2];    /**< Interrupt command */
+	QM_RW apic_reg_pad_t lvttimer;  /**< Timer vector */
+	QM_RW apic_reg_pad_t lvtts;     /**< Thermal sensor vector */
+	QM_RW apic_reg_pad_t lvtpmcr;   /**< Perfmon counter vector */
+	QM_RW apic_reg_pad_t lvtlint0;  /**< Local interrupt 0 vector */
+	QM_RW apic_reg_pad_t lvtlint1;  /**< Local interrupt 1 vector */
+	QM_RW apic_reg_pad_t lvterr;    /**< Error vector */
+	QM_RW apic_reg_pad_t timer_icr; /**< Timer initial count */
+	QM_RW apic_reg_pad_t timer_ccr; /**< Timer current count */
+	QM_RW apic_reg_pad_t reserved3[4];
+	QM_RW apic_reg_pad_t timer_dcr; /**< Timer divide configuration */
+} qm_lapic_reg_t;
+
+#if (HAS_APIC)
+/*
+ * The size of IOAPIC redirection table, as returned by _ioapic_get_redtbl_size
+ * function.
+ */
+#define QM_IOAPIC_NUM_RTES (32)
+
+/**
+ * IRQ context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by
+ * qm_irq_save_context and qm_irq_restore_context functions.
+ */
+typedef struct {
+	/** Redirection Table Entries. */
+	uint32_t redtbl_entries[QM_IOAPIC_NUM_RTES];
+} qm_irq_context_t;
+#endif
+
+/**
+ * PIC TIMER context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by the qm_pic_timer_save_context
+ * and qm_pic_timer_restore_context functions.
+ */
+typedef struct {
+	uint32_t timer_icr; /**< Initial Count Register. */
+	uint32_t timer_dcr; /**< Divide Configuration Register. */
+	uint32_t lvttimer;  /**< Timer Entry in Local Vector Table. */
+} qm_pic_timer_context_t;
+
+#if (UNIT_TEST)
+qm_lapic_reg_t test_lapic;
+#define QM_LAPIC ((qm_lapic_reg_t *)(&test_lapic))
+
+#else
+/* Local APIC. */
+#define QM_LAPIC_BASE (0xFEE00000)
+#define QM_LAPIC ((qm_lapic_reg_t *)QM_LAPIC_BASE)
+#endif
+
+#define QM_INT_CONTROLLER QM_LAPIC
+
+/*
+ * Quark SE has a HW limitation that prevents a LAPIC EOI from being broadcast
+ * into IOAPIC. To trigger this manually we must write the vector number being
+ * serviced into the IOAPIC EOI register.
+ */
+#if defined(ENABLE_EXTERNAL_ISR_HANDLING) || defined(QM_SENSOR)
+#define QM_ISR_EOI(vector)
+#else
+#define QM_ISR_EOI(vector)                                                     \
+	do {                                                                   \
+		QM_INT_CONTROLLER->eoi.reg = 0;                                \
+		QM_IOAPIC->eoi.reg = vector;                                   \
+	} while (0)
+#endif
+
+typedef struct {
+	QM_RW apic_reg_pad_t ioregsel; /**< Register selector. */
+	QM_RW apic_reg_pad_t iowin;    /**< Register window. */
+	QM_RW apic_reg_pad_t reserved[2];
+	QM_RW apic_reg_pad_t eoi; /**< EOI register. */
+} qm_ioapic_reg_t;
+
+#define QM_IOAPIC_REG_VER (0x01)    /* IOAPIC version. */
+#define QM_IOAPIC_REG_REDTBL (0x10) /* Redirection table base. */
+
+#if (UNIT_TEST)
+qm_ioapic_reg_t test_ioapic;
+#define QM_IOAPIC ((qm_ioapic_reg_t *)(&test_ioapic))
+
+#else
+/* IO / APIC base address. */
+#define QM_IOAPIC_BASE (0xFEC00000)
+#define QM_IOAPIC ((qm_ioapic_reg_t *)QM_IOAPIC_BASE)
+#endif
+
+/** @} */
+
+/**
+ * @name Power Management
+ * @{
+ */
+
+/** Power Management register map. */
+typedef struct {
+	QM_RW uint32_t p_lvl2; /**< Processor level 2 */
+	QM_RW uint32_t reserved[4];
+	QM_RW uint32_t pm1c; /**< Power management 1 control */
+	QM_RW uint32_t reserved1[9];
+	QM_RW uint32_t aon_vr;     /**< AON Voltage Regulator */
+	QM_RW uint32_t plat3p3_vr; /**< Platform 3p3 voltage regulator */
+	QM_RW uint32_t plat1p8_vr; /**< Platform 1p8 voltage regulator */
+	QM_RW uint32_t host_vr;    /**< Host Voltage Regulator */
+	QM_RW uint32_t slp_cfg;    /**< Sleeping Configuration */
+	/** Power Management Network (PMNet) Control and Status */
+	QM_RW uint32_t pmnetcs;
+	QM_RW uint32_t pm_wait; /**< Power Management Wait */
+	QM_RW uint32_t reserved2;
+	QM_RW uint32_t p_sts; /**< Processor Status */
+	QM_RW uint32_t reserved3[3];
+	QM_RW uint32_t rstc; /**< Reset Control */
+	QM_RW uint32_t rsts; /**< Reset Status */
+	QM_RW uint32_t reserved4[6];
+	QM_RW uint32_t vr_lock; /**< Voltage regulator lock */
+	QM_RW uint32_t pm_lock; /**< Power Management Lock */
+} qm_scss_pmu_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_pmu_reg_t test_scss_pmu;
+#define QM_SCSS_PMU ((qm_scss_pmu_reg_t *)(&test_scss_pmu))
+
+#else
+#define QM_SCSS_PMU_BASE (0xB0800504)
+#define QM_SCSS_PMU ((qm_scss_pmu_reg_t *)QM_SCSS_PMU_BASE)
+#endif
+
+#define QM_SS_CFG_ARC_RUN_REQ_A BIT(24)
+#define QM_P_STS_HALT_INTERRUPT_REDIRECTION BIT(26)
+#define QM_P_STS_ARC_HALT BIT(14)
+
+#define QM_AON_VR_VSEL_MASK (0xFFE0)
+#define QM_AON_VR_VSEL_1V2 (0x8)
+#define QM_AON_VR_VSEL_1V35 (0xB)
+#define QM_AON_VR_VSEL_1V8 (0x10)
+#define QM_AON_VR_EN BIT(7)
+#define QM_AON_VR_VSTRB BIT(5)
+
+#define QM_SCSS_SLP_CFG_LPMODE_EN BIT(8)
+#define QM_SCSS_SLP_CFG_RTC_DIS BIT(7)
+#define QM_SCSS_PM1C_SLPEN BIT(13)
+#define QM_SCSS_HOST_VR_EN BIT(7)
+#define QM_SCSS_PLAT3P3_VR_EN BIT(7)
+#define QM_SCSS_PLAT1P8_VR_EN BIT(7)
+#define QM_SCSS_HOST_VR_VREG_SEL BIT(6)
+#define QM_SCSS_PLAT3P3_VR_VREG_SEL BIT(6)
+#define QM_SCSS_PLAT1P8_VR_VREG_SEL BIT(6)
+#define QM_SCSS_VR_ROK BIT(10)
+#define QM_SCSS_VR_EN BIT(7)
+#define QM_SCSS_VR_VREG_SEL BIT(6)
+
+/** @} */
+
+/**
+ * @name Sensor Subsystem
+ * @{
+ */
+
+/** Sensor Subsystem register map. */
+typedef struct {
+	QM_RW uint32_t ss_cfg; /**< Sensor Subsystem Configuration */
+	QM_RW uint32_t ss_sts; /**< Sensor Subsystem status */
+} qm_scss_ss_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_ss_reg_t test_scss_ss;
+#define QM_SCSS_SS ((qm_scss_ss_reg_t *)(&test_scss_ss))
+
+#else
+#define QM_SCSS_SS_BASE (0xB0800600)
+#define QM_SCSS_SS ((qm_scss_ss_reg_t *)QM_SCSS_SS_BASE)
+#endif
+
+#define QM_SS_STS_HALT_INTERRUPT_REDIRECTION BIT(26)
+
+/** @} */
+
+/**
+ * @name Always-on Counters.
+ * @{
+ */
+
+/** Number of Always-on counter controllers. */
+typedef enum { QM_AONC_0 = 0, QM_AONC_NUM } qm_aonc_t;
+
+/** Always-on Counter Controller register map. */
+typedef struct {
+	QM_RW uint32_t aonc_cnt;  /**< Always-on counter register. */
+	QM_RW uint32_t aonc_cfg;  /**< Always-on counter enable. */
+	QM_RW uint32_t aonpt_cnt; /**< Always-on periodic timer. */
+	QM_RW uint32_t
+	    aonpt_stat; /**< Always-on periodic timer status register. */
+	QM_RW uint32_t aonpt_ctrl; /**< Always-on periodic timer control. */
+	QM_RW uint32_t
+	    aonpt_cfg; /**< Always-on periodic timer configuration register. */
+} qm_aonc_reg_t;
+
+#if (UNIT_TEST)
+qm_aonc_reg_t test_aonc;
+#define QM_AONC ((qm_aonc_reg_t *)(&test_aonc))
+
+#else
+#define QM_AONC_BASE (0xB0800700)
+#define QM_AONC ((qm_aonc_reg_t *)QM_AONC_BASE)
+#endif
+
+/** @} */
+
+/**
+ * @name Peripheral Registers
+ * @{
+ */
+
+/** Peripheral Registers register map. */
+typedef struct {
+	QM_RW uint32_t usb_phy_cfg0; /**< USB Configuration */
+	QM_RW uint32_t periph_cfg0;  /**< Peripheral Configuration */
+	QM_RW uint32_t reserved[2];
+	QM_RW uint32_t cfg_lock; /**< Configuration Lock */
+} qm_scss_peripheral_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_peripheral_reg_t test_scss_peripheral;
+#define QM_SCSS_PERIPHERAL ((qm_scss_peripheral_reg_t *)(&test_scss_peripheral))
+
+#else
+#define QM_SCSS_PERIPHERAL_BASE (0xB0800800)
+#define QM_SCSS_PERIPHERAL ((qm_scss_peripheral_reg_t *)QM_SCSS_PERIPHERAL_BASE)
+#endif
+
+/** @} */
+
+/**
+ * @name Pin MUX
+ * @{
+ */
+
+/** Pin MUX register map. */
+typedef struct {
+	QM_RW uint32_t pmux_pullup[4]; /**< Pin Mux Pullup */
+	QM_RW uint32_t pmux_slew[4];   /**< Pin Mux Slew Rate */
+	QM_RW uint32_t pmux_in_en[4];  /**< Pin Mux Input Enable */
+	QM_RW uint32_t pmux_sel[5];    /**< Pin Mux Select */
+	QM_RW uint32_t reserved[2];
+	QM_RW uint32_t pmux_pullup_lock; /**< Pin Mux Pullup Lock */
+	QM_RW uint32_t pmux_slew_lock;   /**< Pin Mux Slew Rate Lock */
+	QM_RW uint32_t pmux_sel_lock[3]; /**< Pin Mux Select Lock */
+	QM_RW uint32_t pmux_in_en_lock;  /**< Pin Mux Slew Rate Lock */
+} qm_scss_pmux_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_pmux_reg_t test_scss_pmux;
+#define QM_SCSS_PMUX ((qm_scss_pmux_reg_t *)(&test_scss_pmux))
+
+#else
+#define QM_SCSS_PMUX_BASE (0xB0800900)
+#define QM_SCSS_PMUX ((qm_scss_pmux_reg_t *)QM_SCSS_PMUX_BASE)
+#endif
+
+/* Pin MUX slew rate registers and settings */
+#define QM_PMUX_SLEW_4MA_DRIVER (0xFFFFFFFF)
+#define QM_PMUX_SLEW0 (REG_VAL(0xB0800910))
+#define QM_PMUX_SLEW1 (REG_VAL(0xB0800914))
+#define QM_PMUX_SLEW2 (REG_VAL(0xB0800918))
+#define QM_PMUX_SLEW3 (REG_VAL(0xB080091C))
+
+/** @} */
+
+/**
+ * @name ID
+ * @{
+ */
+
+/** Information register map. */
+typedef struct {
+	QM_RW uint32_t id;
+} qm_scss_info_reg_t;
+
+#if (UNIT_TEST)
+qm_scss_info_reg_t test_scss_info;
+#define QM_SCSS_INFO ((qm_scss_info_reg_t *)(&test_scss_info))
+
+#else
+#define QM_SCSS_INFO_BASE (0xB0801000)
+#define QM_SCSS_INFO ((qm_scss_info_reg_t *)QM_SCSS_INFO_BASE)
+#endif
+
+/** @} */
+
+/**
+ * @name Mailbox
+ * @{
+ */
+
+#define HAS_MAILBOX (1)
+#define NUM_MAILBOXES (8)
+
+#define HAS_MAILBOX_LAKEMONT_DEST (1)
+#define HAS_MAILBOX_SENSOR_SUB_SYSTEM_DEST (1)
+
+/**
+ * Mailbox MBOX_CH_CTRL_N Mailbox Channel Control Word Register
+ *
+ * 31    RW/1S/V  MBOX_CH_CTRL_INT  Mailbox Channel Control Word interrupt
+ * 30:0  RW       MBOX_CH_CTRL      Mailbox Channel Control Word
+ */
+#define QM_MBOX_CH_CTRL_INT BIT(31)
+#define QM_MBOX_CH_CTRL_MASK (0x7FFFFFFF)
+#define QM_MBOX_CH_CTRL_SHIFT (0)
+
+/*
+ * Mailbox Channel Status MBOX_CH_STS_N
+ *
+ * 31:2  RO       reserved
+ * 1     RW/1C/V  MBOX_CH_STS_CTRL_INT  Mailbox Channel Interrupt Status
+ *                - Bit set when message sent, indicates pending interrupt
+ *                - Bit set when a mailbox channel interrupt is pending..
+ *                - Bit cleared by writing 1
+ *                - Bit should be cleared by the receivers isr
+ * 0     RW/1C/V  MBOX_CH_STS           Mailbox Channel Status
+ *                - Bit set when message sent, indicates pending data
+ *                - Bit cleared by writing 1
+ *                - Bit should be cleared by the receiver after
+ *                  consuming the message.
+ */
+#define QM_MBOX_CH_STS_CTRL_INT BIT(1)
+#define QM_MBOX_CH_STS BIT(0)
+
+#define QM_MBOX_STATUS_MASK (QM_MBOX_CH_STS | QM_MBOX_CH_STS_CTRL_INT)
+
+/**
+ * Mailbox MBOX_CHALL_STS Channel Status Bits Register
+ *
+ * 31:16  RO    reserved
+ * 15:0   RO/V  MBOX_CHALL_STS Channel Status Bits
+ */
+#define QM_MBOX_CHALL_STS(N) BIT((N * 2))
+#define QM_MBOX_CHALL_INT_STS(N) BIT((N * 2) + 1)
+
+/**
+ * Mailbox interrupt routing mask register INT_MAILBOX_MASK
+ *
+ * There is only 1 Mailbox interrupt mask register.
+ * The register contains masks for all 8 mailbox channels.
+ *
+ * Note that the Mailbox interrupt mask register does not follow
+ * the same layout as most other interrupt mask registers in the SCSS.
+ *
+ * Mask bit positions for INT_MAILBOX_MASK are listed here:
+ *
+ * 31:24  RW/P/L  INT_MAILBOX_SS_HALT_MASK    Mailbox SS Halt interrupt mask
+ * 23:16  RW/P/L  INT_MAILBOX_HOST_HALT_MASK  Mailbox Host Halt interrupt mask
+ * 15:8   RW/P/L  INT_MAILBOX_SS_MASK         Mailbox SS interrupt mask
+ * 7:0    RW/P/L  INT_MAILBOX_HOST_MASK       Mailbox Host interrupt mask
+ */
+#define QM_MBOX_SS_HALT_MASK_OFFSET (24)
+#define QM_MBOX_SS_HALT_MASK_MASK (0xFF000000)
+#define QM_MBOX_HOST_HALT_MASK_OFFSET (16)
+#define QM_MBOX_HOST_HALT_MASK_MASK (0x00FF0000)
+#define QM_MBOX_SS_MASK_OFFSET (8)
+#define QM_MBOX_SS_MASK_MASK (0x0000FF00)
+#define QM_MBOX_HOST_MASK_OFFSET (0)
+#define QM_MBOX_HOST_MASK_MASK (0x000000FF)
+
+/**
+ * Mailbox Interrupt Mask enable/disable definitions
+ *
+ * \#defines use the channel number to determine the register and bit shift to
+ * use.
+ * The interrupt destination adds an offset to the bit shift.
+ */
+#define QM_MBOX_ENABLE_LMT_INT_MASK(N)                                         \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask &=                             \
+	    ~(BIT(N + QM_MBOX_HOST_MASK_OFFSET))
+#define QM_MBOX_DISABLE_LMT_INT_MASK(N)                                        \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask |=                             \
+	    (BIT(N + QM_MBOX_HOST_MASK_OFFSET))
+#define QM_MBOX_ENABLE_SS_INT_MASK(N)                                          \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask &=                             \
+	    ~(BIT(N + QM_MBOX_SS_MASK_OFFSET))
+#define QM_MBOX_DISABLE_SS_INT_MASK(N)                                         \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask |=                             \
+	    (BIT(N + QM_MBOX_SS_MASK_OFFSET))
+
+/**
+ * Mailbox Interrupt Halt Mask enable/disable definitions
+ *
+ * \#defines use the channel number to determine the register and bit shift to
+ * use.
+ * The interrupt destination adds an offset to the bit shift,
+ * see above for the bit position layout
+ */
+#define QM_MBOX_ENABLE_LMT_INT_HALT_MASK(N)                                    \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask &=                             \
+	    ~(BIT(N + QM_MBOX_HOST_HALT_MASK_OFFSET))
+#define QM_MBOX_DISABLE_LMT_INT_HALT_MASK(N)                                   \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask |=                             \
+	    (BIT(N + QM_MBOX_HOST_HALT_MASK_OFFSET))
+#define QM_MBOX_ENABLE_SS_INT_HALT_MASK(N)                                     \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask &=                             \
+	    ~(BIT(N + QM_MBOX_SS_HALT_MASK_OFFSET))
+#define QM_MBOX_DISABLE_SS_INT_HALT_MASK(N)                                    \
+	QM_INTERRUPT_ROUTER->mailbox_0_int_mask |=                             \
+	    (BIT(N + QM_MBOX_SS_HALT_MASK_OFFSET))
+
+/**
+ * Mailbox interrupt mask definitions to return the current mask values
+ */
+#define QM_MBOX_SS_INT_HALT_MASK                                               \
+	((QM_MBOX_SS_HALT_MASK_MASK &                                          \
+	  QM_INTERRUPT_ROUTER->mailbox_0_int_mask) >>                          \
+	 QM_MBOX_SS_HALT_MASK_OFFSET)
+#define QM_MBOX_LMT_INT_HALT_MASK                                              \
+	((QM_MBOX_HOST_HALT_MASK_MASK &                                        \
+	  QM_INTERRUPT_ROUTER->mailbox_0_int_mask) >>                          \
+	 QM_MBOX_SS_HALT_MASK_OFFSET)
+#define QM_MBOX_SS_INT_MASK                                                    \
+	((QM_MBOX_SS_MASK_MASK & QM_INTERRUPT_ROUTER->mailbox_0_int_mask) >>   \
+	 QM_MBOX_SS_MASK_OFFSET)
+#define QM_MBOX_LMT_INT_MASK                                                   \
+	(QM_MBOX_HOST_MASK_MASK & QM_INTERRUPT_ROUTER->mailbox_0_int_mask)
+
+/**
+ * Mailbox interrupt macros to determine if the specified mailbox interrupt mask
+ * has been locked.
+ */
+#define QM_MBOX_SS_INT_LOCK_HALT_MASK(N)                                       \
+	(QM_INTERRUPT_ROUTER->lock_int_mask_reg & BIT(3))
+#define QM_MBOX_LMT_INT_LOCK_HALT_MASK(N)                                      \
+	(QM_INTERRUPT_ROUTER->lock_int_mask_reg & BIT(2))
+#define QM_MBOX_SS_INT_LOCK_MASK(N)                                            \
+	(QM_INTERRUPT_ROUTER->lock_int_mask_reg & BIT(1))
+#define QM_MBOX_LMT_INT_LOCK_MASK(N)                                           \
+	(QM_INTERRUPT_ROUTER->lock_int_mask_reg & BIT(0))
+
+/** Mailbox register structure. */
+typedef struct {
+	QM_RW uint32_t ch_ctrl;    /**< Channel Control Word */
+	QM_RW uint32_t ch_data[4]; /**< Channel Payload Data Word 0 */
+	QM_RW uint32_t ch_sts;     /**< Channel status */
+} qm_mailbox_t;
+
+/** Mailbox register map. */
+typedef struct {
+	qm_mailbox_t mbox[NUM_MAILBOXES]; /**< 8 Mailboxes */
+	QM_RW uint32_t mbox_chall_sts;    /**< All channel status */
+} qm_mailbox_reg_t;
+
+#if (UNIT_TEST)
+qm_mailbox_reg_t test_mailbox;
+#define QM_MAILBOX ((qm_mailbox_reg_t *)(&test_mailbox))
+
+#else
+#define QM_MAILBOX_BASE (0xB0800A00)
+#define QM_MAILBOX ((qm_mailbox_reg_t *)QM_MAILBOX_BASE)
+#endif
+
+/** @} */
+
+/**
+ * @name PWM / Timer
+ * @{
+ */
+
+/** Number of PWM / Timer controllers. */
+typedef enum { QM_PWM_0 = 0, QM_PWM_NUM } qm_pwm_t;
+
+/** PWM ID type. */
+typedef enum {
+	QM_PWM_ID_0 = 0,
+	QM_PWM_ID_1,
+	QM_PWM_ID_2,
+	QM_PWM_ID_3,
+	QM_PWM_ID_NUM
+} qm_pwm_id_t;
+
+/** PWM / Timer channel register map. */
+typedef struct {
+	QM_RW uint32_t loadcount;    /**< Load Count */
+	QM_RW uint32_t currentvalue; /**< Current Value */
+	QM_RW uint32_t controlreg;   /**< Control */
+	QM_RW uint32_t eoi;	  /**< End Of Interrupt */
+	QM_RW uint32_t intstatus;    /**< Interrupt Status */
+} qm_pwm_channel_t;
+
+/** PWM / Timer register map. */
+typedef struct {
+	qm_pwm_channel_t timer[QM_PWM_ID_NUM]; /**< 4 Timers */
+	QM_RW uint32_t reserved[20];
+	QM_RW uint32_t timersintstatus;    /**< Timers Interrupt Status */
+	QM_RW uint32_t timerseoi;	  /**< Timers End Of Interrupt */
+	QM_RW uint32_t timersrawintstatus; /**< Timers Raw Interrupt Status */
+	QM_RW uint32_t timerscompversion;  /**< Timers Component Version */
+	QM_RW uint32_t
+	    timer_loadcount2[QM_PWM_ID_NUM]; /**< Timer Load Count 2 */
+} qm_pwm_reg_t;
+
+/**
+ * PWM context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by
+ * the qm_pwm_save_context and qm_pwm_restore_context functions.
+ */
+typedef struct {
+	struct {
+		uint32_t loadcount;  /**< Load Count 1. */
+		uint32_t loadcount2; /**< Load Count 2. */
+		uint32_t controlreg; /**< Control Register. */
+	} channel[QM_PWM_ID_NUM];
+} qm_pwm_context_t;
+
+#if (UNIT_TEST)
+qm_pwm_reg_t test_pwm_t;
+#define QM_PWM ((qm_pwm_reg_t *)(&test_pwm_t))
+
+#else
+/* PWM register base address. */
+#define QM_PWM_BASE (0xB0000800)
+/* PWM register block. */
+#define QM_PWM ((qm_pwm_reg_t *)QM_PWM_BASE)
+#endif
+
+#define PWM_START (1)
+
+#define QM_PWM_CONF_MODE_MASK (0xA)
+#define QM_PWM_CONF_INT_EN_MASK (0x4)
+
+#define QM_PWM_INTERRUPT_MASK_OFFSET (0x2)
+
+/**
+ * Timer N Control (TimerNControlReg)
+ *
+ * 31:4  RO  reserved
+ * 3     RW  Timer PWM
+ *               1 - PWM Mode
+ *               0 - Timer Mode
+ * 2     RW  Timer Interrupt Mask, set to 1b to mask interrupt.
+ * 1     RW  Timer Mode
+ *               1 - user-defined count mode
+ *               0 - free-running mode
+ * 0     RW  Timer Enable
+ *               0 - Disable PWM/Timer
+ *               1 - Enable PWM/Timer
+ */
+
+#define QM_PWM_TIMERNCONTROLREG_TIMER_ENABLE (BIT(0))
+#define QM_PWM_TIMERNCONTROLREG_TIMER_MODE (BIT(1))
+#define QM_PWM_TIMERNCONTROLREG_TIMER_INTERRUPT_MASK (BIT(2))
+#define QM_PWM_TIMERNCONTROLREG_TIMER_PWM (BIT(3))
+
+#define QM_PWM_MODE_TIMER_FREE_RUNNING_VALUE (0)
+#define QM_PWM_MODE_TIMER_COUNT_VALUE (QM_PWM_TIMERNCONTROLREG_TIMER_MODE)
+#define QM_PWM_MODE_PWM_VALUE                                                  \
+	(QM_PWM_TIMERNCONTROLREG_TIMER_PWM | QM_PWM_TIMERNCONTROLREG_TIMER_MODE)
+
+/** @} */
+
+/**
+ * @name WDT
+ * @{
+ */
+
+/** Number of WDT controllers. */
+typedef enum { QM_WDT_0 = 0, QM_WDT_NUM } qm_wdt_t;
+
+/** Watchdog timer register map. */
+typedef struct {
+	QM_RW uint32_t wdt_cr;		 /**< Control Register */
+	QM_RW uint32_t wdt_torr;	 /**< Timeout Range Register */
+	QM_RW uint32_t wdt_ccvr;	 /**< Current Counter Value Register */
+	QM_RW uint32_t wdt_crr;		 /**< Current Restart Register */
+	QM_RW uint32_t wdt_stat;	 /**< Interrupt Status Register */
+	QM_RW uint32_t wdt_eoi;		 /**< Interrupt Clear Register */
+	QM_RW uint32_t wdt_comp_param_5; /**<  Component Parameters */
+	QM_RW uint32_t wdt_comp_param_4; /**<  Component Parameters */
+	QM_RW uint32_t wdt_comp_param_3; /**<  Component Parameters */
+	QM_RW uint32_t wdt_comp_param_2; /**<  Component Parameters */
+	QM_RW uint32_t
+	    wdt_comp_param_1; /**<  Component Parameters Register 1 */
+	QM_RW uint32_t wdt_comp_version; /**<  Component Version Register */
+	QM_RW uint32_t wdt_comp_type;    /**< Component Type Register */
+} qm_wdt_reg_t;
+
+/*
+ * WDT context type.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_wdt_save_context and
+ * qm_wdt_restore_context functions.
+ */
+typedef struct {
+	uint32_t wdt_cr;   /**< Control Register. */
+	uint32_t wdt_torr; /**< Timeout Range Register. */
+} qm_wdt_context_t;
+
+#if (UNIT_TEST)
+qm_wdt_reg_t test_wdt;
+#define QM_WDT ((qm_wdt_reg_t *)(&test_wdt))
+
+#else
+/* WDT register base address. */
+#define QM_WDT_BASE (0xB0000000)
+
+/* WDT register block. */
+#define QM_WDT ((qm_wdt_reg_t *)QM_WDT_BASE)
+#endif
+
+/* Watchdog enable. */
+#define QM_WDT_CR_WDT_ENABLE (BIT(0))
+/* Watchdog mode. */
+#define QM_WDT_CR_RMOD (BIT(1))
+/* Watchdog mode offset. */
+#define QM_WDT_CR_RMOD_OFFSET (1)
+/* Watchdog Timeout Mask. */
+#define QM_WDT_TORR_TOP_MASK (0xF)
+
+/**
+ * WDT timeout table (in clock cycles):
+ * Each table entry corresponds with the value loaded
+ * into the WDT at the time of a WDT reload for the
+ * corresponding timeout range register value.
+ *
+ * TORR | Timeout (Clock Cycles)
+ * 0.   | 2^16 (65536)
+ * 1.   | 2^17 (131072)
+ * 2.   | 2^18 (262144)
+ * 3.   | 2^19 (524288)
+ * 4.   | 2^20 (1048576)
+ * 5.   | 2^21 (2097152)
+ * 6.   | 2^22 (4194304)
+ * 7.   | 2^23 (8388608)
+ * 8.   | 2^24 (16777216)
+ * 9.   | 2^25 (33554432)
+ * 10.  | 2^26 (67108864)
+ * 11.  | 2^27 (134217728)
+ * 12.  | 2^28 (268435456)
+ * 13.  | 2^29 (536870912)
+ * 14.  | 2^30 (1073741824)
+ * 15.  | 2^31 (2147483648)
+ */
+
+/** @} */
+
+/**
+ * @name UART
+ * @{
+ */
+
+/* Break character Bit. */
+#define QM_UART_LCR_BREAK BIT(6)
+/* Divisor Latch Access Bit. */
+#define QM_UART_LCR_DLAB BIT(7)
+
+/* Request to Send Bit. */
+#define QM_UART_MCR_RTS BIT(1)
+/* Loopback Enable Bit. */
+#define QM_UART_MCR_LOOPBACK BIT(4)
+/* Auto Flow Control Enable Bit. */
+#define QM_UART_MCR_AFCE BIT(5)
+
+/* FIFO Enable Bit. */
+#define QM_UART_FCR_FIFOE BIT(0)
+/* Reset Receive FIFO. */
+#define QM_UART_FCR_RFIFOR BIT(1)
+/* Reset Transmit FIFO. */
+#define QM_UART_FCR_XFIFOR BIT(2)
+
+/* Default FIFO RX & TX Thresholds, half full for both. */
+#define QM_UART_FCR_DEFAULT_TX_RX_THRESHOLD (0xB0)
+/* Change TX Threshold to empty, keep RX Threshold to default. */
+#define QM_UART_FCR_TX_0_RX_1_2_THRESHOLD (0x80)
+
+/* Transmit Holding Register Empty. */
+#define QM_UART_IIR_THR_EMPTY (0x02)
+/* Received Data Available. */
+#define QM_UART_IIR_RECV_DATA_AVAIL (0x04)
+/* Receiver Line Status. */
+#define QM_UART_IIR_RECV_LINE_STATUS (0x06)
+/* Character Timeout. */
+#define QM_UART_IIR_CHAR_TIMEOUT (0x0C)
+/* Interrupt ID Mask. */
+#define QM_UART_IIR_IID_MASK (0x0F)
+
+/* Data Ready Bit. */
+#define QM_UART_LSR_DR BIT(0)
+/* Overflow Error Bit. */
+#define QM_UART_LSR_OE BIT(1)
+/* Parity Error Bit. */
+#define QM_UART_LSR_PE BIT(2)
+/* Framing Error Bit. */
+#define QM_UART_LSR_FE BIT(3)
+/* Break Interrupt Bit. */
+#define QM_UART_LSR_BI BIT(4)
+/* Transmit Holding Register Empty Bit. */
+#define QM_UART_LSR_THRE BIT(5)
+/* Transmitter Empty Bit. */
+#define QM_UART_LSR_TEMT BIT(6)
+/* Receiver FIFO Error Bit. */
+#define QM_UART_LSR_RFE BIT(7)
+
+/* Enable Received Data Available Interrupt. */
+#define QM_UART_IER_ERBFI BIT(0)
+/* Enable Transmit Holding Register Empty Interrupt. */
+#define QM_UART_IER_ETBEI BIT(1)
+/* Enable Receiver Line Status Interrupt. */
+#define QM_UART_IER_ELSI BIT(2)
+/* Programmable THRE Interrupt Mode. */
+#define QM_UART_IER_PTIME BIT(7)
+
+/* Line Status Errors. */
+#define QM_UART_LSR_ERROR_BITS                                                 \
+	(QM_UART_LSR_OE | QM_UART_LSR_PE | QM_UART_LSR_FE | QM_UART_LSR_BI)
+
+/* FIFO Depth. */
+#define QM_UART_FIFO_DEPTH (16)
+/* FIFO Half Depth. */
+#define QM_UART_FIFO_HALF_DEPTH (QM_UART_FIFO_DEPTH / 2)
+
+/* Divisor Latch High Offset. */
+#define QM_UART_CFG_BAUD_DLH_OFFS 16
+/* Divisor Latch Low Offset. */
+#define QM_UART_CFG_BAUD_DLL_OFFS 8
+/* Divisor Latch Fraction Offset. */
+#define QM_UART_CFG_BAUD_DLF_OFFS 0
+/* Divisor Latch High Mask. */
+#define QM_UART_CFG_BAUD_DLH_MASK (0xFF << QM_UART_CFG_BAUD_DLH_OFFS)
+/* Divisor Latch Low Mask. */
+#define QM_UART_CFG_BAUD_DLL_MASK (0xFF << QM_UART_CFG_BAUD_DLL_OFFS)
+/* Divisor Latch Fraction Mask. */
+#define QM_UART_CFG_BAUD_DLF_MASK (0xFF << QM_UART_CFG_BAUD_DLF_OFFS)
+
+/* Divisor Latch Packing Helper. */
+#define QM_UART_CFG_BAUD_DL_PACK(dlh, dll, dlf)                                \
+	(dlh << QM_UART_CFG_BAUD_DLH_OFFS | dll << QM_UART_CFG_BAUD_DLL_OFFS | \
+	 dlf << QM_UART_CFG_BAUD_DLF_OFFS)
+
+/* Divisor Latch High Unpacking Helper. */
+#define QM_UART_CFG_BAUD_DLH_UNPACK(packed)                                    \
+	((packed & QM_UART_CFG_BAUD_DLH_MASK) >> QM_UART_CFG_BAUD_DLH_OFFS)
+/* Divisor Latch Low Unpacking Helper. */
+#define QM_UART_CFG_BAUD_DLL_UNPACK(packed)                                    \
+	((packed & QM_UART_CFG_BAUD_DLL_MASK) >> QM_UART_CFG_BAUD_DLL_OFFS)
+/* Divisor Latch Fraction Unpacking Helper. */
+#define QM_UART_CFG_BAUD_DLF_UNPACK(packed)                                    \
+	((packed & QM_UART_CFG_BAUD_DLF_MASK) >> QM_UART_CFG_BAUD_DLF_OFFS)
+
+/** Number of UART controllers. */
+typedef enum { QM_UART_0 = 0, QM_UART_1, QM_UART_NUM } qm_uart_t;
+
+/** UART register map. */
+typedef struct {
+	QM_RW uint32_t rbr_thr_dll; /**< Rx Buffer/ Tx Holding/ Div Latch Low */
+	QM_RW uint32_t ier_dlh; /**< Interrupt Enable / Divisor Latch High */
+	QM_RW uint32_t iir_fcr; /**< Interrupt Identification / FIFO Control */
+	QM_RW uint32_t lcr;     /**< Line Control */
+	QM_RW uint32_t mcr;     /**< MODEM Control */
+	QM_RW uint32_t lsr;     /**< Line Status */
+	QM_RW uint32_t msr;     /**< MODEM Status */
+	QM_RW uint32_t scr;     /**< Scratchpad */
+	QM_RW uint32_t reserved[23];
+	QM_RW uint32_t usr; /**< UART Status */
+	QM_RW uint32_t reserved1[9];
+	QM_RW uint32_t htx;   /**< Halt Transmission */
+	QM_RW uint32_t dmasa; /**< DMA Software Acknowledge */
+	QM_RW uint32_t reserved2[5];
+	QM_RW uint32_t dlf;	   /**< Divisor Latch Fraction */
+	QM_RW uint32_t padding[0xCF]; /* (0x400 - 0xC4) / 4 */
+} qm_uart_reg_t;
+
+/**
+ * UART context to be saved between sleep/resume.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_uart_save_context and
+ * qm_uart_restore_context functions.
+ */
+typedef struct {
+	uint32_t ier; /**< Interrupt Enable Register. */
+	uint32_t dlh; /**< Divisor Latch High. */
+	uint32_t dll; /**< Divisor Latch Low. */
+	uint32_t lcr; /**< Line Control. */
+	uint32_t mcr; /**< Modem Control. */
+	uint32_t scr; /**< Scratchpad. */
+	uint32_t htx; /**< Halt Transmission. */
+	uint32_t dlf; /**< Divisor Latch Fraction. */
+} qm_uart_context_t;
+
+#if (UNIT_TEST)
+qm_uart_reg_t test_uart_instance;
+qm_uart_reg_t *test_uart[QM_UART_NUM];
+#define QM_UART test_uart
+
+#else
+/* UART register base address. */
+#define QM_UART_0_BASE (0xB0002000)
+#define QM_UART_1_BASE (0xB0002400)
+/* UART register block. */
+extern qm_uart_reg_t *qm_uart[QM_UART_NUM];
+#define QM_UART qm_uart
+#endif
+
+/** @} */
+
+/**
+ * @name SPI
+ * @{
+ */
+
+/** Number of SPI controllers (only master driver available). */
+typedef enum { QM_SPI_MST_0 = 0, QM_SPI_MST_1, QM_SPI_NUM } qm_spi_t;
+
+/** SPI register map. */
+typedef struct {
+	QM_RW uint32_t ctrlr0; /**< Control Register 0 */
+	QM_RW uint32_t ctrlr1; /**< Control Register 1 */
+	QM_RW uint32_t ssienr; /**< SSI Enable Register */
+	QM_RW uint32_t mwcr;   /**< Microwire Control Register */
+	QM_RW uint32_t ser;    /**< Slave Enable Register */
+	QM_RW uint32_t baudr;  /**< Baud Rate Select */
+	QM_RW uint32_t txftlr; /**< Transmit FIFO Threshold Level */
+	QM_RW uint32_t rxftlr; /**< Receive FIFO Threshold Level */
+	QM_RW uint32_t txflr;  /**< Transmit FIFO Level Register */
+	QM_RW uint32_t rxflr;  /**< Receive FIFO Level Register */
+	QM_RW uint32_t sr;     /**< Status Register */
+	QM_RW uint32_t imr;    /**< Interrupt Mask Register */
+	QM_RW uint32_t isr;    /**< Interrupt Status Register */
+	QM_RW uint32_t risr;   /**< Raw Interrupt Status Register */
+	QM_RW uint32_t txoicr; /**< Tx FIFO Overflow Interrupt Clear Register*/
+	QM_RW uint32_t rxoicr; /**< Rx FIFO Overflow Interrupt Clear Register */
+	QM_RW uint32_t rxuicr; /**< Rx FIFO Underflow Interrupt Clear Register*/
+	QM_RW uint32_t msticr; /**< Multi-Master Interrupt Clear Register */
+	QM_RW uint32_t icr;    /**< Interrupt Clear Register */
+	QM_RW uint32_t dmacr;  /**< DMA Control Register */
+	QM_RW uint32_t dmatdlr;		 /**< DMA Transmit Data Level */
+	QM_RW uint32_t dmardlr;		 /**< DMA Receive Data Level */
+	QM_RW uint32_t idr;		 /**< Identification Register */
+	QM_RW uint32_t ssi_comp_version; /**< coreKit Version ID register */
+	QM_RW uint32_t dr[36];		 /**< Data Register */
+	QM_RW uint32_t rx_sample_dly;    /**< RX Sample Delay Register */
+	QM_RW uint32_t padding[0xC4];    /* (0x400 - 0xF0) / 4 */
+} qm_spi_reg_t;
+
+/**
+ * SPI context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by
+ * the qm_spi_save_context and qm_spi_restore_context functions.
+ */
+typedef struct {
+	uint32_t ctrlr0; /**< Control Register 0. */
+	uint32_t ser;    /**< Slave Enable Register. */
+	uint32_t baudr;  /**< Baud Rate Select. */
+} qm_spi_context_t;
+
+#if (UNIT_TEST)
+qm_spi_reg_t test_spi;
+qm_spi_reg_t *test_spi_controllers[QM_SPI_NUM];
+
+#define QM_SPI test_spi_controllers
+
+#else
+/* SPI Master register base address. */
+#define QM_SPI_MST_0_BASE (0xB0001000)
+#define QM_SPI_MST_1_BASE (0xB0001400)
+extern qm_spi_reg_t *qm_spi_controllers[QM_SPI_NUM];
+#define QM_SPI qm_spi_controllers
+
+/* SPI Slave register base address. */
+#define QM_SPI_SLV_BASE (0xB0001800)
+#endif
+
+/* SPI Ctrlr0 register. */
+#define QM_SPI_CTRLR0_DFS_32_MASK (0x001F0000)
+#define QM_SPI_CTRLR0_TMOD_MASK (0x00000300)
+#define QM_SPI_CTRLR0_SCPOL_SCPH_MASK (0x000000C0)
+#define QM_SPI_CTRLR0_FRF_MASK (0x00000030)
+#define QM_SPI_CTRLR0_DFS_32_OFFSET (16)
+#define QM_SPI_CTRLR0_TMOD_OFFSET (8)
+#define QM_SPI_CTRLR0_SCPOL_SCPH_OFFSET (6)
+#define QM_SPI_CTRLR0_FRF_OFFSET (4)
+
+/* SPI SSI Enable register. */
+#define QM_SPI_SSIENR_SSIENR BIT(0)
+
+/* SPI Status register. */
+#define QM_SPI_SR_BUSY BIT(0)
+#define QM_SPI_SR_TFNF BIT(1)
+#define QM_SPI_SR_TFE BIT(2)
+#define QM_SPI_SR_RFNE BIT(3)
+#define QM_SPI_SR_RFF BIT(4)
+
+/* SPI Interrupt Mask register. */
+#define QM_SPI_IMR_MASK_ALL (0x00)
+#define QM_SPI_IMR_TXEIM BIT(0)
+#define QM_SPI_IMR_TXOIM BIT(1)
+#define QM_SPI_IMR_RXUIM BIT(2)
+#define QM_SPI_IMR_RXOIM BIT(3)
+#define QM_SPI_IMR_RXFIM BIT(4)
+
+/* SPI Interrupt Status register. */
+#define QM_SPI_ISR_TXEIS BIT(0)
+#define QM_SPI_ISR_TXOIS BIT(1)
+#define QM_SPI_ISR_RXUIS BIT(2)
+#define QM_SPI_ISR_RXOIS BIT(3)
+#define QM_SPI_ISR_RXFIS BIT(4)
+
+/* SPI Raw Interrupt Status register. */
+#define QM_SPI_RISR_TXEIR BIT(0)
+#define QM_SPI_RISR_TXOIR BIT(1)
+#define QM_SPI_RISR_RXUIR BIT(2)
+#define QM_SPI_RISR_RXOIR BIT(3)
+#define QM_SPI_RISR_RXFIR BIT(4)
+
+/* SPI DMA control. */
+#define QM_SPI_DMACR_RDMAE BIT(0)
+#define QM_SPI_DMACR_TDMAE BIT(1)
+
+/** @} */
+
+/**
+ * @name RTC
+ * @{
+ */
+
+/** Number of RTC controllers. */
+typedef enum { QM_RTC_0 = 0, QM_RTC_NUM } qm_rtc_t;
+
+/** RTC register map. */
+typedef struct {
+	QM_RW uint32_t rtc_ccvr;	 /**< Current Counter Value Register */
+	QM_RW uint32_t rtc_cmr;		 /**< Current Match Register */
+	QM_RW uint32_t rtc_clr;		 /**< Counter Load Register */
+	QM_RW uint32_t rtc_ccr;		 /**< Counter Control Register */
+	QM_RW uint32_t rtc_stat;	 /**< Interrupt Status Register */
+	QM_RW uint32_t rtc_rstat;	/**< Interrupt Raw Status Register */
+	QM_RW uint32_t rtc_eoi;		 /**< End of Interrupt Register */
+	QM_RW uint32_t rtc_comp_version; /**< End of Interrupt Register */
+} qm_rtc_reg_t;
+
+#define QM_RTC_CCR_INTERRUPT_ENABLE BIT(0)
+#define QM_RTC_CCR_INTERRUPT_MASK BIT(1)
+#define QM_RTC_CCR_ENABLE BIT(2)
+
+#if (UNIT_TEST)
+qm_rtc_reg_t test_rtc;
+#define QM_RTC ((qm_rtc_reg_t *)(&test_rtc))
+
+#else
+/* RTC register base address. */
+#define QM_RTC_BASE (0xB0000400)
+
+/* RTC register block. */
+#define QM_RTC ((qm_rtc_reg_t *)QM_RTC_BASE)
+#endif
+
+/** @} */
+
+/**
+ * @name I2C
+ * @{
+ */
+
+/** Number of I2C controllers. */
+typedef enum { QM_I2C_0 = 0, QM_I2C_1, QM_I2C_NUM } qm_i2c_t;
+
+/** I2C register map. */
+typedef struct {
+	QM_RW uint32_t ic_con;      /**< Control Register */
+	QM_RW uint32_t ic_tar;      /**< Master Target Address */
+	QM_RW uint32_t ic_sar;      /**< Slave Address */
+	QM_RW uint32_t ic_hs_maddr; /**< High Speed Master ID */
+	QM_RW uint32_t ic_data_cmd; /**< Data Buffer and Command */
+	QM_RW uint32_t
+	    ic_ss_scl_hcnt; /**< Standard Speed Clock SCL High Count */
+	QM_RW uint32_t
+	    ic_ss_scl_lcnt; /**< Standard Speed Clock SCL Low Count */
+	QM_RW uint32_t ic_fs_scl_hcnt; /**< Fast Speed Clock SCL High Count */
+	QM_RW uint32_t
+	    ic_fs_scl_lcnt; /**< Fast Speed I2C Clock SCL Low Count */
+	QM_RW uint32_t
+	    ic_hs_scl_hcnt; /**< High Speed I2C Clock SCL High Count */
+	QM_RW uint32_t
+	    ic_hs_scl_lcnt;	  /**< High Speed I2C Clock SCL Low Count */
+	QM_RW uint32_t ic_intr_stat; /**< Interrupt Status */
+	QM_RW uint32_t ic_intr_mask; /**< Interrupt Mask */
+	QM_RW uint32_t ic_raw_intr_stat; /**< Raw Interrupt Status */
+	QM_RW uint32_t ic_rx_tl;	 /**< Receive FIFO Threshold Level */
+	QM_RW uint32_t ic_tx_tl;	 /**< Transmit FIFO Threshold Level */
+	QM_RW uint32_t
+	    ic_clr_intr; /**< Clear Combined and Individual Interrupt */
+	QM_RW uint32_t ic_clr_rx_under;   /**< Clear RX_UNDER Interrupt */
+	QM_RW uint32_t ic_clr_rx_over;    /**< Clear RX_OVER Interrupt */
+	QM_RW uint32_t ic_clr_tx_over;    /**< Clear TX_OVER Interrupt */
+	QM_RW uint32_t ic_clr_rd_req;     /**< Clear RD_REQ Interrupt */
+	QM_RW uint32_t ic_clr_tx_abrt;    /**< Clear TX_ABRT Interrupt */
+	QM_RW uint32_t ic_clr_rx_done;    /**< Clear RX_DONE Interrupt */
+	QM_RW uint32_t ic_clr_activity;   /**< Clear ACTIVITY Interrupt */
+	QM_RW uint32_t ic_clr_stop_det;   /**< Clear STOP_DET Interrupt */
+	QM_RW uint32_t ic_clr_start_det;  /**< Clear START_DET Interrupt */
+	QM_RW uint32_t ic_clr_gen_call;   /**< Clear GEN_CALL Interrupt */
+	QM_RW uint32_t ic_enable;	 /**< Enable */
+	QM_RW uint32_t ic_status;	 /**< Status */
+	QM_RW uint32_t ic_txflr;	  /**< Transmit FIFO Level */
+	QM_RW uint32_t ic_rxflr;	  /**< Receive FIFO Level */
+	QM_RW uint32_t ic_sda_hold;       /**< SDA Hold */
+	QM_RW uint32_t ic_tx_abrt_source; /**< Transmit Abort Source */
+	QM_RW uint32_t reserved;
+	QM_RW uint32_t ic_dma_cr;    /**< SDA Setup */
+	QM_RW uint32_t ic_dma_tdlr;  /**< DMA Transmit Data Level Register */
+	QM_RW uint32_t ic_dma_rdlr;  /**< I2C Receive Data Level Register */
+	QM_RW uint32_t ic_sda_setup; /**< SDA Setup */
+	QM_RW uint32_t ic_ack_general_call; /**< General Call Ack */
+	QM_RW uint32_t ic_enable_status;    /**< Enable Status */
+	QM_RW uint32_t ic_fs_spklen; /**< SS and FS Spike Suppression Limit */
+	QM_RW uint32_t ic_hs_spklen; /**< HS spike suppression limit */
+	QM_RW uint32_t reserved1[19];
+	QM_RW uint32_t ic_comp_param_1; /**< Configuration Parameters */
+	QM_RW uint32_t ic_comp_version; /**< Component Version */
+	QM_RW uint32_t ic_comp_type;    /**< Component Type */
+	QM_RW uint32_t padding[0xC0];   /* Padding (0x400-0xFC)/4 */
+} qm_i2c_reg_t;
+
+/**
+ * I2C context to be saved between sleep/resume.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_i2c_save_context and
+ * qm_i2c_restore_context functions.
+ */
+typedef struct {
+	uint32_t con;	  /**< Control Register. */
+	uint32_t sar;	  /**< Slave Address. */
+	uint32_t ss_scl_hcnt;  /**< Standard Speed Clock SCL High Count. */
+	uint32_t ss_scl_lcnt;  /**< Standard Speed Clock SCL Low Count. */
+	uint32_t fs_scl_hcnt;  /**< Fast Speed Clock SCL High Count. */
+	uint32_t fs_scl_lcnt;  /**< Fast Speed I2C Clock SCL Low Count. */
+	uint32_t enable;       /**< Enable. */
+	uint32_t fs_spklen;    /**< SS and FS Spike Suppression Limit. */
+	uint32_t ic_intr_mask; /**< I2C Interrupt Mask. */
+} qm_i2c_context_t;
+
+#if (UNIT_TEST)
+qm_i2c_reg_t test_i2c_instance[QM_I2C_NUM];
+qm_i2c_reg_t *test_i2c[QM_I2C_NUM];
+
+#define QM_I2C test_i2c
+
+#else
+/* I2C Master register base address. */
+#define QM_I2C_0_BASE (0xB0002800)
+#define QM_I2C_1_BASE (0xB0002C00)
+
+/** I2C register block. */
+extern qm_i2c_reg_t *qm_i2c[QM_I2C_NUM];
+#define QM_I2C qm_i2c
+#endif
+
+#define QM_I2C_IC_ENABLE_CONTROLLER_EN BIT(0)
+#define QM_I2C_IC_ENABLE_CONTROLLER_ABORT BIT(1)
+#define QM_I2C_IC_ENABLE_STATUS_IC_EN BIT(0)
+#define QM_I2C_IC_CON_MASTER_MODE BIT(0)
+#define QM_I2C_IC_CON_SLAVE_DISABLE BIT(6)
+#define QM_I2C_IC_CON_10BITADDR_MASTER BIT(4)
+#define QM_I2C_IC_CON_10BITADDR_MASTER_OFFSET (4)
+#define QM_I2C_IC_CON_10BITADDR_SLAVE BIT(3)
+#define QM_I2C_IC_CON_10BITADDR_SLAVE_OFFSET (3)
+#define QM_I2C_IC_CON_SPEED_OFFSET (1)
+#define QM_I2C_IC_CON_SPEED_SS BIT(1)
+#define QM_I2C_IC_CON_SPEED_FS_FSP BIT(2)
+#define QM_I2C_IC_CON_SPEED_MASK (0x06)
+#define QM_I2C_IC_CON_RESTART_EN BIT(5)
+#define QM_I2C_IC_CON_STOP_DET_IFADDRESSED BIT(7)
+#define QM_I2C_IC_DATA_CMD_READ BIT(8)
+#define QM_I2C_IC_DATA_CMD_STOP_BIT_CTRL BIT(9)
+#define QM_I2C_IC_DATA_CMD_LSB_MASK (0x000000FF)
+#define QM_I2C_IC_RAW_INTR_STAT_RX_FULL BIT(2)
+#define QM_I2C_IC_RAW_INTR_STAT_TX_ABRT BIT(6)
+#define QM_I2C_IC_RAW_INTR_STAT_GEN_CALL BIT(11)
+#define QM_I2C_IC_RAW_INTR_STAT_RESTART_DETECTED BIT(12)
+#define QM_I2C_IC_TX_ABRT_SOURCE_NAK_MASK (0x1F)
+#define QM_I2C_IC_TX_ABRT_SOURCE_ARB_LOST BIT(12)
+#define QM_I2C_IC_TX_ABRT_SOURCE_ABRT_SBYTE_NORSTRT BIT(9)
+#define QM_I2C_IC_TX_ABRT_SOURCE_ALL_MASK (0x1FFFF)
+#define QM_I2C_IC_STATUS_BUSY_MASK (0x00000060)
+#define QM_I2C_IC_STATUS_RFF BIT(4)
+#define QM_I2C_IC_STATUS_RFNE BIT(3)
+#define QM_I2C_IC_STATUS_TFE BIT(2)
+#define QM_I2C_IC_STATUS_TNF BIT(1)
+#define QM_I2C_IC_INTR_MASK_ALL (0x00)
+#define QM_I2C_IC_INTR_MASK_RX_UNDER BIT(0)
+#define QM_I2C_IC_INTR_MASK_RX_OVER BIT(1)
+#define QM_I2C_IC_INTR_MASK_RX_FULL BIT(2)
+#define QM_I2C_IC_INTR_MASK_TX_OVER BIT(3)
+#define QM_I2C_IC_INTR_MASK_TX_EMPTY BIT(4)
+#define QM_I2C_IC_INTR_MASK_RD_REQ BIT(5)
+#define QM_I2C_IC_INTR_MASK_TX_ABORT BIT(6)
+#define QM_I2C_IC_INTR_MASK_RX_DONE BIT(7)
+#define QM_I2C_IC_INTR_MASK_ACTIVITY BIT(8)
+#define QM_I2C_IC_INTR_MASK_STOP_DETECTED BIT(9)
+#define QM_I2C_IC_INTR_MASK_START_DETECTED BIT(10)
+#define QM_I2C_IC_INTR_MASK_GEN_CALL_DETECTED BIT(11)
+#define QM_I2C_IC_INTR_MASK_RESTART_DETECTED BIT(12)
+#define QM_I2C_IC_INTR_STAT_RX_UNDER BIT(0)
+#define QM_I2C_IC_INTR_STAT_RX_OVER BIT(1)
+#define QM_I2C_IC_INTR_STAT_RX_FULL BIT(2)
+#define QM_I2C_IC_INTR_STAT_TX_OVER BIT(3)
+#define QM_I2C_IC_INTR_STAT_TX_EMPTY BIT(4)
+#define QM_I2C_IC_INTR_STAT_RD_REQ BIT(5)
+#define QM_I2C_IC_INTR_STAT_TX_ABRT BIT(6)
+#define QM_I2C_IC_INTR_STAT_RX_DONE BIT(7)
+#define QM_I2C_IC_INTR_STAT_STOP_DETECTED BIT(9)
+#define QM_I2C_IC_INTR_STAT_START_DETECTED BIT(10)
+#define QM_I2C_IC_INTR_STAT_GEN_CALL_DETECTED BIT(11)
+#define QM_I2C_IC_LCNT_MAX (65525)
+#define QM_I2C_IC_LCNT_MIN (8)
+#define QM_I2C_IC_HCNT_MAX (65525)
+#define QM_I2C_IC_HCNT_MIN (6)
+
+#define QM_I2C_FIFO_SIZE (16)
+
+/* I2C DMA */
+#define QM_I2C_IC_DMA_CR_RX_ENABLE BIT(0)
+#define QM_I2C_IC_DMA_CR_TX_ENABLE BIT(1)
+
+/** @} */
+
+/**
+ * @name GPIO
+ * @{
+ */
+
+/** Number of GPIO controllers. */
+typedef enum { QM_GPIO_0 = 0, QM_AON_GPIO_0 = 1, QM_GPIO_NUM } qm_gpio_t;
+
+/** GPIO register map. */
+typedef struct {
+	QM_RW uint32_t gpio_swporta_dr;  /**< Port A Data */
+	QM_RW uint32_t gpio_swporta_ddr; /**< Port A Data Direction */
+	QM_RW uint32_t gpio_swporta_ctl; /**< Port A Data Source */
+	QM_RW uint32_t reserved[9];
+	QM_RW uint32_t gpio_inten;	 /**< Interrupt Enable */
+	QM_RW uint32_t gpio_intmask;       /**< Interrupt Mask */
+	QM_RW uint32_t gpio_inttype_level; /**< Interrupt Type */
+	QM_RW uint32_t gpio_int_polarity;  /**< Interrupt Polarity */
+	QM_RW uint32_t gpio_intstatus;     /**< Interrupt Status */
+	QM_RW uint32_t gpio_raw_intstatus; /**< Raw Interrupt Status */
+	QM_RW uint32_t gpio_debounce;      /**< Debounce Enable */
+	QM_RW uint32_t gpio_porta_eoi;     /**< Clear Interrupt */
+	QM_RW uint32_t gpio_ext_porta;     /**< Port A External Port */
+	QM_RW uint32_t reserved1[3];
+	QM_RW uint32_t gpio_ls_sync; /**< Synchronization Level */
+	QM_RW uint32_t reserved2;
+	QM_RW uint32_t gpio_int_bothedge; /**< Interrupt both edge type */
+	QM_RW uint32_t reserved3;
+	QM_RW uint32_t gpio_config_reg2; /**< GPIO Configuration Register 2 */
+	QM_RW uint32_t gpio_config_reg1; /**< GPIO Configuration Register 1 */
+} qm_gpio_reg_t;
+
+/**
+ * GPIO context type.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_gpio_save_context and
+ * qm_gpio_restore_context functions.
+ */
+typedef struct {
+	uint32_t gpio_swporta_dr;    /**< Port A Data. */
+	uint32_t gpio_swporta_ddr;   /**< Port A Data Direction. */
+	uint32_t gpio_swporta_ctl;   /**< Port A Data Source. */
+	uint32_t gpio_inten;	 /**< Interrupt Enable. */
+	uint32_t gpio_intmask;       /**< Interrupt Mask. */
+	uint32_t gpio_inttype_level; /**< Interrupt Type. */
+	uint32_t gpio_int_polarity;  /**< Interrupt Polarity. */
+	uint32_t gpio_debounce;      /**< Debounce Enable. */
+	uint32_t gpio_ls_sync;       /**< Synchronization Level. */
+	uint32_t gpio_int_bothedge;  /**< Interrupt both edge type. */
+} qm_gpio_context_t;
+
+#define QM_NUM_GPIO_PINS (32)
+#define QM_NUM_AON_GPIO_PINS (6)
+
+#if (UNIT_TEST)
+qm_gpio_reg_t test_gpio_instance;
+qm_gpio_reg_t *test_gpio[QM_GPIO_NUM];
+
+#define QM_GPIO test_gpio
+#else
+
+/* GPIO register base address */
+#define QM_GPIO_BASE (0xB0000C00)
+#define QM_AON_GPIO_BASE (QM_SCSS_CCU_BASE + 0xB00)
+
+/** GPIO register block */
+extern qm_gpio_reg_t *qm_gpio[QM_GPIO_NUM];
+#define QM_GPIO qm_gpio
+#endif
+
+/** @} */
+
+/**
+ * @name Flash
+ * @{
+ */
+
+/** Number of Flash controllers. */
+typedef enum { QM_FLASH_0 = 0, QM_FLASH_1, QM_FLASH_NUM } qm_flash_t;
+
+/** Flash register map. */
+typedef struct {
+	QM_RW uint32_t tmg_ctrl;      /**< TMG_CTRL. */
+	QM_RW uint32_t rom_wr_ctrl;   /**< ROM_WR_CTRL. */
+	QM_RW uint32_t rom_wr_data;   /**< ROM_WR_DATA. */
+	QM_RW uint32_t flash_wr_ctrl; /**< FLASH_WR_CTRL. */
+	QM_RW uint32_t flash_wr_data; /**< FLASH_WR_DATA. */
+	QM_RW uint32_t flash_stts;    /**< FLASH_STTS. */
+	QM_RW uint32_t ctrl;	  /**< CTRL. */
+	QM_RW uint32_t fpr_rd_cfg[4]; /**< 4 FPR_RD_CFG registers. */
+	QM_RW uint32_t
+	    mpr_wr_cfg; /**< Flash Write Protection Control Register. */
+	QM_RW uint32_t mpr_vsts; /**< Protection Status Register. */
+} qm_flash_reg_t;
+
+/**
+ * Flash context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by the
+ * qm_flash_save_context and qm_flash_restore_context functions.
+ */
+typedef struct {
+	/** Flash Timing Control Register. */
+	uint32_t tmg_ctrl;
+	/** Control Register. */
+	uint32_t ctrl;
+} qm_flash_context_t;
+
+#if (UNIT_TEST)
+qm_flash_reg_t test_flash_instance;
+qm_flash_reg_t *test_flash[QM_FLASH_NUM];
+uint8_t test_flash_page[0x800];
+
+#define QM_FLASH test_flash
+
+#define QM_FLASH_REGION_SYS_1_BASE (test_flash_page)
+#define QM_FLASH_REGION_SYS_0_BASE (test_flash_page)
+#define QM_FLASH_REGION_OTP_0_BASE (test_flash_page)
+
+#define QM_FLASH_PAGE_MASK (0xCFF)
+#define QM_FLASH_MAX_ADDR (0xFFFFFFFF)
+#else
+
+/* Flash physical address mappings */
+
+#define QM_FLASH_REGION_SYS_1_BASE (0x40030000)
+#define QM_FLASH_REGION_SYS_0_BASE (0x40000000)
+#define QM_FLASH_REGION_OTP_0_BASE (0xFFFFE000)
+
+#define QM_FLASH_PAGE_MASK (0x3F800)
+#define QM_FLASH_MAX_ADDR (0x30000)
+
+/* Flash controller register base address. */
+#define QM_FLASH_BASE_0 (0xB0100000)
+#define QM_FLASH_BASE_1 (0xB0200000)
+
+/* Flash controller register block. */
+extern qm_flash_reg_t *qm_flash[QM_FLASH_NUM];
+#define QM_FLASH qm_flash
+
+#endif
+
+#define QM_FLASH_REGION_DATA_BASE_OFFSET (0x00)
+#define QM_FLASH_MAX_WAIT_STATES (0xF)
+#define QM_FLASH_MAX_US_COUNT (0x3F)
+#define QM_FLASH_MAX_PAGE_NUM                                                  \
+	(QM_FLASH_MAX_ADDR / (4 * QM_FLASH_PAGE_SIZE_DWORDS))
+#define QM_FLASH_CLK_SLOW BIT(14)
+#define QM_FLASH_LVE_MODE BIT(5)
+
+/* Flash mask to clear timing. */
+#define QM_FLASH_TMG_DEF_MASK (0xFFFFFC00)
+/* Flash mask to clear micro seconds. */
+#define QM_FLASH_MICRO_SEC_COUNT_MASK (0x3F)
+/* Flash mask to clear wait state. */
+#define QM_FLASH_WAIT_STATE_MASK (0x3C0)
+/* Flash wait state offset bit. */
+#define QM_FLASH_WAIT_STATE_OFFSET (6)
+/* Flash write disable offset bit. */
+#define QM_FLASH_WRITE_DISABLE_OFFSET (4)
+/* Flash write disable value. */
+#define QM_FLASH_WRITE_DISABLE_VAL BIT(4)
+
+/* Flash page erase request. */
+#define ER_REQ BIT(1)
+/* Flash page erase done. */
+#define ER_DONE (1)
+/* Flash page write request. */
+#define WR_REQ (1)
+/* Flash page write done. */
+#define WR_DONE BIT(1)
+
+/* Flash write address offset. */
+#define WR_ADDR_OFFSET (2)
+/* Flash perform mass erase includes OTP region. */
+#define MASS_ERASE_INFO BIT(6)
+/* Flash perform mass erase. */
+#define MASS_ERASE BIT(7)
+
+#define QM_FLASH_ADDRESS_MASK (0x7FF)
+/* Increment by 4 bytes each time, but there is an offset of 2, so 0x10. */
+#define QM_FLASH_ADDR_INC (0x10)
+
+/* Flash page size in dwords. */
+#define QM_FLASH_PAGE_SIZE_DWORDS (0x200)
+/* Flash page size in bytes. */
+#define QM_FLASH_PAGE_SIZE_BYTES (0x800)
+/* Flash page size in bits. */
+#define QM_FLASH_PAGE_SIZE_BITS (11)
+
+/** @} */
+
+/**
+ * @name Flash Protection Region
+ * @{
+ */
+
+/**
+ * FPR register map.
+ */
+typedef enum {
+	QM_FPR_0, /**< FPR 0. */
+	QM_FPR_1, /**< FPR 1. */
+	QM_FPR_2, /**< FPR 2. */
+	QM_FPR_3, /**< FPR 3. */
+	QM_FPR_NUM
+} qm_fpr_id_t;
+
+/**
+ * FPR context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by the
+ * qm_fpr_save_context and qm_fpr_restore_context functions.
+ */
+typedef struct {
+	/** Flash Protection Region Read Control Register. */
+	uint32_t fpr_rd_cfg[QM_FPR_NUM];
+} qm_fpr_context_t;
+
+/** @} */
+
+/**
+ * @name Memory Protection Region
+ * @{
+ */
+
+/* MPR identifier */
+typedef enum {
+	QM_MPR_0 = 0, /**< Memory Protection Region 0. */
+	QM_MPR_1,     /**< Memory Protection Region 1. */
+	QM_MPR_2,     /**< Memory Protection Region 2. */
+	QM_MPR_3,     /**< Memory Protection Region 3. */
+	QM_MPR_NUM    /**< Number of Memory Protection Regions. */
+} qm_mpr_id_t;
+
+/** Memory Protection Region register map. */
+typedef struct {
+	QM_RW uint32_t mpr_cfg[4]; /**< MPR CFG */
+	QM_RW uint32_t mpr_vdata;  /**< MPR_VDATA  */
+	QM_RW uint32_t mpr_vsts;   /**< MPR_VSTS  */
+} qm_mpr_reg_t;
+
+/**
+ * MPR context type.
+ *
+ * Application should not modify the content.
+ * This structure is only intended to be used by the qm_mpr_save_context and
+ * qm_mpr_restore_context functions.
+ */
+typedef struct {
+	uint32_t mpr_cfg[QM_MPR_NUM]; /**< MPR Configuration Register. */
+} qm_mpr_context_t;
+
+#if (UNIT_TEST)
+qm_mpr_reg_t test_mpr;
+
+#define QM_MPR ((qm_mpr_reg_t *)(&test_mpr))
+
+#else
+
+#define QM_MPR_BASE (0xB0400000)
+#define QM_MPR ((qm_mpr_reg_t *)QM_MPR_BASE)
+
+#endif
+
+#define QM_MPR_RD_EN_OFFSET (20)
+#define QM_MPR_RD_EN_MASK 0x700000
+#define QM_MPR_WR_EN_OFFSET (24)
+#define QM_MPR_WR_EN_MASK 0x7000000
+#define QM_MPR_EN_LOCK_OFFSET (30)
+#define QM_MPR_EN_LOCK_MASK 0xC0000000
+#define QM_MPR_UP_BOUND_OFFSET (10)
+#define QM_MPR_VSTS_VALID BIT(31)
+/** @} */
+
+#define QM_OSC0_PD BIT(2)
+
+#define QM_CCU_EXTERN_DIV_OFFSET (3)
+#define QM_CCU_EXT_CLK_DIV_EN BIT(2)
+
+/**
+ * @name Peripheral Clock
+ * @{
+ */
+
+/** Peripheral clock type. */
+typedef enum {
+	CLK_PERIPH_REGISTER = BIT(0), /**< Peripheral Clock Gate Enable. */
+	CLK_PERIPH_CLK = BIT(1),      /**< Peripheral Clock Enable. */
+	CLK_PERIPH_I2C_M0 = BIT(2),   /**< I2C Master 0 Clock Enable. */
+	CLK_PERIPH_I2C_M1 = BIT(3),   /**< I2C Master 1 Clock Enable. */
+	CLK_PERIPH_SPI_S = BIT(4),    /**< SPI Slave Clock Enable. */
+	CLK_PERIPH_SPI_M0 = BIT(5),   /**< SPI Master 0 Clock Enable. */
+	CLK_PERIPH_SPI_M1 = BIT(6),   /**< SPI Master 1 Clock Enable. */
+	CLK_PERIPH_GPIO_INTERRUPT = BIT(7), /**< GPIO Interrupt Clock Enable. */
+	CLK_PERIPH_GPIO_DB = BIT(8),	/**< GPIO Debounce Clock Enable. */
+	CLK_PERIPH_I2S = BIT(9),	    /**< I2S Clock Enable. */
+	CLK_PERIPH_WDT_REGISTER = BIT(10),  /**< Watchdog Clock Enable. */
+	CLK_PERIPH_RTC_REGISTER = BIT(11),  /**< RTC Clock Gate Enable. */
+	CLK_PERIPH_PWM_REGISTER = BIT(12),  /**< PWM Clock Gate Enable. */
+	CLK_PERIPH_GPIO_REGISTER = BIT(13), /**< GPIO Clock Gate Enable. */
+	CLK_PERIPH_SPI_M0_REGISTER =
+	    BIT(14), /**< SPI Master 0 Clock Gate Enable. */
+	CLK_PERIPH_SPI_M1_REGISTER =
+	    BIT(15), /**< SPI Master 1 Clock Gate Enable. */
+	CLK_PERIPH_SPI_S_REGISTER =
+	    BIT(16), /**< SPI Slave Clock Gate Enable. */
+	CLK_PERIPH_UARTA_REGISTER = BIT(17), /**< UARTA Clock Gate Enable. */
+	CLK_PERIPH_UARTB_REGISTER = BIT(18), /**< UARTB Clock Gate Enable. */
+	CLK_PERIPH_I2C_M0_REGISTER =
+	    BIT(19), /**< I2C Master 0 Clock Gate Enable. */
+	CLK_PERIPH_I2C_M1_REGISTER =
+	    BIT(20), /**< I2C Master 1 Clock Gate Enable. */
+	CLK_PERIPH_I2S_REGISTER = BIT(21), /**< I2S Clock Gate Enable. */
+	CLK_PERIPH_ALL = 0x3FFFFF	  /**< Quark SE peripherals Mask. */
+} clk_periph_t;
+
+/* Default mask values */
+#define CLK_EXTERN_DIV_DEF_MASK (0xFFFFFFE3)
+#define CLK_SYS_CLK_DIV_DEF_MASK (0xFFFFFC7F)
+#define CLK_RTC_DIV_DEF_MASK (0xFFFFFF83)
+#define CLK_GPIO_DB_DIV_DEF_MASK (0xFFFFFFE1)
+#define CLK_PERIPH_DIV_DEF_MASK (0xFFFFFFF9)
+
+/** @} */
+
+/**
+ * @name DMA
+ * @{
+ */
+
+/** DMA instances. */
+typedef enum {
+	QM_DMA_0,  /**< DMA controller id. */
+	QM_DMA_NUM /**< Number of DMA controllers. */
+} qm_dma_t;
+
+/** DMA channel IDs. */
+typedef enum {
+	QM_DMA_CHANNEL_0 = 0, /**< DMA channel id for channel 0 */
+	QM_DMA_CHANNEL_1,     /**< DMA channel id for channel 1 */
+	QM_DMA_CHANNEL_2,     /**< DMA channel id for channel 2 */
+	QM_DMA_CHANNEL_3,     /**< DMA channel id for channel 3 */
+	QM_DMA_CHANNEL_4,     /**< DMA channel id for channel 4 */
+	QM_DMA_CHANNEL_5,     /**< DMA channel id for channel 5 */
+	QM_DMA_CHANNEL_6,     /**< DMA channel id for channel 6 */
+	QM_DMA_CHANNEL_7,     /**< DMA channel id for channel 7 */
+	QM_DMA_CHANNEL_NUM    /**< Number of DMA channels */
+} qm_dma_channel_id_t;
+
+/** DMA hardware handshake interfaces. */
+typedef enum {
+	DMA_HW_IF_UART_A_TX = 0x0,       /**< UART_A_TX */
+	DMA_HW_IF_UART_A_RX = 0x1,       /**< UART_A_RX */
+	DMA_HW_IF_UART_B_TX = 0x2,       /**< UART_B_TX*/
+	DMA_HW_IF_UART_B_RX = 0x3,       /**< UART_B_RX */
+	DMA_HW_IF_SPI_MASTER_0_TX = 0x4, /**< SPI_Master_0_TX */
+	DMA_HW_IF_SPI_MASTER_0_RX = 0x5, /**< SPI_Master_0_RX */
+	DMA_HW_IF_SPI_MASTER_1_TX = 0x6, /**< SPI_Master_1_TX */
+	DMA_HW_IF_SPI_MASTER_1_RX = 0x7, /**< SPI_Master_1_RX */
+	DMA_HW_IF_SPI_SLAVE_TX = 0x8,    /**< SPI_Slave_TX */
+	DMA_HW_IF_SPI_SLAVE_RX = 0x9,    /**< SPI_Slave_RX */
+	DMA_HW_IF_I2S_PLAYBACK = 0xa,    /**< I2S_Playback channel */
+	DMA_HW_IF_I2S_CAPTURE = 0xb,     /**< I2S_Capture channel */
+	DMA_HW_IF_I2C_MASTER_0_TX = 0xc, /**< I2C_Master_0_TX */
+	DMA_HW_IF_I2C_MASTER_0_RX = 0xd, /**< I2C_Master_0_RX */
+	DMA_HW_IF_I2C_MASTER_1_TX = 0xe, /**< I2C_Master_1_TX */
+	DMA_HW_IF_I2C_MASTER_1_RX = 0xf, /**< I2C_Master_1_RX */
+} qm_dma_handshake_interface_t;
+
+/** DMA channel register map. */
+typedef struct {
+	QM_RW uint32_t sar_low;		   /**< SAR */
+	QM_RW uint32_t sar_high;	   /**< SAR */
+	QM_RW uint32_t dar_low;		   /**< DAR */
+	QM_RW uint32_t dar_high;	   /**< DAR */
+	QM_RW uint32_t llp_low;		   /**< LLP */
+	QM_RW uint32_t llp_high;	   /**< LLP */
+	QM_RW uint32_t ctrl_low;	   /**< CTL */
+	QM_RW uint32_t ctrl_high;	  /**< CTL */
+	QM_RW uint32_t src_stat_low;       /**< SSTAT */
+	QM_RW uint32_t src_stat_high;      /**< SSTAT */
+	QM_RW uint32_t dst_stat_low;       /**< DSTAT */
+	QM_RW uint32_t dst_stat_high;      /**< DSTAT */
+	QM_RW uint32_t src_stat_addr_low;  /**< SSTATAR */
+	QM_RW uint32_t src_stat_addr_high; /**< SSTATAR */
+	QM_RW uint32_t dst_stat_addr_low;  /**< DSTATAR */
+	QM_RW uint32_t dst_stat_addr_high; /**< DSTATAR */
+	QM_RW uint32_t cfg_low;		   /**< CFG */
+	QM_RW uint32_t cfg_high;	   /**< CFG */
+	QM_RW uint32_t src_sg_low;	 /**< SGR */
+	QM_RW uint32_t src_sg_high;	/**< SGR */
+	QM_RW uint32_t dst_sg_low;	 /**< DSR */
+	QM_RW uint32_t dst_sg_high;	/**< DSR */
+} qm_dma_chan_reg_t;
+
+/* DMA channel control register offsets and masks. */
+#define QM_DMA_CTL_L_INT_EN_MASK BIT(0)
+#define QM_DMA_CTL_L_DST_TR_WIDTH_OFFSET (1)
+#define QM_DMA_CTL_L_DST_TR_WIDTH_MASK (0x7 << QM_DMA_CTL_L_DST_TR_WIDTH_OFFSET)
+#define QM_DMA_CTL_L_SRC_TR_WIDTH_OFFSET (4)
+#define QM_DMA_CTL_L_SRC_TR_WIDTH_MASK (0x7 << QM_DMA_CTL_L_SRC_TR_WIDTH_OFFSET)
+#define QM_DMA_CTL_L_DINC_OFFSET (7)
+#define QM_DMA_CTL_L_DINC_MASK (0x3 << QM_DMA_CTL_L_DINC_OFFSET)
+#define QM_DMA_CTL_L_SINC_OFFSET (9)
+#define QM_DMA_CTL_L_SINC_MASK (0x3 << QM_DMA_CTL_L_SINC_OFFSET)
+#define QM_DMA_CTL_L_DEST_MSIZE_OFFSET (11)
+#define QM_DMA_CTL_L_DEST_MSIZE_MASK (0x7 << QM_DMA_CTL_L_DEST_MSIZE_OFFSET)
+#define QM_DMA_CTL_L_SRC_MSIZE_OFFSET (14)
+#define QM_DMA_CTL_L_SRC_MSIZE_MASK (0x7 << QM_DMA_CTL_L_SRC_MSIZE_OFFSET)
+#define QM_DMA_CTL_L_TT_FC_OFFSET (20)
+#define QM_DMA_CTL_L_TT_FC_MASK (0x7 << QM_DMA_CTL_L_TT_FC_OFFSET)
+#define QM_DMA_CTL_L_LLP_DST_EN_MASK BIT(27)
+#define QM_DMA_CTL_L_LLP_SRC_EN_MASK BIT(28)
+#define QM_DMA_CTL_H_BLOCK_TS_OFFSET (0)
+#define QM_DMA_CTL_H_BLOCK_TS_MASK (0xfff << QM_DMA_CTL_H_BLOCK_TS_OFFSET)
+#define QM_DMA_CTL_H_BLOCK_TS_MAX 4095
+#define QM_DMA_CTL_H_BLOCK_TS_MIN 1
+
+/* DMA channel config register offsets and masks. */
+#define QM_DMA_CFG_L_CH_SUSP_MASK BIT(8)
+#define QM_DMA_CFG_L_FIFO_EMPTY_MASK BIT(9)
+#define QM_DMA_CFG_L_HS_SEL_DST_OFFSET 10
+#define QM_DMA_CFG_L_HS_SEL_DST_MASK BIT(QM_DMA_CFG_L_HS_SEL_DST_OFFSET)
+#define QM_DMA_CFG_L_HS_SEL_SRC_OFFSET 11
+#define QM_DMA_CFG_L_HS_SEL_SRC_MASK BIT(QM_DMA_CFG_L_HS_SEL_SRC_OFFSET)
+#define QM_DMA_CFG_L_DST_HS_POL_OFFSET 18
+#define QM_DMA_CFG_L_DST_HS_POL_MASK BIT(QM_DMA_CFG_L_DST_HS_POL_OFFSET)
+#define QM_DMA_CFG_L_SRC_HS_POL_OFFSET 19
+#define QM_DMA_CFG_L_SRC_HS_POL_MASK BIT(QM_DMA_CFG_L_SRC_HS_POL_OFFSET)
+#define QM_DMA_CFG_L_RELOAD_SRC_MASK BIT(30)
+#define QM_DMA_CFG_L_RELOAD_DST_MASK BIT(31)
+#define QM_DMA_CFG_H_DS_UPD_EN_OFFSET (5)
+#define QM_DMA_CFG_H_DS_UPD_EN_MASK BIT(QM_DMA_CFG_H_DS_UPD_EN_OFFSET)
+#define QM_DMA_CFG_H_SS_UPD_EN_OFFSET (6)
+#define QM_DMA_CFG_H_SS_UPD_EN_MASK BIT(QM_DMA_CFG_H_SS_UPD_EN_OFFSET)
+#define QM_DMA_CFG_H_SRC_PER_OFFSET (7)
+#define QM_DMA_CFG_H_SRC_PER_MASK (0xf << QM_DMA_CFG_H_SRC_PER_OFFSET)
+#define QM_DMA_CFG_H_DEST_PER_OFFSET (11)
+#define QM_DMA_CFG_H_DEST_PER_MASK (0xf << QM_DMA_CFG_H_DEST_PER_OFFSET)
+
+/** DMA interrupt register map. */
+typedef struct {
+	QM_RW uint32_t raw_tfr_low;	   /**< RawTfr */
+	QM_RW uint32_t raw_tfr_high;	  /**< RawTfr */
+	QM_RW uint32_t raw_block_low;	 /**< RawBlock */
+	QM_RW uint32_t raw_block_high;	/**< RawBlock */
+	QM_RW uint32_t raw_src_trans_low;     /**< RawSrcTran */
+	QM_RW uint32_t raw_src_trans_high;    /**< RawSrcTran */
+	QM_RW uint32_t raw_dst_trans_low;     /**< RawDstTran */
+	QM_RW uint32_t raw_dst_trans_high;    /**< RawDstTran */
+	QM_RW uint32_t raw_err_low;	   /**< RawErr */
+	QM_RW uint32_t raw_err_high;	  /**< RawErr */
+	QM_RW uint32_t status_tfr_low;	/**< StatusTfr */
+	QM_RW uint32_t status_tfr_high;       /**< StatusTfr */
+	QM_RW uint32_t status_block_low;      /**< StatusBlock */
+	QM_RW uint32_t status_block_high;     /**< StatusBlock */
+	QM_RW uint32_t status_src_trans_low;  /**< StatusSrcTran */
+	QM_RW uint32_t status_src_trans_high; /**< StatusSrcTran */
+	QM_RW uint32_t status_dst_trans_low;  /**< StatusDstTran */
+	QM_RW uint32_t status_dst_trans_high; /**< StatusDstTran */
+	QM_RW uint32_t status_err_low;	/**< StatusErr */
+	QM_RW uint32_t status_err_high;       /**< StatusErr */
+	QM_RW uint32_t mask_tfr_low;	  /**< MaskTfr */
+	QM_RW uint32_t mask_tfr_high;	 /**< MaskTfr */
+	QM_RW uint32_t mask_block_low;	/**< MaskBlock */
+	QM_RW uint32_t mask_block_high;       /**< MaskBlock */
+	QM_RW uint32_t mask_src_trans_low;    /**< MaskSrcTran */
+	QM_RW uint32_t mask_src_trans_high;   /**< MaskSrcTran */
+	QM_RW uint32_t mask_dst_trans_low;    /**< MaskDstTran */
+	QM_RW uint32_t mask_dst_trans_high;   /**< MaskDstTran */
+	QM_RW uint32_t mask_err_low;	  /**< MaskErr */
+	QM_RW uint32_t mask_err_high;	 /**< MaskErr */
+	QM_RW uint32_t clear_tfr_low;	 /**< ClearTfr */
+	QM_RW uint32_t clear_tfr_high;	/**< ClearTfr */
+	QM_RW uint32_t clear_block_low;       /**< ClearBlock */
+	QM_RW uint32_t clear_block_high;      /**< ClearBlock */
+	QM_RW uint32_t clear_src_trans_low;   /**< ClearSrcTran */
+	QM_RW uint32_t clear_src_trans_high;  /**< ClearSrcTran */
+	QM_RW uint32_t clear_dst_trans_low;   /**< ClearDstTran */
+	QM_RW uint32_t clear_dst_trans_high;  /**< ClearDstTran */
+	QM_RW uint32_t clear_err_low;	 /**< ClearErr */
+	QM_RW uint32_t clear_err_high;	/**< ClearErr */
+	QM_RW uint32_t status_int_low;	/**< StatusInt */
+	QM_RW uint32_t status_int_high;       /**< StatusInt */
+} qm_dma_int_reg_t;
+
+/* DMA interrupt status register bits. */
+#define QM_DMA_INT_STATUS_TFR BIT(0)
+#define QM_DMA_INT_STATUS_BLOCK BIT(1)
+#define QM_DMA_INT_STATUS_ERR BIT(4)
+
+/** DMA miscellaneous register map. */
+typedef struct {
+	QM_RW uint32_t cfg_low;      /**< DmaCfgReg */
+	QM_RW uint32_t cfg_high;     /**< DmaCfgReg */
+	QM_RW uint32_t chan_en_low;  /**< ChEnReg */
+	QM_RW uint32_t chan_en_high; /**< ChEnReg */
+	QM_RW uint32_t id_low;       /**< DmaIdReg */
+	QM_RW uint32_t id_high;      /**< DmaIdReg */
+	QM_RW uint32_t test_low;     /**< DmaTestReg */
+	QM_RW uint32_t test_high;    /**< DmaTestReg */
+	QM_RW uint32_t reserved[4];  /**< Reserved */
+} qm_dma_misc_reg_t;
+
+/* Channel write enable in the misc channel enable register. */
+#define QM_DMA_MISC_CHAN_EN_WE_OFFSET (8)
+
+/* Controller enable bit in the misc config register. */
+#define QM_DMA_MISC_CFG_DMA_EN BIT(0)
+
+typedef struct {
+	QM_RW qm_dma_chan_reg_t chan_reg[8]; /**< Channel Register */
+	QM_RW qm_dma_int_reg_t int_reg;      /**< Interrupt Register */
+	QM_RW uint32_t reserved[12];	 /**< Reserved (SW HS) */
+	QM_RW qm_dma_misc_reg_t misc_reg;    /**< Miscellaneous Register */
+} qm_dma_reg_t;
+
+/**
+ * DMA context type.
+ *
+ * Applications should not modify the content.
+ * This structure is only intended to be used by
+ * the qm_dma_save_context and qm_dma_restore_context functions.
+ */
+typedef struct {
+	struct {
+		uint32_t ctrl_low; /**< Channel Control Lower. */
+		uint32_t cfg_low;  /**< Channel Configuration Lower. */
+		uint32_t cfg_high; /**< Channel Configuration Upper. */
+		uint32_t llp_low;  /**< Channel Linked List Pointer. */
+	} channel[QM_DMA_CHANNEL_NUM];
+	uint32_t misc_cfg_low; /**< DMA Configuration. */
+} qm_dma_context_t;
+
+#if (UNIT_TEST)
+qm_dma_reg_t test_dma_instance[QM_DMA_NUM];
+qm_dma_reg_t *test_dma[QM_DMA_NUM];
+#define QM_DMA test_dma
+#else
+#define QM_DMA_BASE (0xB0700000)
+extern qm_dma_reg_t *qm_dma[QM_DMA_NUM];
+#define QM_DMA qm_dma
+#endif
+
+/** @} */
+
+/**
+ * @name USB
+ * @{
+ */
+
+#define QM_USB_EP_DIR_IN_MASK (0x80)
+#define QM_USB_IN_EP_NUM (6)
+#define QM_USB_OUT_EP_NUM (4)
+#define QM_USB_MAX_PACKET_SIZE (64)
+
+/** Number of USB controllers. */
+typedef enum { QM_USB_0 = 0, QM_USB_NUM } qm_usb_t;
+
+typedef enum {
+	QM_USB_IN_EP_0 = 0,
+	QM_USB_IN_EP_1 = 1,
+	QM_USB_IN_EP_2 = 2,
+	QM_USB_IN_EP_3 = 3,
+	QM_USB_IN_EP_4 = 4,
+	QM_USB_IN_EP_5 = 5,
+	QM_USB_OUT_EP_0 = 6,
+	QM_USB_OUT_EP_1 = 7,
+	QM_USB_OUT_EP_2 = 8,
+	QM_USB_OUT_EP_3 = 9
+} qm_usb_ep_idx_t;
+
+/** USB register map. */
+
+/** IN Endpoint Registers. */
+typedef struct {
+	QM_RW uint32_t diepctl;
+	QM_R uint32_t reserved;
+	QM_RW uint32_t diepint;
+	QM_R uint32_t reserved1;
+	QM_RW uint32_t dieptsiz;
+	QM_RW uint32_t diepdma;
+	QM_RW uint32_t dtxfsts;
+	QM_R uint32_t reserved2;
+} qm_usb_in_ep_reg_t;
+
+/** OUT Endpoint Registers. */
+typedef struct {
+	QM_RW uint32_t doepctl;
+	QM_R uint32_t reserved;
+	QM_RW uint32_t doepint;
+	QM_R uint32_t reserved1;
+	QM_RW uint32_t doeptsiz;
+	QM_RW uint32_t doepdma;
+	QM_R uint32_t reserved2[2];
+} qm_usb_out_ep_reg_t;
+
+/**
+ * USB Register block type.
+ */
+typedef struct {
+	QM_RW uint32_t gotgctl;  /**< OTG Control. */
+	QM_RW uint32_t gotgint;  /**< OTG Interrupt. */
+	QM_RW uint32_t gahbcfg;  /**< AHB Configuration. */
+	QM_RW uint32_t gusbcfg;  /**< USB Configuration. */
+	QM_RW uint32_t grstctl;  /**< Reset Register. */
+	QM_RW uint32_t gintsts;  /**< Interrupt Status. */
+	QM_RW uint32_t gintmsk;  /**< Interrupt Mask. */
+	QM_R uint32_t grxstsr;   /**< Receive Status Read/Pop. */
+	QM_R uint32_t grxstsp;   /**< Receive Status Read/Pop. */
+	QM_R uint32_t grxfsiz;   /**< Receive FIFO Size. */
+	QM_R uint32_t gnptxfsiz; /**< Non-periodic Transmit FIFO Size. */
+	QM_R uint32_t reserved[5];
+	QM_R uint32_t gsnpsid;    /**< Synopsys ID. */
+	QM_R uint32_t ghwcfg1;    /**< HW config - Endpoint direction. */
+	QM_R uint32_t ghwcfg2;    /**< HW config 2. */
+	QM_R uint32_t ghwcfg3;    /**< HW config 3. */
+	QM_R uint32_t ghwcfg4;    /**< HW config 4. */
+	QM_RW uint32_t gdfifocfg; /**< Global DFIFO Configuration. */
+	QM_R uint32_t reserved1[43];
+	QM_RW uint32_t dieptxf1;
+	QM_RW uint32_t dieptxf2;
+	QM_RW uint32_t dieptxf3;
+	QM_RW uint32_t dieptxf4;
+	QM_RW uint32_t dieptxf5;
+	QM_R uint32_t reserved2[442];
+	QM_RW uint32_t dcfg; /**< Device config. */
+	QM_RW uint32_t dctl; /**< Device control. */
+	QM_RW uint32_t dsts; /**< Device Status. */
+	QM_R uint32_t reserved3;
+	QM_RW uint32_t diepmsk;  /**< IN EP Common Interrupt Mask. */
+	QM_RW uint32_t doepmsk;  /**< OUT EP Common Interrupt Mask. */
+	QM_R uint32_t daint;     /**< Device Interrupt Register. */
+	QM_RW uint32_t daintmsk; /**< Device Interrupt Mask Register. */
+	QM_R uint32_t reserved4[2];
+	QM_RW uint32_t dvbusdis;   /**< VBUS discharge time register. */
+	QM_RW uint32_t dvbuspulse; /**< Device VBUS discharge time. */
+	QM_RW uint32_t dthrctl;    /**< Device Threshold Ctrl. */
+	QM_RW uint32_t diepempmsk; /**< IN EP FIFO Empty Intr Mask. */
+	QM_R uint32_t reserved5[50];
+	qm_usb_in_ep_reg_t in_ep_reg[QM_USB_IN_EP_NUM];
+	QM_R uint32_t reserved6[80];
+	qm_usb_out_ep_reg_t out_ep_reg[QM_USB_OUT_EP_NUM];
+} qm_usb_reg_t;
+
+#if (UNIT_TEST)
+qm_usb_reg_t test_usb;
+#define QM_USB ((qm_usb_reg_t *)(&test_usb))
+#else
+#define QM_USB_0_BASE (0xB0500000)
+/* USB controller base address */
+#define QM_USB ((qm_usb_reg_t *)QM_USB_0_BASE)
+#endif
+
+/* USB PLL enable bit */
+#define QM_USB_PLL_PDLD BIT(0)
+/* USB PLL has locked when this bit is 1 */
+#define QM_USB_PLL_LOCK BIT(14)
+/* Default values to setup the USB PLL */
+#define QM_USB_PLL_CFG0_DEFAULT (0x00001904)
+
+/* USB PLL register */
+#if (UNIT_TEST)
+uint32_t test_usb_pll;
+#define QM_USB_PLL_CFG0 (test_usb_pll)
+#else
+#define QM_USB_PLL_CFG0 (REG_VAL(0xB0800014))
+#endif
+
+/* USB clock enable bit */
+#define QM_CCU_USB_CLK_EN BIT(1)
+
+/** @} */
+
+/**
+ * @name Hardware Fixes
+ * @{
+ */
+
+/* Refer to "HARDWARE_ISSUES.rst" for fix description. */
+#define FIX_1 (1)
+
+/** @} */
+
+/**
+ * @name Versioning
+ * @{
+ */
+
+#if (UNIT_TEST)
+uint32_t test_rom_version;
+#define ROM_VERSION_ADDRESS &test_rom_version;
+#else
+#define ROM_VERSION_ADDRESS (0xFFFFFFEC);
+#endif
+
+/** @} */
+
+/** @} */
+
+#endif /* __REGISTERS_H__ */
diff --git a/libraries/CuriePowerManagement/src/qmsi/ss_power_states.h b/libraries/CuriePowerManagement/src/qmsi/ss_power_states.h
new file mode 100644
index 00000000..f4dd80b9
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/qmsi/ss_power_states.h
@@ -0,0 +1,295 @@
+/*
+ * Copyright (c) 2016, Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived from this
+ *    software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE INTEL CORPORATION OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __QM_SS_POWER_STATES_H__
+#define __QM_SS_POWER_STATES_H__
+
+#include "qm_sensor_regs.h"
+
+/**
+ * SS Power mode control for Quark SE Microcontrollers.
+ *
+ * @defgroup groupSSPower SS Power states
+ * @{
+ */
+
+/**
+ * Sensor Subsystem SS1 Timers mode type.
+ */
+typedef enum {
+	SS_POWER_CPU_SS1_TIMER_OFF = 0, /**< Disable SS Timers in SS1. */
+	SS_POWER_CPU_SS1_TIMER_ON       /**< Keep SS Timers enabled in SS1. */
+} ss_power_cpu_ss1_mode_t;
+
+/**
+ * Enable LPSS state entry.
+ *
+ * Put the SoC into LPSS on next C2/C2LP and SS2 state combination.<BR>
+ * This function needs to be called on the Sensor Core to
+ * Clock Gate ADC, I2C0, I2C1, SPI0 and SPI1 sensor peripherals.<BR>
+ * Clock Gating sensor peripherals is a requirement to enter LPSS state.<BR>
+ * After LPSS, ss_power_soc_lpss_disable needs to be called to
+ * restore clock gating.<BR>
+ *
+ * This needs to be called before any transition to C2/C2LP and SS2
+ * in order to enter LPSS.<BR>
+ * SoC Hybrid Clock is gated in this state.<BR>
+ * Core Well Clocks are gated.<BR>
+ * RTC is the only clock running.
+ *
+ * Possible SoC wake events are:
+ * 	- Low Power Comparator Interrupt
+ * 	- AON GPIO Interrupt
+ * 	- AON Timer Interrupt
+ * 	- RTC Interrupt
+ */
+//void ss_power_soc_lpss_enable(void);
+
+/**
+ * Enter SoC sleep state and restore after wake up.
+ *
+ * Put the ARC core into sleep state until next SoC wake event
+ * and continue execution after wake up where the application stopped.
+ *
+ * If the library is built with ENABLE_RESTORE_CONTEXT=1, then this function
+ * will use the arc_restore_addr to save restore trap address which brings back
+ * the ARC CPU to the point where this function was called.
+ * This means that applications should refrain from using them.
+ *
+ * This function calls qm_ss_save_context and qm_ss_restore_context
+ * in order to restore execution where it stopped.
+ * All power management transitions are done by power_soc_sleep().
+ */
+//void ss_power_soc_sleep_restore(void);
+/**
+ * Enter SoC sleep state and restore after wake up.
+ *
+ * Put the ARC core into sleep state until next SoC wake event
+ * and continue execution after wake up where the application stopped.
+ *
+ * If the library is built with ENABLE_RESTORE_CONTEXT=1, then this function
+ * will use the arc_restore_addr to save restore trap address which brings back
+ * the ARC CPU to the point where this function was called.
+ * This means that applications should refrain from using them.
+ *
+ * This function calls qm_ss_save_context and qm_ss_restore_context
+ * in order to restore execution where it stopped.
+ * All power management transitions are done by power_soc_deep_sleep().
+ */
+//void ss_power_soc_deep_sleep_restore(void);
+
+/**
+ * Save context, enter ARC SS1 power save state and restore after wake up.
+ *
+ * This routine is same as ss_power_soc_sleep_restore(), just instead of
+ * going to sleep it will go to SS1 power save state.
+ * Note: this function has a while(1) which will spin until we enter
+ * (and exit) sleep and the power state change will be managed by the other
+ * core.
+ */
+//void ss_power_sleep_wait(void);
+
+/**
+ * Enable the SENSOR startup restore flag.
+ */
+//void power_soc_set_ss_restore_flag(void);
+
+/**
+ * Disable LPSS state entry.
+ *
+ * Clear LPSS enable flag.<BR>
+ * Disable Clock Gating of ADC, I2C0, I2C1, SPI0 and SPI1 sensor
+ * peripherals.<BR>
+ * This will prevent entry in LPSS when cores are in C2/C2LP and SS2 states.
+ */
+//void ss_power_soc_lpss_disable(void);
+
+/**
+ * Enter Sensor SS1 state.
+ *
+ * Put the Sensor Subsystem into SS1.<BR>
+ * Processor Clock is gated in this state.
+ *
+ * A wake event causes the Sensor Subsystem to transition to SS0.<BR>
+ * A wake event is a sensor subsystem interrupt.
+ *
+ * According to the mode selected, Sensor Subsystem Timers can be disabled.
+ *
+ * @param[in] mode Mode selection for SS1 state.
+ */
+//void ss_power_cpu_ss1(const ss_power_cpu_ss1_mode_t mode);
+
+/**
+ * Enter Sensor SS2 state or SoC LPSS state.
+ *
+ * Put the Sensor Subsystem into SS2.<BR>
+ * Sensor Complex Clock is gated in this state.<BR>
+ * Sensor Peripherals are gated in this state.<BR>
+ *
+ * This enables entry in LPSS if:
+ *  - Sensor Subsystem is in SS2
+ *  - Lakemont is in C2 or C2LP
+ *  - LPSS entry is enabled
+ *
+ * A wake event causes the Sensor Subsystem to transition to SS0.<BR>
+ * There are two kinds of wake event depending on the Sensor Subsystem
+ * and SoC state:
+ *  - SS2: a wake event is a Sensor Subsystem interrupt
+ *  - LPSS: a wake event is a Sensor Subsystem interrupt or a Lakemont interrupt
+ *
+ * LPSS wake events apply if LPSS is entered.
+ * If Host wakes the SoC from LPSS,
+ * Sensor also transitions back to SS0.
+ */
+//void ss_power_cpu_ss2(void);
+
+/**
+ * Save resume vector.
+ *
+ * Saves the resume vector in the global "arc_restore_addr" location.
+ * The ARC will jump to the resume vector once a wake up event is
+ * triggered and x86 resumes the ARC.
+ */
+#define qm_ss_set_resume_vector(_restore_label, arc_restore_addr)              \
+	__asm__ __volatile__("mov r0, @arc_restore_addr\n\t"                   \
+			     "st " #_restore_label ", [r0]\n\t"                \
+			     : /* Output operands. */                          \
+			     : /* Input operands. */                           \
+			     : /* Clobbered registers list. */                 \
+			     "r0")
+
+/* Save execution context.
+ *
+ * This routine saves CPU registers onto cpu_context,
+ * array.
+ *
+ */
+#define qm_ss_save_context(cpu_context)                                        \
+	__asm__ __volatile__("push_s r0\n\t"                                   \
+			     "mov r0, @cpu_context\n\t"                        \
+			     "st r1, [r0, 4]\n\t"                              \
+			     "st r2, [r0, 8]\n\t"                              \
+			     "st r3, [r0, 12]\n\t"                             \
+			     "st r4, [r0, 16]\n\t"                             \
+			     "st r5, [r0, 20]\n\t"                             \
+			     "st r6, [r0, 24]\n\t"                             \
+			     "st r7, [r0, 28]\n\t"                             \
+			     "st r8, [r0, 32]\n\t"                             \
+			     "st r9, [r0, 36]\n\t"                             \
+			     "st r10, [r0, 40]\n\t"                            \
+			     "st r11, [r0, 44]\n\t"                            \
+			     "st r12, [r0, 48]\n\t"                            \
+			     "st r13, [r0, 52]\n\t"                            \
+			     "st r14, [r0, 56]\n\t"                            \
+			     "st r15, [r0, 60]\n\t"                            \
+			     "st r16, [r0, 64]\n\t"                            \
+			     "st r17, [r0, 68]\n\t"                            \
+			     "st r18, [r0, 72]\n\t"                            \
+			     "st r19, [r0, 76]\n\t"                            \
+			     "st r20, [r0, 80]\n\t"                            \
+			     "st r21, [r0, 84]\n\t"                            \
+			     "st r22, [r0, 88]\n\t"                            \
+			     "st r23, [r0, 92]\n\t"                            \
+			     "st r24, [r0, 96]\n\t"                            \
+			     "st r25, [r0, 100]\n\t"                           \
+			     "st r26, [r0, 104]\n\t"                           \
+			     "st r27, [r0, 108]\n\t"                           \
+			     "st r28, [r0, 112]\n\t"                           \
+			     "st r29, [r0, 116]\n\t"                           \
+			     "st r30, [r0, 120]\n\t"                           \
+			     "st r31, [r0, 124]\n\t"                           \
+			     "lr r31, [ic_ctrl]\n\t"                           \
+			     "st r31, [r0, 128]\n\t"                           \
+			     : /* Output operands. */                          \
+			     : /* Input operands. */                           \
+			     [ic_ctrl] "i"(QM_SS_AUX_IC_CTRL)                  \
+			     : /* Clobbered registers list. */                 \
+			     "r0")
+
+/* Restore execution context.
+ *
+ * This routine restores CPU registers from cpu_context,
+ * array.
+ *
+ * This routine is called from the bootloader to restore the execution context
+ * from before entering in sleep mode.
+ */
+#define qm_ss_restore_context(_restore_label, cpu_context)                     \
+	__asm__ __volatile__(                                                  \
+	    #_restore_label                                                    \
+	    ":\n\t"                                                            \
+	    "mov r0, @cpu_context\n\t"                                         \
+	    "ld r1, [r0, 4]\n\t"                                               \
+	    "ld r2, [r0, 8]\n\t"                                               \
+	    "ld r3, [r0, 12]\n\t"                                              \
+	    "ld r4, [r0, 16]\n\t"                                              \
+	    "ld r5, [r0, 20]\n\t"                                              \
+	    "ld r6, [r0, 24]\n\t"                                              \
+	    "ld r7, [r0, 28]\n\t"                                              \
+	    "ld r8, [r0, 32]\n\t"                                              \
+	    "ld r9, [r0, 36]\n\t"                                              \
+	    "ld r10, [r0, 40]\n\t"                                             \
+	    "ld r11, [r0, 44]\n\t"                                             \
+	    "ld r12, [r0, 48]\n\t"                                             \
+	    "ld r13, [r0, 52]\n\t"                                             \
+	    "ld r14, [r0, 56]\n\t"                                             \
+	    "ld r15, [r0, 60]\n\t"                                             \
+	    "ld r16, [r0, 64]\n\t"                                             \
+	    "ld r17, [r0, 68]\n\t"                                             \
+	    "ld r18, [r0, 72]\n\t"                                             \
+	    "ld r19, [r0, 76]\n\t"                                             \
+	    "ld r20, [r0, 80]\n\t"                                             \
+	    "ld r21, [r0, 84]\n\t"                                             \
+	    "ld r22, [r0, 88]\n\t"                                             \
+	    "ld r23, [r0, 92]\n\t"                                             \
+	    "ld r24, [r0, 96]\n\t"                                             \
+	    "ld r25, [r0, 100]\n\t"                                            \
+	    "ld r26, [r0, 104]\n\t"                                            \
+	    "ld r27, [r0, 108]\n\t"                                            \
+	    "ld r28, [r0, 112]\n\t"                                            \
+	    "ld r29, [r0, 116]\n\t"                                            \
+	    "ld r30, [r0, 120]\n\t"                                            \
+	    "ld r31, [r0, 124]\n\t"                                            \
+	    "ld r0, [r0, 128]\n\t"                                             \
+	    "sr r0, [ic_ctrl]\n\t"                                             \
+	    "pop_s r0\n\t"                                                     \
+	    "sr 0,[0x101]\n\t" /* Setup Sensor Subsystem TimeStamp Counter */  \
+	    "sr 0,[0x100]\n\t"                                                 \
+	    "sr -1,[0x102]\n\t"                                                \
+	    : /* Output operands. */                                           \
+	    : /* Input operands. */                                            \
+	    [ic_ctrl] "i"(QM_SS_AUX_IC_CTRL)                                   \
+	    : /* Clobbered registers list. */                                  \
+	    "r0")
+
+/**
+ * @}
+ */
+
+#endif /* __QM_SS_POWER_STATES_H__ */
diff --git a/libraries/CuriePowerManagement/src/wsrc.c b/libraries/CuriePowerManagement/src/wsrc.c
new file mode 100644
index 00000000..abc8c1f2
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/wsrc.c
@@ -0,0 +1,172 @@
+#include <stdint.h>
+#include "dccm/dccm_alloc.h"
+#include "wsrc.h"
+#include "string.h"
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+static uint8_t new_index = 0;
+static uint8_t old_index = 0;
+static uint8_t num_active = 0;
+static wsrc_active_t wsrc_active[MAX_ATTACHABLE];
+
+wsrc_entry_t wsrc_table[NUM_WAKEUP] = {
+    { .irq = IRQ_GPIO1_INTR, .status = 0 },     /* I00 */
+    { .irq = IRQ_GPIO1_INTR, .status = 0 },     /* I01 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I02 */
+    { .irq = IRQ_GPIO1_INTR, .status = 0 },     /* I03 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I04 */
+    { .irq = IRQ_GPIO1_INTR, .status = 0 },     /* I05 */
+    { .irq = IRQ_GPIO1_INTR, .status = 0 },     /* I06 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I07 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I08 */
+    { .irq = IRQ_GPIO1_INTR, .status = 0 },     /* I09 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I010 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I011 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I012 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I013 */
+    { .irq = IRQ_GPIO0_INTR, .status = 0 },     /* I014 */
+    { .irq = IRQ_GPIO0_INTR, .status = 0 },     /* I015 */
+    { .irq = IRQ_GPIO0_INTR, .status = 0 },     /* I016 */
+    { .irq = IRQ_GPIO0_INTR, .status = 0 },     /* I017 */
+    { .irq = IRQ_GPIO0_INTR, .status = 0 },     /* I018 */
+    { .irq = IRQ_GPIO0_INTR, .status = 0 },     /* I019 */
+    { .irq = IRQ_GPIO0_INTR, .status = 0 },     /* I020 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I021 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I022 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I023 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I024 */
+    { .irq = IRQ_GPIO_INTR, .status = 0 },      /* I025 */
+    { .irq = IRQ_ALWAYS_ON_GPIO, .status = 1 }, /* IO26 AON_GPIO0*/
+    { .irq = IRQ_ALWAYS_ON_GPIO, .status = 1 }, /* IO27 AON_GPIO1*/
+    { .irq = IRQ_ALWAYS_ON_GPIO, .status = 1 }, /* IO28 AON_GPIO2*/
+    { .irq = IRQ_ALWAYS_ON_GPIO, .status = 1 }, /* IO29 AON_GPIO3*/
+    { .irq = IRQ_ALWAYS_ON_GPIO, .status = 1 }, /* BLE_WAKEUP */
+    { .irq = IRQ_ALWAYS_ON_GPIO, .status = 1 }, /* IMU_WAKEUP */
+    { .irq = IRQ_I2S_INTR, .status = 0 },       /* I2S_WAKEUP */
+    { .irq = IRQ_I2C0_RX_AVAIL, .status = 0 },  /* WIRE_RX_WAKEUP */
+    { .irq = IRQ_USB_INTR, .status = 0 },       /* SERIAL_WAKEUP */
+    { .irq = IRQ_UART1_INTR, .status = 0 },     /* SERIAL1_WAKEUP */
+    { .irq = IRQ_ADC_IRQ, .status = 0 },        /* ADC_WAKEUP */
+    { .irq = IRQ_ALWAYS_ON_TMR, .status = 0 },  /* AON_TIMER_WAKEUP */
+    { .irq = IRQ_TIMER1, .status = 0 },         /* TIMER1_WAKEUP */
+    { .irq = IRQ_PWM_TIMER_INTR, .status = 0 }, /* PWM_TIMER_WAKEUP */
+    { .irq = IRQ_SPI1_RX_AVAIL, .status = 0 },  /* SPI_RX_WAKEUP */
+    { .irq = IRQ_SPI0_RX_AVAIL, .status = 0 },  /* SPI1_RX_WAKEUP */
+    { .irq = IRQ_RTC_INTR, .status = 1 },       /* RTC_WAKEUP */
+    { .irq = IRQ_WDOG_INTR, .status = 0 },      /* WATCHDOG_WAKEUP */
+    { .irq = IRQ_MAILBOXES_INTR, .status = 0 }, /* MAILBOX_WAKEUP */
+    { .irq = IRQ_COMPARATORS_INTR, .status = 1},/* COMPARATORS_WAKEUP*/
+};
+
+static void wsrc_set_active (uint8_t index, void (*cb)(void))
+{
+    if (num_active < MAX_ATTACHABLE) {
+        wsrc_active[num_active].cb = (uint32_t)cb;
+        wsrc_active[num_active++].index = index;
+        ++new_index;
+    }
+}
+
+static void wsrc_clear_active (uint8_t index)
+{
+    unsigned int i, j;
+
+    for (i = 0; i < num_active; i++) {
+
+        /* Look for this value in wsrc_active ... */
+        if (wsrc_active[i].index == index) {
+
+            /* ... and shift over all remaining values that follow it */
+            for (j = i + 1; j < num_active; j++) {
+                wsrc_active[j - 1] = wsrc_active[j];
+            }
+            --num_active;
+            --new_index;
+            break;
+        }
+    }
+}
+
+int wsrc_getIndex(int id)
+{
+   for (int i = 0; i < num_active; i++) {
+       if (wsrc_active[i].index == id){
+           return i;
+       }
+   }
+   return 0; 
+}
+
+int wsrc_get_newest_attached (wsrc_t *wsrc)
+{
+    uint8_t i;
+    
+    if(num_active == 0){
+        return 0;
+    }
+    
+    if (new_index == 0 || !wsrc) {
+        new_index = num_active;
+        return 0;
+    }
+
+    i = wsrc_active[--new_index].index;
+
+    wsrc->irq = wsrc_table[i].irq;
+    wsrc->callback = wsrc_active[new_index].cb;
+    wsrc->status = wsrc_table[i].status;
+    wsrc->id = i;
+
+    return 1;
+}
+
+int wsrc_get_oldest_attached (wsrc_t *wsrc)
+{
+    uint8_t i;
+
+    if (old_index == num_active || !wsrc) {
+        old_index = 0;
+        return 0;
+    }
+
+    i = wsrc_active[old_index].index;
+
+    wsrc->irq = wsrc_table[i].irq;
+    wsrc->callback = wsrc_active[old_index++].cb;
+    wsrc->status = wsrc_table[i].status;
+    wsrc->id = i;
+
+    return 1;
+}
+
+void wsrc_register_gpio (uint32_t pin, void (*callback)(void), uint32_t mode)
+{
+    /* Set interrupt mode */
+    wsrc_set_gpio_mode(SRC_STAT(pin), mode);
+
+    if (!wsrc_wakeup(SRC_STAT(pin))) {
+        wsrc_set_active(pin, callback);
+        wsrc_set_wakeup(SRC_STAT(pin));
+    }
+}
+
+void wsrc_register_id (int id, void (*callback)(void))
+{
+    if (!wsrc_wakeup(SRC_STAT(id))) {
+        wsrc_set_active(id, callback);
+        wsrc_set_wakeup(SRC_STAT(id));
+    }
+}
+
+void wsrc_unregister (int id)
+{
+    wsrc_clear_active(id);
+    wsrc_clear_wakeup(SRC_STAT(id));
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/libraries/CuriePowerManagement/src/wsrc.h b/libraries/CuriePowerManagement/src/wsrc.h
new file mode 100644
index 00000000..d4e0fcb5
--- /dev/null
+++ b/libraries/CuriePowerManagement/src/wsrc.h
@@ -0,0 +1,113 @@
+#ifndef WSRC_H_
+#define WSRC_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#include "board.h"
+#include "pins_arduino.h"
+
+#define GPIO_START                     0
+#define GPIO_END                       (NUM_DIGITAL_PINS - 6)
+
+#define GPIO_MODE_MASK                 0x38
+#define SRC_STAT(src)                  (wsrc_table[src].status)
+
+#define MAX_ATTACHABLE                 10
+#define wsrc_cansleep(stat)            (stat & 1)
+#define wsrc_working(stat)             (stat & 2)
+#define wsrc_wakeup(stat)              (stat & 4)
+#define wsrc_set_cansleep(stat)        (stat |= 1)
+#define wsrc_clear_cansleep(stat)      (stat &= ~1)
+#define wsrc_set_working(stat)         (stat |= 2)
+#define wsrc_clear_working(stat)       (stat &= ~2)
+#define wsrc_set_wakeup(stat)          (stat |= 4)
+#define wsrc_clear_wakeup(stat)        (stat &= ~4)
+#define wsrc_gpio_mode(stat)           ((stat & GPIO_MODE_MASK) >> 3)
+#define wsrc_set_gpio_mode(stat, mode)({\
+                        stat &= ~GPIO_MODE_MASK;\ 
+                        stat |= (mode << 3);\
+                        });
+
+enum {
+    /* TODO: not all of these can actually be used as wakeup sources.
+     * find out which ones they are. */
+    AON_GPIO0 = GPIO_END ,
+    AON_GPIO1 = GPIO_END + 1,
+    AON_GPIO2 = GPIO_END + 2,
+    AON_GPIO3 = GPIO_END + 3 ,
+    IMU_WAKEUP = GPIO_END + 4,
+    BLE_WAKEUP = GPIO_END + 5,
+    I2S_WAKEUP = GPIO_END + 6,
+    WIRE_RX_WAKEUP = GPIO_END + 7,
+    SERIAL_WAKEUP = GPIO_END + 8,
+    SERIAL1_WAKEUP = GPIO_END + 9,
+    ADC_WAKEUP = GPIO_END + 10,
+    AON_TIMER_WAKEUP = GPIO_END + 11,
+    TIMER1_WAKEUP = GPIO_END + 12,
+    PWM_TIMER_WAKEUP = GPIO_END + 13,
+    SPI_RX_WAKEUP = GPIO_END + 14,
+    SPI1_RX_WAKEUP = GPIO_END + 15,
+    RTC_WAKEUP = GPIO_END + 16,
+    WATCHDOG_WAKEUP = GPIO_END + 17,
+    MAILBOX_WAKEUP = GPIO_END + 18,
+    COMPARATORS_WAKEUP = GPIO_END + 19,
+    NUM_WAKEUP = GPIO_END + 20
+};
+
+struct wsrc_entry {
+    /* IRQ associated with this wakeup source */
+    uint8_t irq;
+    /* Status flags;
+     *
+     * Bit 0:  sleep:         indicates if this wakeup source can be used in
+     *                        deeper sleep modes (if unset, can only be used in
+     *                        doze mode)
+     * Bit 1:  working:       indicates this entry is waiting for something
+     *                        to complete in interrupt context (e.g.
+     *                        interrupt-based UART transfer in progress)
+     * Bit 2:  wakeup:        indicates this entry is to be used as a wakeup
+     *                        source
+     * Bits 3-5: GPIO mode:   0 - LOW
+     *                        1 - HIGH
+     *                        2 - CHANGE
+     *                        3 - FALLING
+     *                        4 - RISING
+     *
+     * Bits 6-7: Reserved.    */
+    volatile uint8_t status;
+} __attribute__((packed));
+
+struct wsrc_active {
+    uint32_t cb;
+    uint8_t index;
+} __attribute__((packed));
+
+struct wsrc {
+    uint32_t callback;
+    uint8_t status;
+    uint8_t irq;
+    uint8_t id;
+};
+
+typedef struct wsrc wsrc_t;
+typedef struct wsrc_entry wsrc_entry_t;
+typedef struct wsrc_active wsrc_active_t;
+
+extern wsrc_entry_t wsrc_table[NUM_WAKEUP];
+
+void wsrc_table_init (void);
+void wsrc_register_gpio (uint32_t pin, void (*callback)(void), uint32_t mode);
+void wsrc_register_id (int id, void (*callback)(void));
+void wsrc_unregister (int id);
+int wsrc_getIndex(int id);
+
+int wsrc_get_newest_attached (wsrc_t *wsrc);
+int wsrc_get_oldest_attached (wsrc_t *wsrc);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/libraries/CurieSerialFlash/CurieSerialFlash.h b/libraries/CurieSerialFlash/CurieSerialFlash.h
new file mode 100644
index 00000000..47ce73d9
--- /dev/null
+++ b/libraries/CurieSerialFlash/CurieSerialFlash.h
@@ -0,0 +1,4 @@
+#if !defined (__arc__)
+#error These examples only work with onboard SPI flash on Arduino/Genuino 101 board
+#endif
+#include "SerialFlash.h"
diff --git a/libraries/SerialFlash/examples/CopyFromSerial/CopyFromSerial.ino b/libraries/CurieSerialFlash/examples/CopyFromSerial/CopyFromSerial.ino
similarity index 97%
rename from libraries/SerialFlash/examples/CopyFromSerial/CopyFromSerial.ino
rename to libraries/CurieSerialFlash/examples/CopyFromSerial/CopyFromSerial.ino
index d7c4f376..68a30af3 100644
--- a/libraries/SerialFlash/examples/CopyFromSerial/CopyFromSerial.ino
+++ b/libraries/CurieSerialFlash/examples/CopyFromSerial/CopyFromSerial.ino
@@ -57,9 +57,12 @@
  * Enjoy!
  *
  * SerialFlash library API: https://github.com/PaulStoffregen/SerialFlash
+ *
+ * This example depends on Paul Stoffregen's SerialFlash library.
+ * Download at https://github.com/PaulStoffregen/SerialFlash.
  */
 
-#include <SerialFlash.h>
+#include <CurieSerialFlash.h>
 #include <SPI.h>
 
 //Buffer sizes
@@ -79,14 +82,12 @@
 #define BYTE_ESCAPE      0x7d
 #define BYTE_SEPARATOR    0x7c
 
-#define CSPIN             21
-
 void setup(){
   Serial.begin(9600);  //Teensy serial is always at full USB speed and buffered... the baud rate here is required but ignored
 
   pinMode(13, OUTPUT);
   
-  SerialFlash.begin(CSPIN);
+  SerialFlash.begin(ONBOARD_FLASH_SPI_PORT, ONBOARD_FLASH_CS_PIN);
 
   //We start by formatting the flash...
   uint8_t id[5];
diff --git a/libraries/SerialFlash/examples/EraseEverything/EraseEverything.ino b/libraries/CurieSerialFlash/examples/EraseEverything/EraseEverything.ino
similarity index 90%
rename from libraries/SerialFlash/examples/EraseEverything/EraseEverything.ino
rename to libraries/CurieSerialFlash/examples/EraseEverything/EraseEverything.ino
index d3b3f4d7..43425324 100644
--- a/libraries/SerialFlash/examples/EraseEverything/EraseEverything.ino
+++ b/libraries/CurieSerialFlash/examples/EraseEverything/EraseEverything.ino
@@ -1,10 +1,9 @@
-//SerialFlash library API: https://github.com/PaulStoffregen/SerialFlash
+// This example depends on Paul Stoffregen's SerialFlash library
+// Download at https://github.com/PaulStoffregen/SerialFlash
 
-#include <SerialFlash.h>
+#include <CurieSerialFlash.h>
 #include <SPI.h>
 
-const int FlashChipSelect = 21; // digital pin for flash chip CS pin
-
 SerialFlashFile file;
 
 const unsigned long testIncrement = 4096;
@@ -26,7 +25,7 @@ void setup() {
   while (!Serial && (millis() - startMillis < 10000)) ;
   delay(100);
 
-  SerialFlash.begin(FlashChipSelect);
+  SerialFlash.begin(ONBOARD_FLASH_SPI_PORT, ONBOARD_FLASH_CS_PIN);
   unsigned char id[5];
   SerialFlash.readID(id);
   unsigned long size = SerialFlash.capacity(id);
diff --git a/libraries/SerialFlash/examples/FileWrite/FileWrite.ino b/libraries/CurieSerialFlash/examples/FileWrite/FileWrite.ino
similarity index 81%
rename from libraries/SerialFlash/examples/FileWrite/FileWrite.ino
rename to libraries/CurieSerialFlash/examples/FileWrite/FileWrite.ino
index dbeb438a..e9f5ab9a 100755
--- a/libraries/SerialFlash/examples/FileWrite/FileWrite.ino
+++ b/libraries/CurieSerialFlash/examples/FileWrite/FileWrite.ino
@@ -1,6 +1,7 @@
-//SerialFlash library API: https://github.com/PaulStoffregen/SerialFlash
+// This example depends on Paul Stoffregen's SerialFlash library
+// Download at https://github.com/PaulStoffregen/SerialFlash
 
-#include <SerialFlash.h>
+#include <CurieSerialFlash.h>
 #include <SPI.h>
 
 #define FSIZE 256
@@ -8,8 +9,6 @@
 const char *filename = "myfile.txt";
 const char *contents = "0123456789ABCDEF";
 
-const int FlashChipSelect = 21; // digital pin for flash chip CS pin
-
 void setup() {
   Serial.begin(9600);
 
@@ -18,7 +17,7 @@ void setup() {
   delay(100);
 
   // Init. SPI Flash chip
-  if (!SerialFlash.begin(FlashChipSelect)) {
+  if (!SerialFlash.begin(ONBOARD_FLASH_SPI_PORT, ONBOARD_FLASH_CS_PIN)) {
     Serial.println("Unable to access SPI Flash chip");
   }
 
@@ -47,4 +46,4 @@ bool create_if_not_exists (const char *filename) {
 }
 
 void loop() {
-}
+}
diff --git a/libraries/SerialFlash/examples/ListFiles/ListFiles.ino b/libraries/CurieSerialFlash/examples/ListFiles/ListFiles.ino
similarity index 84%
rename from libraries/SerialFlash/examples/ListFiles/ListFiles.ino
rename to libraries/CurieSerialFlash/examples/ListFiles/ListFiles.ino
index bc15b89b..6dfda8b7 100644
--- a/libraries/SerialFlash/examples/ListFiles/ListFiles.ino
+++ b/libraries/CurieSerialFlash/examples/ListFiles/ListFiles.ino
@@ -1,10 +1,9 @@
-// SerialFlash library API: https://github.com/PaulStoffregen/SerialFlash
+// This example depends on Paul Stoffregen's SerialFlash library
+// Download at https://github.com/PaulStoffregen/SerialFlash
 
-#include <SerialFlash.h>
+#include <CurieSerialFlash.h>
 #include <SPI.h>
 
-const int FlashChipSelect = 21; // digital pin for flash chip CS pin
-
 void setup() {
   //uncomment these if using Teensy audio shield
   //SPI.setSCK(14);  // Audio shield has SCK on pin 14
@@ -22,7 +21,7 @@ void setup() {
   delay(100);
   Serial.println("All Files on SPI Flash chip:");
 
-  if (!SerialFlash.begin(FlashChipSelect)) {
+  if (!SerialFlash.begin(ONBOARD_FLASH_SPI_PORT, ONBOARD_FLASH_CS_PIN)) {
     error("Unable to access SPI Flash chip");
   }
 
diff --git a/libraries/SerialFlash/examples/RawHardwareTest/RawHardwareTest.ino b/libraries/CurieSerialFlash/examples/RawHardwareTest/RawHardwareTest.ino
similarity index 98%
rename from libraries/SerialFlash/examples/RawHardwareTest/RawHardwareTest.ino
rename to libraries/CurieSerialFlash/examples/RawHardwareTest/RawHardwareTest.ino
index 74afa10a..a124010f 100644
--- a/libraries/SerialFlash/examples/RawHardwareTest/RawHardwareTest.ino
+++ b/libraries/CurieSerialFlash/examples/RawHardwareTest/RawHardwareTest.ino
@@ -18,13 +18,13 @@
 // https://github.com/PaulStoffregen/SerialFlash/issues
 // You MUST post the complete output of this program, and
 // the exact part number and manufacturer of the chip.
+//
+// This example depends on Paul Stoffregen's SerialFlash library
+// Download at https://github.com/PaulStoffregen/SerialFlash
 
-
-#include <SerialFlash.h>
+#include <CurieSerialFlash.h>
 #include <SPI.h>
 
-const int FlashChipSelect = 21; // digital pin for flash chip CS pin
-
 SerialFlashFile file;
 
 const unsigned long testIncrement = 4096;
@@ -46,7 +46,7 @@ void setup() {
   delay(100);
 
   Serial.println("Raw SerialFlash Hardware Test");
-  SerialFlash.begin(FlashChipSelect);
+  SerialFlash.begin(ONBOARD_FLASH_SPI_PORT, ONBOARD_FLASH_CS_PIN);
 
   if (test()) {
     Serial.println();
diff --git a/libraries/CurieSoftwareSerial/examples/SoftwareSerialExample/SoftwareSerialExample.ino b/libraries/CurieSoftwareSerial/examples/SoftwareSerialExample/SoftwareSerialExample.ino
index 14e88720..581f3c82 100644
--- a/libraries/CurieSoftwareSerial/examples/SoftwareSerialExample/SoftwareSerialExample.ino
+++ b/libraries/CurieSoftwareSerial/examples/SoftwareSerialExample/SoftwareSerialExample.ino
@@ -34,7 +34,7 @@ void setup()
   // Open serial communications and wait for port to open:
   Serial.begin(115200);
   while (!Serial) {
-    ; // wait for serial port to connect. Needed for Leonardo only
+    ; // wait for serial port to connect. Needed for Native USB only
   }
 
 
diff --git a/libraries/CurieSoftwareSerial/src/SoftwareSerial.cpp b/libraries/CurieSoftwareSerial/src/SoftwareSerial.cpp
index a72ee892..4d3a6365 100644
--- a/libraries/CurieSoftwareSerial/src/SoftwareSerial.cpp
+++ b/libraries/CurieSoftwareSerial/src/SoftwareSerial.cpp
@@ -31,14 +31,13 @@ Rx does not work for pin 13
 // 
 // Includes
 // 
-#include <Arduino.h>
 #include <SoftwareSerial.h>
 
 //
 // Statics
 //
 SoftwareSerial *SoftwareSerial::active_object = 0;
-char SoftwareSerial::_receive_buffer[_SS_MAX_RX_BUFF]; 
+char *SoftwareSerial::_receive_buffer;
 volatile uint8_t SoftwareSerial::_receive_buffer_tail = 0;
 volatile uint8_t SoftwareSerial::_receive_buffer_head = 0;
 
@@ -227,10 +226,9 @@ SoftwareSerial::SoftwareSerial(uint32_t receivePin, uint32_t transmitPin, bool i
   _inverse_logic(inverse_logic)
 {
   _inverse_logic = inverse_logic;
-  setTX(transmitPin);
   _transmitPin = transmitPin;
-  setRX(receivePin);
   _receivePin = receivePin;
+  _receive_buffer = (char*)dccm_malloc(_SS_MAX_RX_BUFF);
 }
 
 //
@@ -272,6 +270,9 @@ uint16_t SoftwareSerial::subtract_cap(uint16_t num, uint16_t sub) {
 
 void SoftwareSerial::begin(long speed)
 {
+  setTX(_transmitPin);
+  setRX(_receivePin);
+    
   _rx_delay_centering = _rx_delay_intrabit = _rx_delay_stopbit = _tx_delay = 0;
   //pre-calculate delays
   _bit_delay = (F_CPU/speed);
@@ -280,8 +281,8 @@ void SoftwareSerial::begin(long speed)
   {
     _isSOCGpio = true;
   }
-  //toggling a pin takes about 62 ticks
-  _tx_delay = _bit_delay - 62;
+  //toggling a pin takes about 68 ticks
+  _tx_delay = _bit_delay - 68;
   //reading a pin takes about 70 ticks
   _rx_delay_intrabit = _bit_delay - 70;
   //it takes about 272 ticks from when the start bit is received to when the ISR is called
diff --git a/libraries/CurieSoftwareSerial/src/SoftwareSerial.h b/libraries/CurieSoftwareSerial/src/SoftwareSerial.h
index 8e97cf79..1934d4e2 100644
--- a/libraries/CurieSoftwareSerial/src/SoftwareSerial.h
+++ b/libraries/CurieSoftwareSerial/src/SoftwareSerial.h
@@ -27,7 +27,7 @@ Rx does not work for pin 13
 
 #include <inttypes.h>
 #include <Stream.h>
-
+#include <Arduino.h>
 /******************************************************************************
 * Definitions
 ******************************************************************************/
@@ -56,7 +56,7 @@ class SoftwareSerial : public Stream
   bool _inverse_logic = false;
 
   // static data
-  static char _receive_buffer[_SS_MAX_RX_BUFF]; 
+  static char *_receive_buffer;
   static volatile uint8_t _receive_buffer_tail;
   static volatile uint8_t _receive_buffer_head;
   static SoftwareSerial *active_object;
diff --git a/libraries/CurieTime/examples/ReadTest/ReadTest.ino b/libraries/CurieTime/examples/ReadTest/ReadTest.ino
index 4483268f..12593f73 100644
--- a/libraries/CurieTime/examples/ReadTest/ReadTest.ino
+++ b/libraries/CurieTime/examples/ReadTest/ReadTest.ino
@@ -6,8 +6,8 @@
 #include <CurieTime.h>
 
 void setup() {
-  while (!Serial);
-  Serial.begin(9600);
+  Serial.begin(9600); // initialize Serial communication
+  while (!Serial);    // wait for serial port to connect.
 
   Serial.println("CurieTime Read Test");
   Serial.println("-------------------");
diff --git a/libraries/CurieTime/examples/SetTime/SetTime.ino b/libraries/CurieTime/examples/SetTime/SetTime.ino
index 97dbefc6..42d1a611 100644
--- a/libraries/CurieTime/examples/SetTime/SetTime.ino
+++ b/libraries/CurieTime/examples/SetTime/SetTime.ino
@@ -6,8 +6,8 @@
 #include <CurieTime.h>
 
 void setup() {
-  while (!Serial);
-  Serial.begin(9600);
+  Serial.begin(9600); // initialize Serial communication
+  while(!Serial) ;    // wait for serial port to connect.
 
   // set the current time to 14:27:00, December 14th, 2015
   setTime(14, 27, 00, 14, 12, 2015);
diff --git a/libraries/CurieTime/src/CurieTime.cpp b/libraries/CurieTime/src/CurieTime.cpp
index 8f6bdd00..197fd543 100644
--- a/libraries/CurieTime/src/CurieTime.cpp
+++ b/libraries/CurieTime/src/CurieTime.cpp
@@ -50,7 +50,7 @@ int year(unsigned long t)
 {
     struct tm* tm = tTm(t);
 
-    return (tm->tm_year + YEAR_OFFSET);
+    return (tm) ? (tm->tm_year + YEAR_OFFSET) : -1;
 }
 
 int month()
@@ -64,7 +64,7 @@ int month(unsigned long t)
 {
     struct tm* tm = tTm(t);
 
-    return (tm->tm_mon + MONTH_OFFSET);
+    return (tm) ? (tm->tm_mon + MONTH_OFFSET) : -1;
 }
 
 int day()
@@ -78,7 +78,7 @@ int day(unsigned long t)
 {
     struct tm* tm = tTm(t);
 
-    return tm->tm_mday;
+    return (tm) ? tm->tm_mday : -1;
 }
 
 int hour()
@@ -92,7 +92,7 @@ int hour(unsigned long t)
 {
     struct tm* tm = tTm(t);
 
-    return tm->tm_hour;
+    return (tm) ? tm->tm_hour : -1;
 }
 
 int minute()
@@ -106,7 +106,7 @@ int minute(unsigned long t)
 {
     struct tm* tm = tTm(t);
 
-    return tm->tm_min;
+    return (tm) ? tm->tm_min : -1;
 }
 
 int second()
@@ -120,7 +120,7 @@ int second(unsigned long t)
 {
     struct tm* tm = tTm(t);
 
-    return tm->tm_sec;
+    return (tm) ? tm->tm_sec : -1;
 }
 
 void setTime(unsigned long t)
diff --git a/libraries/CurieTimerOne/examples/CurieTimer1BlinkSpeed/CurieTimer1BlinkSpeed.ino b/libraries/CurieTimerOne/examples/CurieTimer1BlinkSpeed/CurieTimer1BlinkSpeed.ino
index 04147cf0..88f2ee7e 100644
--- a/libraries/CurieTimerOne/examples/CurieTimer1BlinkSpeed/CurieTimer1BlinkSpeed.ino
+++ b/libraries/CurieTimerOne/examples/CurieTimer1BlinkSpeed/CurieTimer1BlinkSpeed.ino
@@ -5,7 +5,8 @@ const int blinkPin = 13;
 void setup(void)
 {
   CurieTimerOne.initialize(50000);
-  Serial.begin(9600);
+  Serial.begin(9600); // initialize Serial communication
+  while(!Serial) ;    // wait for serial port to connect.
 }
 
 void loop(void)
diff --git a/libraries/CurieTimerOne/examples/CurieTimer1Interrupt/CurieTimer1Interrupt.ino b/libraries/CurieTimerOne/examples/CurieTimer1Interrupt/CurieTimer1Interrupt.ino
index 30750399..a7284b5b 100644
--- a/libraries/CurieTimerOne/examples/CurieTimer1Interrupt/CurieTimer1Interrupt.ino
+++ b/libraries/CurieTimerOne/examples/CurieTimer1Interrupt/CurieTimer1Interrupt.ino
@@ -38,8 +38,8 @@ void timedBlinkIsr()   // callback function when interrupt is asserted
 void setup() {
 
 #ifdef SERIAL_PORT_LOG_ENABLE
-  Serial.begin(115200);
-  while (!Serial);  //  wait for the serial monitor to open
+  Serial.begin(115200);  //  initialize Serial communication
+  while (!Serial);       //  wait for the serial monitor to open
 #endif
 
   // Initialize pin 13 as an output - onboard LED.
diff --git a/libraries/CurieTimerOne/keywords.txt b/libraries/CurieTimerOne/keywords.txt
index 734a16df..87562f6a 100644
--- a/libraries/CurieTimerOne/keywords.txt
+++ b/libraries/CurieTimerOne/keywords.txt
@@ -6,29 +6,29 @@
 # Datatypes (KEYWORD1)
 #######################################
 
-CurieTimer KEYWORD1
+CurieTimer	KEYWORD1
 
 #######################################
 # Methods and Functions (KEYWORD2)
 #######################################
-initialize      KEYWORD2
-setPeriod       KEYWORD2
-start           KEYWORD2
-stop            KEYWORD2
-restart         KEYWORD2
-resume          KEYWORD2
-setPwmDuty      KEYWORD2
-pwm             KEYWORD2
-disablePwm      KEYWORD2
-attachInterrupt KEYWORD2
-detachInterrupt KEYWORD2
-kill            KEYWORD2
-readTickCount   KEYWORD2
-rdRstTickCount  KEYWORD2
-pause           KEYWORD2
-pwmStart        KEYWORD2
-pwmStop         KEYWORD2
+initialize	KEYWORD2
+setPeriod	KEYWORD2
+start	KEYWORD2
+stop	KEYWORD2
+restart	KEYWORD2
+resume	KEYWORD2
+setPwmDuty	KEYWORD2
+pwm	KEYWORD2
+disablePwm	KEYWORD2
+attachInterrupt	KEYWORD2
+detachInterrupt	KEYWORD2
+kill	KEYWORD2
+readTickCount	KEYWORD2
+rdRstTickCount	KEYWORD2
+pause	KEYWORD2
+pwmStart	KEYWORD2
+pwmStop	KEYWORD2
 #######################################
 # Instances (KEYWORD2)
 #######################################
-CurieTimerOne   KEYWORD2
+CurieTimerOne	KEYWORD2
diff --git a/libraries/EEPROM/src/EEPROM.cpp b/libraries/EEPROM/src/EEPROM.cpp
new file mode 100644
index 00000000..de567eed
--- /dev/null
+++ b/libraries/EEPROM/src/EEPROM.cpp
@@ -0,0 +1,125 @@
+/*
+  EEPROM.h - EEPROM library
+  Original Copyright (c) 2006 David A. Mellis.  All right reserved.
+  New version by Christopher Andrews 2015.
+  Curie porting by Intel and Arduino LLC - 2016
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include "EEPROM.h"
+
+EEPROMClass EEPROM;
+
+void CurieClear()
+{
+  //erase the 2k bytes of the eeprom section inside the otp area
+  *(uint32_t*)(ROM_WR_CTRL) = 0x4002;
+  //wait for erase to be complete
+  #if 0
+  // TODO: wait for FLASH_STTS.ER_DONE to be set to 1
+  while(((*(uint32_t*)FLASH_STTS) & 0x01) == 0) {
+    delay(1);
+  }
+  #endif
+  delay(5);
+}
+
+void CurieRestoreMemory(uint32_t* buffer, uint32_t size)
+{
+  uint32_t rom_wr_ctrl = 0;
+  uint32_t address;
+
+  for (uint32_t i=0; i<size; i++) {
+
+    uint32_t data32 = buffer[i];
+    if (data32 == 0xFFFFFFFF) {
+      continue;
+    }
+
+    //store data into ROM_WR_DATA register
+    *(uint32_t*)(ROM_WR_DATA) = data32;
+    address = i * 4 + EEPROM_OFFSET;
+    //shift left 2 bits to store offset into bits 19:2 (WR_ADDR)
+    rom_wr_ctrl = (address)<<2;
+    rom_wr_ctrl |= 0x00000001; //set (WR_REQ) bit
+    *(uint32_t*)(ROM_WR_CTRL) = rom_wr_ctrl;
+
+    delay(5); //give it enough time to finish writing
+  }
+}
+
+uint8_t CurieRead8(uint32_t address)
+{
+  if((address > 0x7FF))
+  {
+    return 0;
+  }
+  int offset = address%4;
+  uint32_t value = *(uint32_t*)(EEPROM_ADDR+(address/4)*4);
+  value = (value >> ((3-offset)*8)) & 0xFF;
+  return (uint8_t)value;
+}
+
+uint32_t CurieRead32(uint32_t address)
+{
+  if((address > 0x7FF))
+  {
+    return 0;
+  }
+  uint32_t value = *(uint32_t*)(EEPROM_ADDR+(address/4)*4);
+  return value;
+}
+
+void CurieWrite8(uint32_t address, uint8_t data)
+{
+  //make sure address is valid
+  if((address > 0x7FF))
+  {
+    return;
+  }
+
+  uint8_t currentValue = CurieRead8(address);
+  //only do something if value is different from what is currently stored
+  if(currentValue==data)
+  {
+    return;
+  }
+
+  uint32_t currentDword = CurieRead32(address);
+
+  int offset = address%4;
+
+  uint32_t data32 = (currentDword & ~(uint32_t)(0xFF << ((3-offset)*8)));
+  data32 = data32 | (data << ((3-offset)*8));
+
+  if (currentValue != 0xFF) {
+    uint32_t dump[EEPROM_SIZE/4];
+    memcpy(dump, (uint32_t *)EEPROM_ADDR, EEPROM_SIZE);
+    dump[(address >> 2)] = data32;
+    CurieClear();
+    CurieRestoreMemory((uint32_t *)dump, EEPROM_SIZE/sizeof(uint32_t));
+    return;
+  }
+
+  uint32_t rom_wr_ctrl = 0;
+
+  //store data into ROM_WR_DATA register
+  *(uint32_t*)(ROM_WR_DATA) = data32;
+  address = ((address >> 2) << 2) + EEPROM_OFFSET;
+  //shift left 2 bits to store offset into bits 19:2 (WR_ADDR)
+  rom_wr_ctrl = (address)<<2;
+  rom_wr_ctrl |= 0x00000001; //set (WR_REQ) bit
+  *(uint32_t*)(ROM_WR_CTRL) = rom_wr_ctrl;
+
+  delay(5); //give it enough time to finish writing
+}
diff --git a/libraries/EEPROM/src/EEPROM.h b/libraries/EEPROM/src/EEPROM.h
index c52737c7..ac5e05f4 100644
--- a/libraries/EEPROM/src/EEPROM.h
+++ b/libraries/EEPROM/src/EEPROM.h
@@ -16,8 +16,8 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
-#ifndef EEPROM_h
-#define EEPROM_h
+#ifndef EEPROM_H
+#define EEPROM_H
 
 #define ROM_WR_CTRL 0xb0100004
 #define ROM_WR_DATA 0xb0100008
@@ -28,112 +28,17 @@
 
 #define EEPROM_SIZE 2048 //EEPROM size in bytes
 
-
 #include <inttypes.h>
 #include "Arduino.h"
 
 /* Curie specific implementation of "atomic" read8 and write8 on OTP flash storage */
 
-void CurieClear()
-{
-  //erase the 2k bytes of the eeprom section inside the otp area
-  *(uint32_t*)(ROM_WR_CTRL) = 0x4002;
-  //wait for erase to be complete
-  #if 0
-  while(((*(uint32_t*)FLASH_STTS) & 0x01) == 0) { // TODO: wait for FLASH_STTS.ER_DONE to be set to 1
-    delay(1);
-  }
-  #endif
-  delay(5);
-}
-
-void CurieRestoreMemory(uint32_t* buffer, uint32_t size)
-{
-  uint32_t rom_wr_ctrl = 0;
-  uint32_t address;
-
-  for (uint32_t i=0; i<size; i++) {
-
-    uint32_t data32 = buffer[i];
-    if (data32 == 0xFFFFFFFF) {
-      continue;
-    }
-
-    //store data into ROM_WR_DATA register
-    *(uint32_t*)(ROM_WR_DATA) = data32;
-    address = i * 4 + EEPROM_OFFSET;
-    rom_wr_ctrl = (address)<<2; //shift left 2 bits to store offset into bits 19:2 (WR_ADDR)
-    rom_wr_ctrl |= 0x00000001; //set (WR_REQ) bit
-    *(uint32_t*)(ROM_WR_CTRL) = rom_wr_ctrl;
-
-    delay(3); //give it enough time to finish writing
-  }
-}
-
-uint8_t CurieRead8(uint32_t address)
-{
-  if((address > 0x7FF))
-  {
-    return 0;
-  }
-  int offset = address%4;
-  uint32_t value = *(uint32_t*)(EEPROM_ADDR+(address/4)*4);
-  value = (value >> ((3-offset)*8)) & 0xFF;
-  return (uint8_t)value;
-}
-
-uint32_t CurieRead32(uint32_t address)
-{
-  if((address > 0x7FF))
-  {
-    return 0;
-  }
-  uint32_t value = *(uint32_t*)(EEPROM_ADDR+(address/4)*4);
-  return value;
-}
-
-void CurieWrite8(uint32_t address, uint8_t data)
-{
-  //make sure address is valid
-  if((address > 0x7FF))
-  {
-    return;
-  }
-
-  uint8_t currentValue = CurieRead8(address);
-  //only do something if value is different from what is currently stored
-  if(currentValue==data)
-  {
-    return;
-  }
-
-  uint32_t currentDword = CurieRead32(address);
-
-  int offset = address%4;
-
-  uint32_t data32 = (currentDword & ~(uint32_t)(0xFF << ((3-offset)*8)));
-  data32 = data32 | (data << ((3-offset)*8));
-
-  if (currentValue != 0xFF) {
-    uint32_t dump[EEPROM_SIZE/4];
-    memcpy(dump, (uint32_t *)EEPROM_ADDR, EEPROM_SIZE);
-    dump[(address >> 2)] = data32;
-    CurieClear();
-    CurieRestoreMemory((uint32_t *)dump, EEPROM_SIZE/sizeof(uint32_t));
-    return;
-  }
-
-  uint32_t rom_wr_ctrl = 0;
-
-  //store data into ROM_WR_DATA register
-  *(uint32_t*)(ROM_WR_DATA) = data32;
-  address = ((address >> 2) << 2) + EEPROM_OFFSET;
-  rom_wr_ctrl = (address)<<2; //shift left 2 bits to store offset into bits 19:2 (WR_ADDR)
-  rom_wr_ctrl |= 0x00000001; //set (WR_REQ) bit
-  *(uint32_t*)(ROM_WR_CTRL) = rom_wr_ctrl;
+void CurieClear();
+void CurieRestoreMemory(uint32_t* buffer, uint32_t size);
 
-  delay(3); //give it enough time to finish writing
-}
+uint8_t CurieRead8(uint32_t address);
+uint32_t CurieRead32(uint32_t address);
+void CurieWrite8(uint32_t address, uint8_t data);
 
 /***
     EERef class.
@@ -252,5 +157,5 @@ struct EEPROMClass{
     }
 };
 
-static EEPROMClass EEPROM;
-#endif
\ No newline at end of file
+extern EEPROMClass EEPROM;
+#endif
diff --git a/libraries/MemoryFree/README.md b/libraries/MemoryFree/README.md
new file mode 100644
index 00000000..d08aa930
--- /dev/null
+++ b/libraries/MemoryFree/README.md
@@ -0,0 +1,41 @@
+## MemoryFree library for Arduino/Genuino101 and tinyTILE
+
+This library is a re-write of http://playground.arduino.cc/Code/AvailableMemory
+specifically for Curie-based devices. This library defines the same header file
+`MemoryFree.h`, and provides the function `freeMemory()`. In addition, two extra
+functions are provided; `freeHeap()`, for heap space only, and `freeStack()`,
+for free stack space. `freeStack()` can also be used to detect a stack overflow.
+
+The rewrite is necessary for two reasons:
+
+* AVR-based boards use a different implementation of `malloc()` than Curie-based
+  boards, and in order to determine the amount of free heap space, we depend
+  on specific symbols defined by the `malloc()` implementation. The `malloc()`
+  implementation used by Curie-based devices
+  [can be seen here](https://github.com/foss-for-synopsys-dwc-arc-processors/glibc).
+
+* Curie-based boards have a different memory layout than AVR-based boards. See
+  the [linker script](https://github.com/01org/corelibs-arduino101/blob/master/variants/arduino_101/linker_scripts/flash.ld)
+  for Arduino/Genuino 101 for details of the memory layout
+  for Curie-based devices.
+
+## Functions
+
+
+`int freeHeap (void)`
+
+Returns the number of bytes free in the heap, i.e. the number of bytes free
+to be allocated using `malloc()`.
+
+`int freeStack (void)`
+
+Returns the number of bytes free in the stack, i.e. the space between the
+current stack frame and the end of the stack area. This function will return
+a negative number in the event of a stack overflow, representing the size of
+the overflow; for example, a return value of -20 means that the current stack
+frame is 20 bytes past the end of the stack area.
+
+`int freeMemory (void)`
+
+Returns the number of bytes free in both the stack and the heap. If a stack
+overflow has occurred, only the number of bytes free in the heap is returned.
diff --git a/libraries/MemoryFree/examples/freeMemory/freeMemory.ino b/libraries/MemoryFree/examples/freeMemory/freeMemory.ino
new file mode 100644
index 00000000..15c08d28
--- /dev/null
+++ b/libraries/MemoryFree/examples/freeMemory/freeMemory.ino
@@ -0,0 +1,50 @@
+/*
+ * freeMemory.ino: This sketch demonstrates the use of the freeMemory()
+ * function to measure the amount of free memory available in the system,
+ * before and after using 'malloc' to allocate some memory.
+ *
+ * Copyright (c) 2016 Intel Corporation.  All rights reserved.
+ * See the bottom of this file for the license terms.
+ */
+
+#include <MemoryFree.h>
+
+void setup () {
+    Serial.begin(9600);
+    while(!Serial);
+}
+
+void loop() {
+    char *p;
+
+    Serial.println("Free memory: " + String(freeMemory()));
+    Serial.println("Allocating 24 bytes ...");
+
+    p = (char *)malloc(24);
+    Serial.println("Free memory: " + String(freeMemory()));
+
+    Serial.println("Freeing 24 bytes ...");
+    free(p); 
+    Serial.println("Free memory: " + String(freeMemory()));
+
+    delay(2000);
+}
+
+/*
+ * Copyright (c) 2017 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
diff --git a/libraries/MemoryFree/keywords.txt b/libraries/MemoryFree/keywords.txt
new file mode 100644
index 00000000..9da291a1
--- /dev/null
+++ b/libraries/MemoryFree/keywords.txt
@@ -0,0 +1,11 @@
+#######################################
+# Syntax Coloring Map For MemoryFree
+#######################################
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+freeStack	KEYWORD2
+freeHeap	KEYWORD2
+freeMemory	KEYWORD2
diff --git a/libraries/MemoryFree/library.properties b/libraries/MemoryFree/library.properties
new file mode 100644
index 00000000..1acd530e
--- /dev/null
+++ b/libraries/MemoryFree/library.properties
@@ -0,0 +1,10 @@
+name=MemoryFree
+version=1.0
+author=Erik Nyquist
+maintainer=Erik Nyquist <eknyquist@gmail.com>
+sentence=Determines the amount of available memory in the heap
+paragraph=Determines the amount of memory, in bytes, that is available for allocation using malloc()
+category=Uncategorized
+url=
+architectures=arc32
+
diff --git a/libraries/MemoryFree/src/MemoryFree.cpp b/libraries/MemoryFree/src/MemoryFree.cpp
new file mode 100644
index 00000000..8fbba7ee
--- /dev/null
+++ b/libraries/MemoryFree/src/MemoryFree.cpp
@@ -0,0 +1,58 @@
+/*
+ * MemoryFree.cpp: taken from http://playground.arduino.cc/Code/AvailableMemory,
+ * re-written for the Arduino 101 which uses a different malloc implementation.
+ *
+ * Arduino 101 malloc source:
+ * https://github.com/foss-for-synopsys-dwc-arc-processors/glibc
+ *
+ * mallinfo() struct details:
+ * http://man7.org/linux/man-pages/man3/mallinfo.3.html
+ *
+ * Copyright (c) 2017 Intel Corporation.  All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#include <malloc.h>
+#include "MemoryFree.h"
+
+extern char __start_heap;
+extern char __end_heap;
+extern char __stack_size;
+extern char __stack_start;
+
+int freeStack() {
+    int stack_end;
+    int mark;
+
+    stack_end = ((int)&__stack_start) - ((int)&__stack_size);
+    return ((int)&mark) - stack_end;
+}
+
+int freeHeap (void) {
+    int hsize;
+    struct mallinfo mi;
+
+    mi = mallinfo();
+    hsize = (int)&__end_heap - (int)&__start_heap;
+    return (hsize - mi.arena) + mi.fordblks;
+}
+
+int freeMemory (void) {
+    int heap = freeHeap();
+    int stack = freeStack();
+    return (stack < 0) ? heap : stack + heap;
+}
diff --git a/libraries/MemoryFree/src/MemoryFree.h b/libraries/MemoryFree/src/MemoryFree.h
new file mode 100644
index 00000000..bf532bc2
--- /dev/null
+++ b/libraries/MemoryFree/src/MemoryFree.h
@@ -0,0 +1,48 @@
+/*
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifndef MEMORYFREE_H
+#define MEMORYFREE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* freeHeap: returns the size (in bytes) of unused space on the heap,
+ * i.e. the number of bytes available for allocation by 'malloc()' */
+int freeHeap(void);
+
+/* freeStack: returns the size (in bytes) of remaining free space in the stack,
+ * i.e. the difference between our current position in the stack, and the end
+ * of usable stack space.
+ *
+ * NOTE: This function will return a negative number to indicate a stack
+ * overflow, i.e. a return value of -20 means you have overrun the allocated
+ * stack area by 20 bytes. */
+int freeStack(void);
+
+/* freeMemory: returns the combined free memory in both the stack and heap,
+ * except in the case where a stack overflow has occurred (i.e. freeStack
+ * returns a negative number). In this case, only the amount of free heap
+ * space will be returned. */
+int freeMemory(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor.ino b/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor.ino
index 8104fcbc..4e5a2734 100644
--- a/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor.ino
+++ b/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor.ino
@@ -38,7 +38,8 @@ const int dataReadyPin = 6;
 const int chipSelectPin = 7;
 
 void setup() {
-  Serial.begin(9600);
+  Serial.begin(9600); // initialize Serial communication
+  while(!Serial) ;    // wait for serial port to connect.
 
   // start the SPI library:
   SPI.begin();
diff --git a/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino b/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino
index b135a74f..a06f1133 100644
--- a/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino
+++ b/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino
@@ -63,9 +63,11 @@ void loop() {
 void digitalPotWrite(int address, int value) {
   // take the SS pin low to select the chip:
   digitalWrite(slaveSelectPin, LOW);
+  delay(100);
   //  send in the address and value via SPI:
   SPI.transfer(address);
   SPI.transfer(value);
+  delay(100);
   // take the SS pin high to de-select the chip:
   digitalWrite(slaveSelectPin, HIGH);
 }
diff --git a/libraries/SPI/keywords.txt b/libraries/SPI/keywords.txt
index ff058279..05834754 100644
--- a/libraries/SPI/keywords.txt
+++ b/libraries/SPI/keywords.txt
@@ -7,7 +7,7 @@
 #######################################
 
 SPI	KEYWORD1
-SPI1    KEYWORD1
+SPI1	KEYWORD1
 
 #######################################
 # Methods and Functions (KEYWORD2)
diff --git a/libraries/SPI/src/SPI.cpp b/libraries/SPI/src/SPI.cpp
index 57644358..628756a8 100755
--- a/libraries/SPI/src/SPI.cpp
+++ b/libraries/SPI/src/SPI.cpp
@@ -94,10 +94,9 @@ void SPIClass::begin()
         SET_PIN_MODE(g_APinDescription[MOSI].ulSocPin, SPI_MUX_MODE);
         SET_PIN_MODE(g_APinDescription[MISO].ulSocPin, SPI_MUX_MODE);
         SET_PIN_MODE(g_APinDescription[SCK].ulSocPin,  SPI_MUX_MODE);
-        g_APinDescription[MOSI].ulPinMode = SPI_MUX_MODE;
-        g_APinDescription[MISO].ulPinMode = SPI_MUX_MODE;
-        g_APinDescription[SCK].ulPinMode  = SPI_MUX_MODE;
-
+        pinmuxMode[MISO] = SPI_MUX_MODE;
+        pinmuxMode[MOSI] = SPI_MUX_MODE;
+        pinmuxMode[SCK] = SPI_MUX_MODE;
     }
     initialized++; /* reference count */
     interrupt_unlock(flags);
diff --git a/libraries/SerialFlash/SerialFlash.h b/libraries/SerialFlash/SerialFlash.h
deleted file mode 100644
index e56df624..00000000
--- a/libraries/SerialFlash/SerialFlash.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/* SerialFlash Library - for filesystem-like access to SPI Serial Flash memory
- * https://github.com/PaulStoffregen/SerialFlash
- * Copyright (C) 2015, Paul Stoffregen, paul@pjrc.com
- *
- * Development of this library was funded by PJRC.COM, LLC by sales of Teensy.
- * Please support PJRC's efforts to develop open source software by purchasing
- * Teensy or other genuine PJRC products.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice, development funding notice, and this permission
- * notice shall be included in all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#ifndef SerialFlash_h_
-#define SerialFlash_h_
-
-#include <Arduino.h>
-#include <SPI.h>
-
-class SerialFlashFile;
-
-class SerialFlashChip
-{
-public:
-	static bool begin(uint8_t pin = 6);
-	static uint32_t capacity(const uint8_t *id);
-	static uint32_t blockSize();
-	static void sleep();
-	static void wakeup();
-	static void readID(uint8_t *buf);
-	static void readSerialNumber(uint8_t *buf);
-	static void read(uint32_t addr, void *buf, uint32_t len);
-	static bool ready();
-	static void wait();
-	static void write(uint32_t addr, const void *buf, uint32_t len);
-	static void eraseAll();
-	static void eraseBlock(uint32_t addr);
-
-	static SerialFlashFile open(const char *filename);
-	static bool create(const char *filename, uint32_t length, uint32_t align = 0);
-	static bool createErasable(const char *filename, uint32_t length) {
-		return create(filename, length, blockSize());
-	}
-	static bool exists(const char *filename);
-	static bool remove(const char *filename);
-	static bool remove(SerialFlashFile &file);
-	static void opendir() { dirindex = 0; }
-	static bool readdir(char *filename, uint32_t strsize, uint32_t &filesize);
-private:
-	static uint16_t dirindex; // current position for readdir()
-	static uint8_t flags;	// chip features
-	static uint8_t busy;	// 0 = ready
-				// 1 = suspendable program operation
-				// 2 = suspendable erase operation
-				// 3 = busy for realz!!
-};
-
-extern SerialFlashChip SerialFlash;
-
-
-class SerialFlashFile
-{
-public:
-	SerialFlashFile() : address(0) {
-	}
-	operator bool() {
-		if (address > 0) return true;
-		return false;
-	}
-	uint32_t read(void *buf, uint32_t rdlen) {
-		if (offset + rdlen > length) {
-			if (offset >= length) return 0;
-			rdlen = length - offset;
-		}
-		SerialFlash.read(address + offset, buf, rdlen);
-		offset += rdlen;
-		return rdlen;
-	}
-	uint32_t write(const void *buf, uint32_t wrlen) {
-		if (offset + wrlen > length) {
-			if (offset >= length) return 0;
-			wrlen = length - offset;
-		}
-		SerialFlash.write(address + offset, buf, wrlen);
-		offset += wrlen;
-		return wrlen;
-	}
-	void seek(uint32_t n) {
-		offset = n;
-	}
-	uint32_t position() {
-		return offset;
-	}
-	uint32_t size() {
-		return length;
-	}
-	uint32_t available() {
-		if (offset >= length) return 0;
-		return length - offset;
-	}
-	void erase();
-	void flush() {
-	}
-	void close() {
-	}
-	uint32_t getFlashAddress() {
-		return address;
-	}
-protected:
-	friend class SerialFlashChip;
-	uint32_t address;  // where this file's data begins in the Flash, or zero
-	uint32_t length;   // total length of the data in the Flash chip
-	uint32_t offset; // current read/write offset in the file
-	uint16_t dirindex;
-};
-
-
-#endif
diff --git a/libraries/SerialFlash/SerialFlashChip.cpp b/libraries/SerialFlash/SerialFlashChip.cpp
deleted file mode 100644
index 4e467f57..00000000
--- a/libraries/SerialFlash/SerialFlashChip.cpp
+++ /dev/null
@@ -1,523 +0,0 @@
-/* SerialFlash Library - for filesystem-like access to SPI Serial Flash memory
- * https://github.com/PaulStoffregen/SerialFlash
- * Copyright (C) 2015, Paul Stoffregen, paul@pjrc.com
- *
- * Development of this library was funded by PJRC.COM, LLC by sales of Teensy.
- * Please support PJRC's efforts to develop open source software by purchasing
- * Teensy or other genuine PJRC products.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice, development funding notice, and this permission
- * notice shall be included in all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "SerialFlash.h"
-#include "util/SerialFlash_directwrite.h"
-
-#define CSASSERT()  DIRECT_WRITE_LOW(cspin_basereg, cspin_bitmask)
-#define CSRELEASE() DIRECT_WRITE_HIGH(cspin_basereg, cspin_bitmask)
-#define SPICONFIG   SPISettings(50000000, MSBFIRST, SPI_MODE0)
-
-#if defined(__arc__)
-// Use SPI1 on Arduino 101 (accesses chip already on the board)
-#define SPIPORT SPI1
-#elif 0
-// Add cases here, if you wish to use other SPI ports...
-#else
-// Otherwise, use the normal SPI port.
-#define SPIPORT SPI
-#endif
-
-uint16_t SerialFlashChip::dirindex = 0;
-uint8_t SerialFlashChip::flags = 0;
-uint8_t SerialFlashChip::busy = 0;
-
-static volatile IO_REG_TYPE *cspin_basereg;
-static IO_REG_TYPE cspin_bitmask;
-
-#define FLAG_32BIT_ADDR		0x01	// larger than 16 MByte address
-#define FLAG_STATUS_CMD70	0x02	// requires special busy flag check
-#define FLAG_DIFF_SUSPEND	0x04	// uses 2 different suspend commands
-#define FLAG_MULTI_DIE		0x08	// multiple die, don't read cross 32M barrier
-#define FLAG_256K_BLOCKS	0x10	// has 256K erase blocks
-#define FLAG_DIE_MASK		0xC0	// top 2 bits count during multi-die erase
-
-void SerialFlashChip::wait(void)
-{
-	uint32_t status;
-	//Serial.print("wait-");
-	while (1) {
-		SPIPORT.beginTransaction(SPICONFIG);
-		CSASSERT();
-		if (flags & FLAG_STATUS_CMD70) {
-			// some Micron chips require this different
-			// command to detect program and erase completion
-			SPIPORT.transfer(0x70);
-			status = SPIPORT.transfer(0);
-			CSRELEASE();
-			SPIPORT.endTransaction();
-			//Serial.printf("b=%02x.", status & 0xFF);
-			if ((status & 0x80)) break;
-		} else {
-			// all others work by simply reading the status reg
-			SPIPORT.transfer(0x05);
-			status = SPIPORT.transfer(0);
-			CSRELEASE();
-			SPIPORT.endTransaction();
-			//Serial.printf("b=%02x.", status & 0xFF);
-			if (!(status & 1)) break;
-		}
-	}
-	busy = 0;
-	//Serial.println();
-}
-
-void SerialFlashChip::read(uint32_t addr, void *buf, uint32_t len)
-{
-	uint8_t *p = (uint8_t *)buf;
-	uint8_t b, f, status, cmd;
-
-	memset(p, 0, len);
-	f = flags;
-	SPIPORT.beginTransaction(SPICONFIG);
-	b = busy;
-	if (b) {
-		// read status register ... chip may no longer be busy
-		CSASSERT();
-		if (flags & FLAG_STATUS_CMD70) {
-			SPIPORT.transfer(0x70);
-			status = SPIPORT.transfer(0);
-			if ((status & 0x80)) b = 0;
-		} else {
-			SPIPORT.transfer(0x05);
-			status = SPIPORT.transfer(0);
-			if (!(status & 1)) b = 0;
-		}
-		CSRELEASE();
-		if (b == 0) {
-			// chip is no longer busy :-)
-			busy = 0;
-		} else if (b < 3) {
-			// TODO: this may not work on Spansion chips
-			// which apparently have 2 different suspend
-			// commands, for program vs erase
-			CSASSERT();
-			SPIPORT.transfer(0x06); // write enable (Micron req'd)
-			CSRELEASE();
-			delayMicroseconds(1);
-			cmd = 0x75; //Suspend program/erase for almost all chips
-			// but Spansion just has to be different for program suspend!
-			if ((f & FLAG_DIFF_SUSPEND) && (b == 1)) cmd = 0x85;
-			CSASSERT();
-			SPIPORT.transfer(cmd); // Suspend command
-			CSRELEASE();
-			if (f & FLAG_STATUS_CMD70) {
-				// Micron chips don't actually suspend until flags read
-				CSASSERT();
-				SPIPORT.transfer(0x70);
-				do {
-					status = SPIPORT.transfer(0);
-				} while (!(status & 0x80));
-				CSRELEASE();
-			} else {
-				CSASSERT();
-				SPIPORT.transfer(0x05);
-				do {
-					status = SPIPORT.transfer(0);
-				} while ((status & 0x01));
-				CSRELEASE();
-			}
-		} else {
-			// chip is busy with an operation that can not suspend
-			SPIPORT.endTransaction();	// is this a good idea?
-			wait();			// should we wait without ending
-			b = 0;			// the transaction??
-			SPIPORT.beginTransaction(SPICONFIG);
-		}
-	}
-	do {
-		uint32_t rdlen = len;
-		if (f & FLAG_MULTI_DIE) {
-			if ((addr & 0xFE000000) != ((addr + len - 1) & 0xFE000000)) {
-				rdlen = 0x2000000 - (addr & 0x1FFFFFF);
-			}
-		}
-		CSASSERT();
-		// TODO: FIFO optimize....
-		if (f & FLAG_32BIT_ADDR) {
-			SPIPORT.transfer(0x03);
-			SPIPORT.transfer16(addr >> 16);
-			SPIPORT.transfer16(addr);
-		} else {
-			SPIPORT.transfer16(0x0300 | ((addr >> 16) & 255));
-			SPIPORT.transfer16(addr);
-		}
-		SPIPORT.transfer(p, rdlen);
-		CSRELEASE();
-		p += rdlen;
-		addr += rdlen;
-		len -= rdlen;
-	} while (len > 0);
-	if (b) {
-		CSASSERT();
-		SPIPORT.transfer(0x06); // write enable (Micron req'd)
-		CSRELEASE();
-		delayMicroseconds(1);
-		cmd = 0x7A;
-		if ((f & FLAG_DIFF_SUSPEND) && (b == 1)) cmd = 0x8A;
-		CSASSERT();
-		SPIPORT.transfer(cmd); // Resume program/erase
-		CSRELEASE();
-	}
-	SPIPORT.endTransaction();
-}
-
-void SerialFlashChip::write(uint32_t addr, const void *buf, uint32_t len)
-{
-	const uint8_t *p = (const uint8_t *)buf;
-	uint32_t max, pagelen;
-
-	 //Serial.printf("WR: addr %08X, len %d\n", addr, len);
-	do {
-		if (busy) wait();
-		SPIPORT.beginTransaction(SPICONFIG);
-		CSASSERT();
-		// write enable command
-		SPIPORT.transfer(0x06);
-		CSRELEASE();
-		max = 256 - (addr & 0xFF);
-		pagelen = (len <= max) ? len : max;
-		 //Serial.printf("WR: addr %08X, pagelen %d\n", addr, pagelen);
-		delayMicroseconds(1); // TODO: reduce this, but prefer safety first
-		CSASSERT();
-		if (flags & FLAG_32BIT_ADDR) {
-			SPIPORT.transfer(0x02); // program page command
-			SPIPORT.transfer16(addr >> 16);
-			SPIPORT.transfer16(addr);
-		} else {
-			SPIPORT.transfer16(0x0200 | ((addr >> 16) & 255));
-			SPIPORT.transfer16(addr);
-		}
-		addr += pagelen;
-		len -= pagelen;
-		do {
-			SPIPORT.transfer(*p++);
-		} while (--pagelen > 0);
-		CSRELEASE();
-		busy = 4;
-		SPIPORT.endTransaction();
-	} while (len > 0);
-}
-
-void SerialFlashChip::eraseAll()
-{
-	if (busy) wait();
-	uint8_t id[5];
-	readID(id);
-	//Serial.printf("ID: %02X %02X %02X\n", id[0], id[1], id[2]);
-	if (id[0] == 0x20 && id[2] >= 0x20 && id[2] <= 0x22) {
-		// Micron's multi-die chips require special die erase commands
-		//  N25Q512A	20 BA 20  2 dies  32 Mbyte/die   65 nm transitors
-		//  N25Q00AA	20 BA 21  4 dies  32 Mbyte/die   65 nm transitors
-		//  MT25QL02GC	20 BA 22  2 dies  128 Mbyte/die  45 nm transitors
-		uint8_t die_count = 2;
-		if (id[2] == 0x21) die_count = 4;
-		uint8_t die_index = flags >> 6;
-		 //Serial.printf("Micron die erase %d\n", die_index);
-		flags &= 0x3F;
-		if (die_index >= die_count) return; // all dies erased :-)
-		uint8_t die_size = 2;  // in 16 Mbyte units
-		if (id[2] == 0x22) die_size = 8;
-		SPIPORT.beginTransaction(SPICONFIG);
-		CSASSERT();
-		SPIPORT.transfer(0x06); // write enable command
-		CSRELEASE();
-		 delayMicroseconds(1);
-		CSASSERT();
-		// die erase command
-		SPIPORT.transfer(0xC4);
-		SPIPORT.transfer16((die_index * die_size) << 8);
-		SPIPORT.transfer16(0x0000);
-		CSRELEASE();
-		 //Serial.printf("Micron erase begin\n");
-		flags |= (die_index + 1) << 6;
-	} else {
-		// All other chips support the bulk erase command
-		SPIPORT.beginTransaction(SPICONFIG);
-		CSASSERT();
-		// write enable command
-		SPIPORT.transfer(0x06);
-		CSRELEASE();
-		 delayMicroseconds(1);
-		CSASSERT();
-		// bulk erase command
-		SPIPORT.transfer(0xC7);
-		CSRELEASE();
-		SPIPORT.endTransaction();
-	}
-	busy = 3;
-}
-
-void SerialFlashChip::eraseBlock(uint32_t addr)
-{
-	uint8_t f = flags;
-	if (busy) wait();
-	SPIPORT.beginTransaction(SPICONFIG);
-	CSASSERT();
-	SPIPORT.transfer(0x06); // write enable command
-	CSRELEASE();
-	 delayMicroseconds(1);
-	CSASSERT();
-	if (f & FLAG_32BIT_ADDR) {
-		SPIPORT.transfer(0xD8);
-		SPIPORT.transfer16(addr >> 16);
-		SPIPORT.transfer16(addr);
-	} else {
-		SPIPORT.transfer16(0xD800 | ((addr >> 16) & 255));
-		SPIPORT.transfer16(addr);
-	}
-	CSRELEASE();
-	SPIPORT.endTransaction();
-	busy = 2;
-}
-
-
-bool SerialFlashChip::ready()
-{
-	uint32_t status;
-	if (!busy) return true;
-	SPIPORT.beginTransaction(SPICONFIG);
-	CSASSERT();
-	if (flags & FLAG_STATUS_CMD70) {
-		// some Micron chips require this different
-		// command to detect program and erase completion
-		SPIPORT.transfer(0x70);
-		status = SPIPORT.transfer(0);
-		CSRELEASE();
-		SPIPORT.endTransaction();
-		//Serial.printf("ready=%02x\n", status & 0xFF);
-		if ((status & 0x80) == 0) return false;
-	} else {
-		// all others work by simply reading the status reg
-		SPIPORT.transfer(0x05);
-		status = SPIPORT.transfer(0);
-		CSRELEASE();
-		SPIPORT.endTransaction();
-		//Serial.printf("ready=%02x\n", status & 0xFF);
-		if ((status & 1)) return false;
-	}
-	busy = 0;
-	if (flags & 0xC0) {
-		// continue a multi-die erase
-		eraseAll();
-		return false;
-	}
-	return true;
-}
-
-
-#define ID0_WINBOND	0xEF
-#define ID0_SPANSION	0x01
-#define ID0_MICRON	0x20
-#define ID0_MACRONIX	0xC2
-#define ID0_SST		0xBF
-
-//#define FLAG_32BIT_ADDR	0x01	// larger than 16 MByte address
-//#define FLAG_STATUS_CMD70	0x02	// requires special busy flag check
-//#define FLAG_DIFF_SUSPEND	0x04	// uses 2 different suspend commands
-//#define FLAG_256K_BLOCKS	0x10	// has 256K erase blocks
-
-bool SerialFlashChip::begin(uint8_t pin)
-{
-	uint8_t id[5];
-	uint8_t f;
-	uint32_t size;
-
-	cspin_basereg = PIN_TO_BASEREG(pin);
-	cspin_bitmask = PIN_TO_BITMASK(pin);
-	SPIPORT.begin();
-	pinMode(pin, OUTPUT);
-	CSRELEASE();
-	readID(id);
-	f = 0;
-	size = capacity(id);
-	if (size > 16777216) {
-		// more than 16 Mbyte requires 32 bit addresses
-		f |= FLAG_32BIT_ADDR;
-		SPIPORT.beginTransaction(SPICONFIG);
-		if (id[0] == ID0_SPANSION) {
-			// spansion uses MSB of bank register
-			CSASSERT();
-			SPIPORT.transfer16(0x1780); // bank register write
-			CSRELEASE();
-		} else {
-			// micron & winbond & macronix use command
-			CSASSERT();
-			SPIPORT.transfer(0x06); // write enable
-			CSRELEASE();
-			delayMicroseconds(1);
-			CSASSERT();
-			SPIPORT.transfer(0xB7); // enter 4 byte addr mode
-			CSRELEASE();
-		}
-		SPIPORT.endTransaction();
-		if (id[0] == ID0_MICRON) f |= FLAG_MULTI_DIE;
-	}
-	if (id[0] == ID0_SPANSION) {
-		// Spansion has separate suspend commands
-		f |= FLAG_DIFF_SUSPEND;
-		if (!id[4]) {
-			// Spansion chips with id[4] == 0 use 256K sectors
-			f |= FLAG_256K_BLOCKS;
-		}
-	}
-	if (id[0] == ID0_MICRON) {
-		// Micron requires busy checks with a different command
-		f |= FLAG_STATUS_CMD70; // TODO: all or just multi-die chips?
-	}
-	flags = f;
-	readID(id);
-	return true;
-}
-
-// chips tested: https://github.com/PaulStoffregen/SerialFlash/pull/12#issuecomment-169596992
-//
-void SerialFlashChip::sleep()
-{
-	if (busy) wait();
-	SPIPORT.beginTransaction(SPICONFIG);
-	CSASSERT();
-	SPIPORT.transfer(0xB9); // Deep power down command
-	CSRELEASE();
-}
-
-void SerialFlashChip::wakeup()
-{
-	SPIPORT.beginTransaction(SPICONFIG);
-	CSASSERT();
-	SPIPORT.transfer(0xAB); // Wake up from deep power down command
-	CSRELEASE();
-}
-
-void SerialFlashChip::readID(uint8_t *buf)
-{
-	if (busy) wait();
-	SPIPORT.beginTransaction(SPICONFIG);
-	CSASSERT();
-	SPIPORT.transfer(0x9F);
-	buf[0] = SPIPORT.transfer(0); // manufacturer ID
-	buf[1] = SPIPORT.transfer(0); // memory type
-	buf[2] = SPIPORT.transfer(0); // capacity
-	if (buf[0] == ID0_SPANSION) {
-		buf[3] = SPIPORT.transfer(0); // ID-CFI
-		buf[4] = SPIPORT.transfer(0); // sector size
-	}
-	CSRELEASE();
-	SPIPORT.endTransaction();
-	//Serial.printf("ID: %02X %02X %02X\n", buf[0], buf[1], buf[2]);
-}
-
-void SerialFlashChip::readSerialNumber(uint8_t *buf) //needs room for 8 bytes
-{
-	if (busy) wait();
-	SPIPORT.beginTransaction(SPICONFIG);
-	CSASSERT();
-	SPIPORT.transfer(0x4B);			
-	SPIPORT.transfer16(0);	
-	SPIPORT.transfer16(0);
-	for (int i=0; i<8; i++) {		
-		buf[i] = SPIPORT.transfer(0);
-	}
-	CSRELEASE();
-	SPIPORT.endTransaction();
-//	Serial.printf("Serial Number: %02X %02X %02X %02X %02X %02X %02X %02X\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
-}
-
-uint32_t SerialFlashChip::capacity(const uint8_t *id)
-{
-	uint32_t n = 1048576; // unknown chips, default to 1 MByte
-
-	if (id[2] >= 16 && id[2] <= 31) {
-		n = 1ul << id[2];
-	} else
-	if (id[2] >= 32 && id[2] <= 37) {
-		n = 1ul << (id[2] - 6);
-	} else
-	if ((id[0]==0 && id[1]==0 && id[2]==0) || 
-		(id[0]==255 && id[1]==255 && id[2]==255)) {
-		n = 0;
-	}
-	//Serial.printf("capacity %lu\n", n);
-	return n;
-}
-
-uint32_t SerialFlashChip::blockSize()
-{
-	// Spansion chips >= 512 mbit use 256K sectors
-	if (flags & FLAG_256K_BLOCKS) return 262144;
-	// everything else seems to have 64K sectors
-	return 65536;
-}
-
-
-
-
-/*
-Chip		Uniform Sector Erase
-		20/21	52	D8/DC
-		-----	--	-----
-W25Q64CV	4	32	64
-W25Q128FV	4	32	64
-S25FL127S			64
-N25Q512A	4		64
-N25Q00AA	4		64
-S25FL512S			256
-SST26VF032	4
-*/
-
-
-
-//			size	sector			busy	pgm/erase	chip
-// Part			Mbyte	kbyte	ID bytes	cmd	suspend		erase
-// ----			----	-----	--------	---	-------		-----
-// Winbond W25Q64CV	8	64	EF 40 17
-// Winbond W25Q128FV	16	64	EF 40 18	05	single		60 & C7
-// Winbond W25Q256FV	32	64	EF 40 19	
-// Spansion S25FL064A	8	?	01 02 16
-// Spansion S25FL127S	16	64	01 20 18	05
-// Spansion S25FL128P	16	64	01 20 18
-// Spansion S25FL256S	32	64	01 02 19	05			60 & C7
-// Spansion S25FL512S	64	256	01 02 20
-// Macronix MX25L12805D 16	?	C2 20 18
-// Macronix MX66L51235F	64		C2 20 1A
-// Numonyx M25P128	16	?	20 20 18
-// Micron M25P80	1	?	20 20 14
-// Micron N25Q128A	16	64	20 BA 18
-// Micron N25Q512A	64	?	20 BA 20	70	single		C4 x2
-// Micron N25Q00AA	128	64	20 BA 21		single		C4 x4
-// Micron MT25QL02GC	256	64	20 BA 22	70			C4 x2
-// SST SST25WF010	1/8	?	BF 25 02
-// SST SST25WF020	1/4	?	BF 25 03
-// SST SST25WF040	1/2	?	BF 25 04
-// SST SST25VF016B	1	?	BF 25 41
-// SST26VF016			?	BF 26 01
-// SST26VF032			?	BF 26 02
-// SST25VF032		4	64	BF 25 4A
-// SST26VF064		8	?	BF 26 43
-// LE25U40CMC		1/2	64	62 06 13
-
-SerialFlashChip SerialFlash;
diff --git a/libraries/SerialFlash/SerialFlashDirectory.cpp b/libraries/SerialFlash/SerialFlashDirectory.cpp
deleted file mode 100644
index 116c14ec..00000000
--- a/libraries/SerialFlash/SerialFlashDirectory.cpp
+++ /dev/null
@@ -1,395 +0,0 @@
-/* SerialFlash Library - for filesystem-like access to SPI Serial Flash memory
- * https://github.com/PaulStoffregen/SerialFlash
- * Copyright (C) 2015, Paul Stoffregen, paul@pjrc.com
- *
- * Development of this library was funded by PJRC.COM, LLC by sales of Teensy.
- * Please support PJRC's efforts to develop open source software by purchasing
- * Teensy or other genuine PJRC products.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice, development funding notice, and this permission
- * notice shall be included in all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "SerialFlash.h"
-
-/* On-chip SerialFlash file allocation data structures:
-
-  uint32_t signature = 0xFA96554C;
-  uint16_t maxfiles          
-  uint16_t stringssize  // div by 4
-  uint16_t hashes[maxfiles]
-  struct {
-    uint32_t file_begin
-    uint32_t file_length
-    uint16_t string_index  // div4
-  } fileinfo[maxfiles]
-  char strings[stringssize]
-
-A 32 bit signature is stored at the beginning of the flash memory.
-If 0xFFFFFFFF is seen, the entire chip should be assumed blank.
-If any value other than 0xFA96554C is found, a different data format
-is stored.  This could should refuse to access the flash.
-
-The next 4 bytes store number of files and size of the strings
-section, which allow the position of every other item to be found.
-The string section size is the 16 bit integer times 4, which allows
-up to 262140 bytes for string data.
-
-An array of 16 bit filename hashes allows for quick linear search
-for potentially matching filenames.  A hash value of 0xFFFF indicates
-no file is allocated for the remainder of the array.
-
-Following the hashes, and array of 10 byte structs give the location
-and length of the file's actual data, and the offset of its filename
-in the strings section.
-
-Strings are null terminated.  The remainder of the chip is file data.
-*/
-
-#define DEFAULT_MAXFILES      600
-#define DEFAULT_STRINGS_SIZE  25560
-
-
-static uint32_t check_signature(void)
-{
-	uint32_t sig[2];
-
-	SerialFlash.read(0, sig, 8);
-	 //Serial.printf("sig: %08X %08X\n", sig[0], sig[1]);
-	if (sig[0] == 0xFA96554C) return sig[1];
-	if (sig[0] == 0xFFFFFFFF) {
-		sig[0] = 0xFA96554C;
-		sig[1] = ((uint32_t)(DEFAULT_STRINGS_SIZE/4) << 16) | DEFAULT_MAXFILES;
-		SerialFlash.write(0, sig, 8);
-		while (!SerialFlash.ready()) ; // TODO: timeout
-		SerialFlash.read(0, sig, 8);
-		if (sig[0] == 0xFA96554C) return sig[1];
-	}
-	return 0;
-}
-
-static uint16_t filename_hash(const char *filename)
-{
-	// http://isthe.com/chongo/tech/comp/fnv/
-	uint32_t hash = 2166136261;
-	const char *p;
-
-	for (p=filename; *p; p++) {
-		hash ^= *p;
-		hash *= 16777619;
-	}
-	hash = (hash % (uint32_t)0xFFFE) + 1; // all values except 0000 & FFFF
-	return hash;
-}
-
-static bool filename_compare(const char *filename, uint32_t straddr)
-{
-	unsigned int i;
-	const char *p;
-	char buf[16];
-
-	p = filename;
-	while (1) {
-		SerialFlash.read(straddr, buf, sizeof(buf));
-		straddr += sizeof(buf);
-		for (i=0; i < sizeof(buf); i++) {
-			if (*p++ != buf[i]) return false;
-			if (buf[i] == 0) return true;
-		}
-	}
-}
-
-#if 0
-void pbuf(const void *buf, uint32_t len)
-{
-  const uint8_t *p = (const uint8_t *)buf;
-  do {
-    Serial.printf("%02X ", *p++);
-  } while (--len > 0);
-  Serial.println();
-}
-#endif
-
-SerialFlashFile SerialFlashChip::open(const char *filename)
-{
-	uint32_t maxfiles, straddr;
-	uint16_t hash, hashtable[8];
-	uint32_t i, n, index=0;
-	uint32_t buf[3];
-	SerialFlashFile file;
-
-	maxfiles = check_signature();
-	 //Serial.printf("sig: %08X\n", maxfiles);
-	if (!maxfiles) return file;
-	maxfiles &= 0xFFFF;
-	hash = filename_hash(filename);
-	 //Serial.printf("hash %04X for \"%s\"\n", hash, filename);
-	while (index < maxfiles) {
-		n = 8;
-		if (n > maxfiles - index) n = maxfiles - index;
-		SerialFlash.read(8 + index * 2, hashtable, n * 2);
-		 //Serial.printf(" read %u: ", 8 + index * 2);
-		 //pbuf(hashtable, n * 2);
-		for (i=0; i < n; i++) {
-			if (hashtable[i] == hash) {
-				 //Serial.printf("  hash match at index %u\n", index+i);
-				buf[2] = 0;
-				SerialFlash.read(8 + maxfiles * 2 + (index+i) * 10, buf, 10);
-
-				 //Serial.printf("  maxf=%d, index=%d, i=%d\n", maxfiles, index, i);
-				 //Serial.printf("  read %u: ", 8 + maxfiles * 2 + (index+i) * 10);
-				 //pbuf(buf, 10);
-				straddr = 8 + maxfiles * 12 + buf[2] * 4;
-				 //Serial.printf("  straddr = %u\n", straddr);
-				if (filename_compare(filename, straddr)) {
-					 //Serial.printf("  match!\n");
-					 //Serial.printf("  addr = %u\n", buf[0]);
-					 //Serial.printf("  len =  %u\n", buf[1]);
-					file.address = buf[0];
-					file.length = buf[1];
-					file.offset = 0;
-					file.dirindex = index + i;
-					return file;
-				}
-			} else if (hashtable[i] == 0xFFFF) {
-				return file;
-			}
-		}
-		index += n;
-	}
-	return file;
-}
-
-bool SerialFlashChip::exists(const char *filename)
-{
-	SerialFlashFile file = open(filename);
-	return (bool)file;
-}
-
-bool SerialFlashChip::remove(const char *filename)
-{
-	SerialFlashFile file = open(filename);
-	return remove(file);
-}
-
-bool SerialFlashChip::remove(SerialFlashFile &file)
-{
-	// To "remove" a file, we simply zero its hash in the lookup
-	// table, so it can't be found by open().  The space on the
-	// flash memory is not freed.
-	if (!file) return false;
-	uint16_t hash;
-	SerialFlash.read(8 + file.dirindex * 2, &hash, 2);
-	 //Serial.printf("remove hash %04X at %d index\n", hash, file.dirindex);
-	hash ^= 0xFFFF;  // write zeros to all ones
-	SerialFlash.write(8 + file.dirindex * 2, &hash, 2);
-	while (!SerialFlash.ready()) ; // wait...  TODO: timeout
-	SerialFlash.read(8 + file.dirindex * 2, &hash, 2);
-	if (hash != 0)  {
-		 //Serial.printf("remove failed, hash %04X\n", hash);
-		return false;
-	}
-	file.address = 0;
-	file.length = 0;
-	return true;
-}
-
-static uint32_t find_first_unallocated_file_index(uint32_t maxfiles)
-{
-	uint16_t hashtable[8];
-	uint32_t i, n, index=0;
-
-	do {
-		n = 8;
-		if (index + n > maxfiles) n = maxfiles - index;
-		SerialFlash.read(8 + index * 2, hashtable, n * 2);
-		for (i=0; i < n; i++) {
-			if (hashtable[i] == 0xFFFF) return index + i;
-		}
-		index += n;
-	} while (index < maxfiles);
-	return 0xFFFFFFFF;
-}
-
-static uint32_t string_length(uint32_t addr)
-{
-	char buf[16];
-	const char *p;
-	uint32_t len=0;
-
-	while (1) {
-		SerialFlash.read(addr, buf, sizeof(buf));
-		for (p=buf; p < buf + sizeof(buf); p++) {
-			len++;
-			if (*p == 0) return len;
-		}
-		addr += sizeof(buf);
-	}
-}
-
-//  uint32_t signature = 0xFA96554C;
-//  uint16_t maxfiles          
-//  uint16_t stringssize  // div by 4
-//  uint16_t hashes[maxfiles]
-//  struct {
-//    uint32_t file_begin
-//    uint32_t file_length
-//    uint16_t string_index  // div 4
-//  } fileinfo[maxfiles]
-//  char strings[stringssize]
-
-bool SerialFlashChip::create(const char *filename, uint32_t length, uint32_t align)
-{
-	uint32_t maxfiles, stringsize;
-	uint32_t index, buf[3];
-	uint32_t address, straddr, len;
-	SerialFlashFile file;
-
-	// check if the file already exists
-	if (exists(filename)) return false;
-
-	// first, get the filesystem parameters
-	maxfiles = check_signature();
-	if (!maxfiles) return false;
-	stringsize = (maxfiles & 0xFFFF0000) >> 14;
-	maxfiles &= 0xFFFF;
-
-	// find the first unused slot for this file
-	index = find_first_unallocated_file_index(maxfiles);
-	if (index >= maxfiles) return false;
-	 //Serial.printf("index = %u\n", index);
-	// compute where to store the filename and actual data
-	straddr = 8 + maxfiles * 12;
-	if (index == 0) {
-		address = straddr + stringsize;
-	} else {
-		buf[2] = 0;
-		SerialFlash.read(8 + maxfiles * 2 + (index-1) * 10, buf, 10);
-		address = buf[0] + buf[1];
-		straddr += buf[2] * 4;
-		straddr += string_length(straddr);
-		straddr = (straddr + 3) & 0x0003FFFC;
-	}
-	 //Serial.printf("straddr = %u\n", straddr);
-	 //Serial.printf("address = %u\n", address);
-	 //Serial.printf("length = %u\n", length);
-	if (align > 0) {
-		// for files aligned to sectors, adjust addr & len
-		address += align - 1;
-		address /= align;
-		address *= align;
-		 //Serial.printf("align address = %u\n", address);
-		length += align - 1;
-		length /= align;
-		length *= align;
-		 //Serial.printf("align length = %u\n", length);
-	} else {
-		// always align every file to a page boundary
-		// for predictable write latency and to guarantee
-		// write suspend for reading another file can't
-		// conflict on the same page (2 files never share
-		// a write page).
-		address = (address + 255) & 0xFFFFFF00;
-	}
-	 //Serial.printf("address = %u\n", address);
-	// last check, if enough space exists...
-	len = strlen(filename);
-	// TODO: check for enough string space for filename
-
-    // 5 bytes, to allow for extra 2 bytes in Spansion device IDs
-	uint8_t id[5];
-	SerialFlash.readID(id);
-	if (address + length > SerialFlash.capacity(id)) return false;
-
-	SerialFlash.write(straddr, filename, len+1);
-	buf[0] = address;
-	buf[1] = length;
-	buf[2] = (straddr - (8 + maxfiles * 12)) / 4;
-	SerialFlash.write(8 + maxfiles * 2 + index * 10, buf, 10);
-	 //Serial.printf("  write %u: ", 8 + maxfiles * 2 + index * 10);
-	 //pbuf(buf, 10);
-	while (!SerialFlash.ready()) ;  // TODO: timeout
-	 
-	buf[0] = filename_hash(filename);
-	 //Serial.printf("hash = %04X\n", buf[0]);
-	SerialFlash.write(8 + index * 2, buf, 2);
-	while (!SerialFlash.ready()) ;  // TODO: timeout
-	return true;
-}
-
-bool SerialFlashChip::readdir(char *filename, uint32_t strsize, uint32_t &filesize)
-{
-	uint32_t maxfiles, index, straddr;
-	uint32_t i, n;
-	uint32_t buf[2];
-	uint16_t hash;
-	char str[16], *p=filename;
-
-	filename[0] = 0;
-	maxfiles = check_signature();
-	if (!maxfiles) return false;
-	maxfiles &= 0xFFFF; 
-	index = dirindex;
-	while (1) {
-		if (index >= maxfiles) return false;
-		 //Serial.printf("readdir, index = %u\n", index);
-		SerialFlash.read(8 + index * 2, &hash, 2);
-		if (hash != 0) break;
-		index++;  // skip deleted entries
-	}
-	dirindex = index + 1;
-	buf[1] = 0;
-	SerialFlash.read(8 + 4 + maxfiles * 2 + index * 10, buf, 6);
-	if (buf[0] == 0xFFFFFFFF) return false;
-	filesize = buf[0];
-	straddr = 8 + maxfiles * 12 + buf[1] * 4;
-	 //Serial.printf("  length = %u\n", buf[0]);
-	 //Serial.printf("  straddr = %u\n", straddr);
-
-	while (strsize) {
-		n = strsize;
-		if (n > sizeof(str)) n = sizeof(str);
-		SerialFlash.read(straddr, str, n);
-		for (i=0; i < n; i++) {
-			*p++ = str[i];
-			if (str[i] == 0) {
-				 //Serial.printf("  name = %s\n", filename);
-				return true;
-			}
-		}
-		strsize -= n;
-		straddr += n;
-	}
-	*(p - 1) = 0;
-	 //Serial.printf("  name(overflow) = %s\n", filename);
-	return true;
-}
-
-
-void SerialFlashFile::erase()
-{
-	uint32_t i, blocksize;
-
-	blocksize = SerialFlash.blockSize();
-	if (address & (blocksize - 1)) return; // must begin on a block boundary
-	if (length & (blocksize - 1)) return;  // must be exact number of blocks
-	for (i=0; i < length; i += blocksize) {
-		SerialFlash.eraseBlock(address + i);
-	}
-}
-
diff --git a/libraries/SerialFlash/keywords.txt b/libraries/SerialFlash/keywords.txt
deleted file mode 100644
index 1e473961..00000000
--- a/libraries/SerialFlash/keywords.txt
+++ /dev/null
@@ -1,9 +0,0 @@
-SerialFlash	KEYWORD1
-SerialFlashFile	KEYWORD1
-createWritable	KEYWORD2
-erase	KEYWORD2
-eraseAll	KEYWORD2
-ready	KEYWORD2
-create	KEYWORD2
-createWritable	KEYWORD2
-getAddress	KEYWORD2
diff --git a/libraries/SerialFlash/util/SerialFlash_directwrite.h b/libraries/SerialFlash/util/SerialFlash_directwrite.h
deleted file mode 100644
index d323a055..00000000
--- a/libraries/SerialFlash/util/SerialFlash_directwrite.h
+++ /dev/null
@@ -1,204 +0,0 @@
-#ifndef SerialFlash_directwrite_h
-#define SerialFlash_directwrite_h
-
-#include <inttypes.h>
-
-// Adapted from OneWire.h
-
-#if ARDUINO >= 100
-#include "Arduino.h"       // for delayMicroseconds, digitalPinToBitMask, etc
-#else
-#include "WProgram.h"      // for delayMicroseconds
-#include "pins_arduino.h"  // for digitalPinToBitMask, etc
-#endif
-
-// Platform specific I/O definitions
-
-#if defined(__AVR__)
-#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
-#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
-#define IO_REG_TYPE uint8_t
-#define IO_REG_ASM asm("r30")
-#define DIRECT_READ(base, mask)         (((*(base)) & (mask)) ? 1 : 0)
-#define DIRECT_MODE_INPUT(base, mask)   ((*((base)+1)) &= ~(mask))
-#define DIRECT_MODE_OUTPUT(base, mask)  ((*((base)+1)) |= (mask))
-#define DIRECT_WRITE_LOW(base, mask)    ((*((base)+2)) &= ~(mask))
-#define DIRECT_WRITE_HIGH(base, mask)   ((*((base)+2)) |= (mask))
-
-#elif defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK66FX1M0__) || defined(__MK64FX512__)
-#define PIN_TO_BASEREG(pin)             (portOutputRegister(pin))
-#define PIN_TO_BITMASK(pin)             (1)
-#define IO_REG_TYPE uint8_t
-#define IO_REG_ASM
-#define DIRECT_READ(base, mask)         (*((base)+512))
-#define DIRECT_MODE_INPUT(base, mask)   (*((base)+640) = 0)
-#define DIRECT_MODE_OUTPUT(base, mask)  (*((base)+640) = 1)
-#define DIRECT_WRITE_LOW(base, mask)    (*((base)+256) = 1)
-#define DIRECT_WRITE_HIGH(base, mask)   (*((base)+128) = 1)
-
-#elif defined(__MKL26Z64__)
-#define PIN_TO_BASEREG(pin)             (portOutputRegister(pin))
-#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
-#define IO_REG_TYPE uint8_t
-#define IO_REG_ASM
-#define DIRECT_READ(base, mask)         ((*((base)+16) & (mask)) ? 1 : 0)
-#define DIRECT_MODE_INPUT(base, mask)   (*((base)+20) &= ~(mask))
-#define DIRECT_MODE_OUTPUT(base, mask)  (*((base)+20) |= (mask))
-#define DIRECT_WRITE_LOW(base, mask)    (*((base)+8) = (mask))
-#define DIRECT_WRITE_HIGH(base, mask)   (*((base)+4) = (mask))
-
-#elif defined(__SAM3X8E__)
-#define PIN_TO_BASEREG(pin)             (&(digitalPinToPort(pin)->PIO_PER))
-#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
-#define IO_REG_TYPE uint32_t
-#define IO_REG_ASM
-#define DIRECT_READ(base, mask)         (((*((base)+15)) & (mask)) ? 1 : 0)
-#define DIRECT_MODE_INPUT(base, mask)   ((*((base)+5)) = (mask))
-#define DIRECT_MODE_OUTPUT(base, mask)  ((*((base)+4)) = (mask))
-#define DIRECT_WRITE_LOW(base, mask)    ((*((base)+13)) = (mask))
-#define DIRECT_WRITE_HIGH(base, mask)   ((*((base)+12)) = (mask))
-#ifndef PROGMEM
-#define PROGMEM
-#endif
-#ifndef pgm_read_byte
-#define pgm_read_byte(addr) (*(const uint8_t *)(addr))
-#endif
-
-#elif defined(__PIC32MX__)
-#define PIN_TO_BASEREG(pin)             (portModeRegister(digitalPinToPort(pin)))
-#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
-#define IO_REG_TYPE uint32_t
-#define IO_REG_ASM
-#define DIRECT_READ(base, mask)         (((*(base+4)) & (mask)) ? 1 : 0)  //PORTX + 0x10
-#define DIRECT_MODE_INPUT(base, mask)   ((*(base+2)) = (mask))            //TRISXSET + 0x08
-#define DIRECT_MODE_OUTPUT(base, mask)  ((*(base+1)) = (mask))            //TRISXCLR + 0x04
-#define DIRECT_WRITE_LOW(base, mask)    ((*(base+8+1)) = (mask))          //LATXCLR  + 0x24
-#define DIRECT_WRITE_HIGH(base, mask)   ((*(base+8+2)) = (mask))          //LATXSET + 0x28
-
-#elif defined(ARDUINO_ARCH_ESP8266)
-#define PIN_TO_BASEREG(pin)             ((volatile uint32_t*) GPO)
-#define PIN_TO_BITMASK(pin)             (1 << pin)
-#define IO_REG_TYPE uint32_t
-#define IO_REG_ASM
-#define DIRECT_READ(base, mask)         ((GPI & (mask)) ? 1 : 0)    //GPIO_IN_ADDRESS
-#define DIRECT_MODE_INPUT(base, mask)   (GPE &= ~(mask))            //GPIO_ENABLE_W1TC_ADDRESS
-#define DIRECT_MODE_OUTPUT(base, mask)  (GPE |= (mask))             //GPIO_ENABLE_W1TS_ADDRESS
-#define DIRECT_WRITE_LOW(base, mask)    (GPOC = (mask))             //GPIO_OUT_W1TC_ADDRESS
-#define DIRECT_WRITE_HIGH(base, mask)   (GPOS = (mask))             //GPIO_OUT_W1TS_ADDRESS
-
-#elif defined(__SAMD21G18A__)
-#define PIN_TO_BASEREG(pin)             portModeRegister(digitalPinToPort(pin))
-#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
-#define IO_REG_TYPE uint32_t
-#define IO_REG_ASM
-#define DIRECT_READ(base, mask)         (((*((base)+8)) & (mask)) ? 1 : 0)
-#define DIRECT_MODE_INPUT(base, mask)   ((*((base)+1)) = (mask))
-#define DIRECT_MODE_OUTPUT(base, mask)  ((*((base)+2)) = (mask))
-#define DIRECT_WRITE_LOW(base, mask)    ((*((base)+5)) = (mask))
-#define DIRECT_WRITE_HIGH(base, mask)   ((*((base)+6)) = (mask))
-
-#elif defined(RBL_NRF51822)
-#define PIN_TO_BASEREG(pin)             (0)
-#define PIN_TO_BITMASK(pin)             (pin)
-#define IO_REG_TYPE uint32_t
-#define IO_REG_ASM
-#define DIRECT_READ(base, pin)          nrf_gpio_pin_read(pin)
-#define DIRECT_WRITE_LOW(base, pin)     nrf_gpio_pin_clear(pin)
-#define DIRECT_WRITE_HIGH(base, pin)    nrf_gpio_pin_set(pin)
-#define DIRECT_MODE_INPUT(base, pin)    nrf_gpio_cfg_input(pin, NRF_GPIO_PIN_NOPULL)
-#define DIRECT_MODE_OUTPUT(base, pin)   nrf_gpio_cfg_output(pin)
-
-#elif defined(__arc__) /* Arduino101/Genuino101 specifics */
-
-#include "scss_registers.h"
-#include "portable.h"
-#include "avr/pgmspace.h"
-
-#define GPIO_ID(pin)			(g_APinDescription[pin].ulGPIOId)
-#define GPIO_TYPE(pin)			(g_APinDescription[pin].ulGPIOType)
-#define GPIO_BASE(pin)			(g_APinDescription[pin].ulGPIOBase)
-#define DIR_OFFSET_SS			0x01
-#define DIR_OFFSET_SOC			0x04
-#define EXT_PORT_OFFSET_SS		0x0A
-#define EXT_PORT_OFFSET_SOC		0x50
-
-/* GPIO registers base address */
-#define PIN_TO_BASEREG(pin)		((volatile uint32_t *)g_APinDescription[pin].ulGPIOBase)
-#define PIN_TO_BITMASK(pin)		pin
-#define IO_REG_TYPE			uint32_t
-#define IO_REG_ASM
-
-static inline __attribute__((always_inline))
-IO_REG_TYPE directRead(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
-{
-    IO_REG_TYPE ret;
-    if (SS_GPIO == GPIO_TYPE(pin)) {
-        ret = READ_ARC_REG(((IO_REG_TYPE)base + EXT_PORT_OFFSET_SS));
-    } else {
-        ret = MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, EXT_PORT_OFFSET_SOC);
-    }
-    return ((ret >> GPIO_ID(pin)) & 0x01);
-}
-
-static inline __attribute__((always_inline))
-void directModeInput(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
-{
-    if (SS_GPIO == GPIO_TYPE(pin)) {
-        WRITE_ARC_REG(READ_ARC_REG((((IO_REG_TYPE)base) + DIR_OFFSET_SS)) & ~(0x01 << GPIO_ID(pin)),
-			((IO_REG_TYPE)(base) + DIR_OFFSET_SS));
-    } else {
-        MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, DIR_OFFSET_SOC) &= ~(0x01 << GPIO_ID(pin));
-    }
-}
-
-static inline __attribute__((always_inline))
-void directModeOutput(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
-{
-    if (SS_GPIO == GPIO_TYPE(pin)) {
-        WRITE_ARC_REG(READ_ARC_REG(((IO_REG_TYPE)(base) + DIR_OFFSET_SS)) | (0x01 << GPIO_ID(pin)),
-			((IO_REG_TYPE)(base) + DIR_OFFSET_SS));
-    } else {
-        MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, DIR_OFFSET_SOC) |= (0x01 << GPIO_ID(pin));
-    }
-}
-
-static inline __attribute__((always_inline))
-void directWriteLow(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
-{
-    if (SS_GPIO == GPIO_TYPE(pin)) {
-        WRITE_ARC_REG(READ_ARC_REG(base) & ~(0x01 << GPIO_ID(pin)), base);
-    } else {
-        MMIO_REG_VAL(base) &= ~(0x01 << GPIO_ID(pin));
-    }
-}
-
-static inline __attribute__((always_inline))
-void directWriteHigh(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
-{
-    if (SS_GPIO == GPIO_TYPE(pin)) {
-        WRITE_ARC_REG(READ_ARC_REG(base) | (0x01 << GPIO_ID(pin)), base);
-    } else {
-        MMIO_REG_VAL(base) |= (0x01 << GPIO_ID(pin));
-    }
-}
-
-#define DIRECT_READ(base, pin)		directRead(base, pin)
-#define DIRECT_MODE_INPUT(base, pin)	directModeInput(base, pin)
-#define DIRECT_MODE_OUTPUT(base, pin)	directModeOutput(base, pin)
-#define DIRECT_WRITE_LOW(base, pin)	directWriteLow(base, pin)
-#define DIRECT_WRITE_HIGH(base, pin)	directWriteHigh(base, pin)
-
-#else
-#define PIN_TO_BASEREG(pin)             (0)
-#define PIN_TO_BITMASK(pin)             (pin)
-#define IO_REG_TYPE unsigned int
-#define IO_REG_ASM
-#define DIRECT_READ(base, pin)          digitalRead(pin)
-#define DIRECT_WRITE_LOW(base, pin)     digitalWrite(pin, LOW)
-#define DIRECT_WRITE_HIGH(base, pin)    digitalWrite(pin, HIGH)
-#define DIRECT_MODE_INPUT(base, pin)    pinMode(pin,INPUT)
-#define DIRECT_MODE_OUTPUT(base, pin)   pinMode(pin,OUTPUT)
-
-#endif
-
-#endif
diff --git a/libraries/Wire/examples/bus_scan/bus_scan.ino b/libraries/Wire/examples/bus_scan/bus_scan.ino
index 65fc0946..22ef7495 100644
--- a/libraries/Wire/examples/bus_scan/bus_scan.ino
+++ b/libraries/Wire/examples/bus_scan/bus_scan.ino
@@ -47,7 +47,7 @@ void setup()
   while(!Serial);
 }
 
-boolean toggle = false;          // state of the LED
+bool toggle = false;          // state of the LED
 void loop()
 {
   toggle = !toggle;
diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index 2f8601f7..3315fe8e 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -33,7 +33,8 @@ TwoWire::TwoWire(I2C_CONTROLLER _controller_id)
     : rxBufferIndex(0), rxBufferLength(0), init_status(-1),
       controller_id(_controller_id)
 {
-    // Empty
+    rxBuffer = (uint8_t*)dccm_malloc(BUFFER_LENGTH);
+    txBuffer = (uint8_t*)dccm_malloc(BUFFER_LENGTH);
 }
 
 void TwoWire::begin(void)
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index a18e1f19..c7f270e4 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -58,13 +58,13 @@ class TwoWire : public Stream {
 
 private:
 	// RX Buffer
-	uint8_t rxBuffer[BUFFER_LENGTH];
+	uint8_t *rxBuffer;
 	uint8_t rxBufferIndex;
 	uint8_t rxBufferLength;
 
 	// TX Buffer
 	uint8_t txAddress;
-	uint8_t txBuffer[BUFFER_LENGTH];
+	uint8_t *txBuffer;
 	uint8_t txBufferLength;
 
 	int init_status;
diff --git a/platform.txt b/platform.txt
index e5493cb2..66812628 100644
--- a/platform.txt
+++ b/platform.txt
@@ -5,7 +5,7 @@
 # https://github.com/arduino/Arduino/wiki/Arduino-IDE-1.5---3rd-party-Hardware-specification
 
 name=Intel Curie (32-bit) Boards
-version=1.0.7
+version=2.1.0
 
 # Arduino 101 compile variables
 # ----------------------
@@ -14,12 +14,12 @@ compiler.prefix=arc-elf32
 
 compiler.path={runtime.tools.arc-elf32.path}/bin/
 compiler.c.cmd=arc-elf32-gcc
-compiler.c.flags=-c -std=gnu11 -mcpu=quarkse_em -mlittle-endian -g -Os -Wall -fno-reorder-functions -fno-asynchronous-unwind-tables -fno-omit-frame-pointer -fno-defer-pop -Wno-unused-but-set-variable -Wno-main -ffreestanding -fno-stack-protector -mno-sdata -ffunction-sections -fdata-sections -fsigned-char -MMD -D__ARDUINO_ARC__
+compiler.c.flags=-c -std=gnu11 -mcpu=quarkse_em -mlittle-endian -g -Os -Wall -fno-reorder-functions -fno-asynchronous-unwind-tables -fno-omit-frame-pointer -fno-defer-pop -Wno-unused-but-set-variable -Wno-main -ffreestanding -fno-stack-protector -mno-sdata -ffunction-sections -fdata-sections -fsigned-char -MMD -D__ARDUINO_ARC__ -DCONFIG_BLUETOOTH_PERIPHERAL -DCONFIG_BLUETOOTH_CENTRAL -DCONFIG_BLUETOOTH_GATT_CLIENT
 compiler.c.elf.cmd=arc-elf32-gcc
 compiler.c.elf.flags=-nostartfiles -nodefaultlibs -nostdlib -static -Wl,-X -Wl,-N -Wl,-mcpu=quarkse_em -Wl,-marcelf -Wl,--gc-sections
 compiler.S.flags=-c -g -x assembler-with-cpp
 compiler.cpp.cmd=arc-elf32-g++
-compiler.cpp.flags=-c -mcpu=quarkse_em -mlittle-endian -g -Os -Wall -fno-reorder-functions -fno-asynchronous-unwind-tables -fno-omit-frame-pointer -fno-defer-pop -Wno-unused-but-set-variable -Wno-main -ffreestanding -fno-stack-protector -mno-sdata -ffunction-sections -fdata-sections -fsigned-char -MMD -fno-rtti -fno-exceptions -D__ARDUINO_ARC__ -std=c++11
+compiler.cpp.flags=-c -mcpu=quarkse_em -mlittle-endian -g -Os -Wall -fno-reorder-functions -fno-asynchronous-unwind-tables -fno-omit-frame-pointer -fno-defer-pop -Wno-unused-but-set-variable -Wno-main -ffreestanding -fno-stack-protector -mno-sdata -ffunction-sections -fdata-sections -fsigned-char -MMD -fno-rtti -fno-exceptions -fcheck-new -D__ARDUINO_ARC__ -std=c++11 -DCONFIG_BLUETOOTH_PERIPHERAL -DCONFIG_BLUETOOTH_CENTRAL -DCONFIG_BLUETOOTH_GATT_CLIENT
 compiler.ar.cmd=arc-elf32-ar
 compiler.ar.flags=rcs
 compiler.objcopy.cmd=arc-elf32-objcopy
@@ -70,9 +70,8 @@ recipe.ar.pattern="{compiler.path}{compiler.ar.cmd}" {compiler.ar.flags} {compil
 ## Combine gc-sections, archives, and objects
 recipe.c.combine.pattern="{compiler.path}{compiler.c.elf.cmd}" {compiler.c.elf.flags} "-T{build.variant.path}/{build.ldscript}" "-Wl,-Map,{build.path}/{build.project_name}.map" {compiler.c.elf.extra_flags} -o "{build.path}/{build.project_name}.elf" "-L{build.path}" "-L{build.variant.path}" -Wl,--whole-archive "-l{build.variant_system_lib}" -Wl,--no-whole-archive -Wl,--start-group "-l{build.variant_system_lib}" -lnsim -lc -lm -lgcc {object_files} "{build.path}/{archive_file}"
 
-## Save output with debug symbols (.debug.elf file)
-recipe.hooks.objcopy.preobjcopy.1.pattern=cp -f "{build.path}/{build.project_name}.elf" "{build.path}/../arduino101_sketch.debug.elf"
-recipe.hooks.objcopy.preobjcopy.1.pattern.windows=cmd /C copy /y "{build.path}\{build.project_name}.elf" "{build.path}\..\arduino101_sketch.debug.elf"
+## Save output with debug symbols (.debug.elf file). Uncomment if you wish to use OpenOCD to debug.
+recipe.hooks.objcopy.preobjcopy.1.pattern="{runtime.tools.arduino101load.path}/arduino101load" -c -from="{build.path}/{build.project_name}.elf" -to="{build.path}/../arduino101_sketch.debug.elf"
 
 ## Create output (.bin file)
 recipe.objcopy.bin.pattern="{compiler.path}{compiler.elf2bin.cmd}" {compiler.elf2bin.flags} {compiler.elf2hex.extra_flags} "{build.path}/{build.project_name}.elf" "{build.path}/{build.project_name}.bin"
@@ -91,25 +90,29 @@ recipe.output.save_file={build.project_name}.{build.variant}.hex
 recipe.size.pattern="{compiler.path}{compiler.size.cmd}" -A "{build.path}/{build.project_name}.elf"
 recipe.size.regex=^(?:text|ctors|rodata|datas)\s+([0-9]+).*
 
-# BLE firmware check
+# Firmware check
 # -------------------
-tools.arduino101load.ble.fw.string="ATP1BLE000-1541C5635"
-tools.arduino101load.ble.fw.position=141312
+tools.arduino101load.ble.fw.string="ATP1BLE00R-1631C4439"
+tools.arduino101load.ble.fw.position=169984
+# Dummy RTOS walues - don't update RTOS if not strictly needed
+tools.arduino101load.rtos.fw.string="firmware_1.8.0_arduino101"
+tools.arduino101load.rtos.fw.position=147424
+#not ideal - should be inherited by the platform
+tools.arduino101load.version=2.2.0
 
 # Arc Uploader/Programmers tools
 # -------------------
-tools.arduino101load.cmd.path={runtime.tools.arduino101load.path}/arduino101load/arduino101load
+tools.arduino101load.cmd.path={runtime.tools.arduino101load.path}/arduino101load
 
-tools.arduino101load.upload.params.verbose=verbose
-tools.arduino101load.upload.params.quiet=quiet
+tools.arduino101load.upload.params.verbose=-v
+tools.arduino101load.upload.params.quiet=-q
 
-tools.arduino101load.upload.pattern="{cmd.path}" "{runtime.tools.arduino101load.path}/x86/bin" {build.path}/{build.project_name}.bin {serial.port} "{upload.verbose}" {ble.fw.string} {ble.fw.position}
+tools.arduino101load.upload.pattern="{cmd.path}" "-dfu={runtime.tools.dfu-util.path}" "-bin={build.path}/{build.project_name}.bin" -port={serial.port} "{upload.verbose}" -ble_fw_str={ble.fw.string} -ble_fw_pos={ble.fw.position} -rtos_fw_str={rtos.fw.string} -rtos_fw_pos={rtos.fw.position} -core={version}
 
 # This is needed to avoid an error on unexistent fields
 tools.arduino101load.erase.params.verbose=
 tools.arduino101load.erase.params.quiet=
 tools.arduino101load.erase.pattern=
-tools.arduino101load.bootloader.params.verbose=
-tools.arduino101load.bootloader.params.quiet=
-tools.arduino101load.bootloader.pattern={runtime.tools.flashpack.path}/flash_dfu.sh
-tools.arduino101load.bootloader.pattern.windows={runtime.tools.flashpack.path}/flash_dfu.bat
+tools.arduino101load.bootloader.params.verbose=-v
+tools.arduino101load.bootloader.params.quiet=-q
+tools.arduino101load.bootloader.pattern="{cmd.path}" "-dfu={runtime.tools.dfu-util.path}"  -port={serial.port} "{bootloader.verbose}" -f -core={version}
diff --git a/post_install.bat b/post_install.bat
new file mode 100644
index 00000000..73e16e14
--- /dev/null
+++ b/post_install.bat
@@ -0,0 +1,10 @@
+@echo off
+set ARGS=/A /SE /SW /SA
+if "%PROCESSOR_ARCHITECTURE%" == "AMD64" (
+  drivers\dpinst-amd64.exe %ARGS%
+) ELSE IF "%PROCESSOR_ARCHITEW6432%" == "AMD64" (
+  drivers\dpinst-amd64.exe %ARGS%
+) ELSE (
+  drivers\dpinst-x86.exe %ARGS%
+)
+exit /b 0
diff --git a/scripts/create_dfu_udev_rule b/scripts/create_dfu_udev_rule
new file mode 100755
index 00000000..4e64ea64
--- /dev/null
+++ b/scripts/create_dfu_udev_rule
@@ -0,0 +1,17 @@
+#!/bin/bash
+#
+
+if [ "$(id -u)" != "0" ]; then
+   echo "This script must be run as root"
+   exit
+fi
+
+NAME=99-arduino-101.rules
+
+echo >/etc/udev/rules.d/$NAME
+echo \# Arduino 101 in DFU Mode >>/etc/udev/rules.d/$NAME
+echo SUBSYSTEM==\"tty\", ENV{ID_REVISION}==\"8087\", ENV{ID_MODEL_ID}==\"0ab6\", MODE=\"0666\", ENV{ID_MM_DEVICE_IGNORE}=\"1\", ENV{ID_MM_CANDIDATE}=\"0\" >>/etc/udev/rules.d/$NAME
+echo SUBSYSTEM==\"usb\", ATTR{idVendor}==\"8087\", ATTR{idProduct}==\"0aba\", MODE=\"0666\", ENV{ID_MM_DEVICE_IGNORE}=\"1\" >>/etc/udev/rules.d/$NAME
+
+udevadm control --reload-rules
+udevadm trigger
diff --git a/system/libarc32_arduino101/Makefile b/system/libarc32_arduino101/Makefile
index 9602e3ad..bccd2ab3 100644
--- a/system/libarc32_arduino101/Makefile
+++ b/system/libarc32_arduino101/Makefile
@@ -3,10 +3,12 @@ ASM_SRC+=$(wildcard $(PWD)/drivers/*.S)
 ASM_SRC+=$(wildcard $(PWD)/common/*.S)
 C_SRC+=$(wildcard $(PWD)/bootcode/*.c)
 C_SRC+=$(wildcard $(PWD)/drivers/*.c)
+C_SRC+=$(wildcard $(PWD)/drivers/rpc/*.c)
 C_SRC+=$(wildcard $(PWD)/common/*.c)
 C_SRC+=$(wildcard $(PWD)/framework/src/*.c)
 C_SRC+=$(wildcard $(PWD)/framework/src/services/*.c)
 C_SRC+=$(wildcard $(PWD)/framework/src/services/ble/*.c)
+C_SRC+=$(wildcard $(PWD)/framework/src/services/ble_service/*.c)
 C_SRC+=$(wildcard $(PWD)/framework/src/cfw/*.c)
 C_SRC+=$(wildcard $(PWD)/framework/src/infra/*.c)
 C_SRC+=$(wildcard $(PWD)/framework/src/util/*.c)
@@ -15,6 +17,7 @@ C_SRC+=$(wildcard $(PWD)/framework/src/os/*.c)
 ARCH=ARC
 CC=arc-elf32-gcc
 AS=arc-elf32-as
+STRIP=arc-elf32-strip
 RM=rm -f
 INSTALL=install
 
@@ -25,8 +28,21 @@ HWFLAGS=-mARCv2EM -mav2em -mlittle-endian
 
 CFGFLAGS=-DCONFIG_SOC_GPIO_32 -DCONFIG_SOC_GPIO_AON -DINFRA_MULTI_CPU_SUPPORT -DCFW_MULTI_CPU_SUPPORT -DCONFIG_HAS_SHARED_MEM -DCONFIG_INFRA_IS_MASTER
 OPTFLAGS=-g -Os -Wall -Werror
+CFGFLAGS+=-DCONFIG_SOC_QUARK_SE
+#CFGFLAGS+=-DTRACK_ALLOCS
+#CFGFLAGS+=-DIPC_UART_DBG_RX
+#CFGFLAGS+=-DIPC_UART_DBG_TX
+CFGFLAGS+=-DBT_GATT_DEBUG
+CFGFLAGS+=-DCONFIG_RPC_IN
+CFGFLAGS+=-DCONFIG_IPC_UART_NS16550
 CFGFLAGS+=-DCONFIG_IPC_UART_BAUDRATE=1000000
-INCLUDES=-I. -Icommon -Idrivers -Ibootcode -Iframework/include -Iframework/src/services/ble
+CFGFLAGS+=-DCONFIG_BLUETOOTH_MAX_CONN=2
+CFGFLAGS+=-DCONFIG_BT_GATT_BLE_MAX_SERVICES=10
+CFGFLAGS+=-DCONFIG_BLUETOOTH_GATT_CLIENT
+CFGFLAGS+=-DCONFIG_BLUETOOTH_CENTRAL -DCONFIG_BLUETOOTH_PERIPHERAL
+INCLUDES=-I. -Icommon -Idrivers -Ibootcode -Iframework/include -Iframework/include/services/ble -Iframework/src/services/ble_service -I../../cores/arduino/dccm
+#-Iframework/src/services/ble -Iframework/include/services/ble
+INCLUDES+= -Idrivers/rpc -Iframework/src
 EXTRA_CFLAGS=-D__CPU_ARC__ -DCLOCK_SPEED=32 -std=c99 -fno-reorder-functions -fno-asynchronous-unwind-tables -fno-omit-frame-pointer -fno-defer-pop -Wno-unused-but-set-variable -Wno-main -ffreestanding -fno-stack-protector -mno-sdata -ffunction-sections -fdata-sections
 CFLAGS=$(HWFLAGS) $(OPTFLAGS) $(EXTRA_CFLAGS) $(CFGFLAGS) $(INCLUDES)
 
@@ -39,7 +55,10 @@ lib: $(TARGET_LIB)
 
 $(TARGET_LIB): $(C_OBJ) $(ASM_OBJ)
 	@echo "Link $@"
-	$(AR) rcs $@ $^
+	@$(AR) rcs $@ $^
+
+strip: $(TARGET_LIB)
+	@$(STRIP) --strip-unneeded $^
 
 %.o: %.S
 	@echo "Assembling $<"
@@ -47,7 +66,7 @@ $(TARGET_LIB): $(C_OBJ) $(ASM_OBJ)
 
 %.o: %.c
 	@echo "Compiling $<"
-	$(CC) -c $(CFLAGS) $< -o $@
+	@$(CC) -c $(CFLAGS) $< -o $@
 
 lib_install: lib
 	@if test "$(LIB_INSTALL_PATH)" = "" ; then \
diff --git a/system/libarc32_arduino101/bootcode/c_init.c b/system/libarc32_arduino101/bootcode/c_init.c
index 243622d6..a6256c45 100644
--- a/system/libarc32_arduino101/bootcode/c_init.c
+++ b/system/libarc32_arduino101/bootcode/c_init.c
@@ -21,6 +21,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 
 #include "interrupt.h"
 #include "arcv2_timer0.h"
+#include "os/os.h"
 
 /* Application main() function prototype */
 extern int main (void);
@@ -60,6 +61,8 @@ static void _exec_ctors (void)
     interrupt_unit_device_init();
     /* Start the system's virtual 64-bit Real Time Counter */
     timer0_driver_init();
+    /* Initialize the memory buffer for balloc() calls. */
+    os_abstraction_init_malloc();
     /* Jump to application main() */
     main ();
     /* Never reached */
diff --git a/system/libarc32_arduino101/bootcode/init.S b/system/libarc32_arduino101/bootcode/init.S
index 28aab7d6..a8bee403 100644
--- a/system/libarc32_arduino101/bootcode/init.S
+++ b/system/libarc32_arduino101/bootcode/init.S
@@ -61,9 +61,10 @@ _do_reset:
 .balign 4
 _do_fault:
 _exit_halt:
-    /* Set halt flag */
-    flag 0x01
+    /* Set halt flag
+    flag 0x0 */
     nop
+    j @_Fault
     nop
     nop
     /* loop forever */
@@ -76,8 +77,15 @@ _exit_halt:
  */
 .balign 4
 _do_isr:
-     /* Init the SP for the FIRQ bank */
-     mov sp, @__firq_stack_start
+    /* Init the SP for the FIRQ bank */
+    mov sp, @__firq_stack_start
+    /* Save the loop count related register */
+    mov r0, lp_count
+    lr r1, [ARC_V2_LP_START]
+    lr r2, [ARC_V2_LP_END]
+    push_s r0
+    push_s r1
+    push_s r2
     /* Read IRQ Cause */
     lr r0, [ARC_V2_ICAUSE]
     sub r0, r0, 16
@@ -89,5 +97,13 @@ _do_isr:
     jl_s.d [r1]
     nop
 
+    /* Restore the loop count related register */
+    pop_s r2
+    pop_s r1
+    pop_s r0
+	
+    sr r2, [ARC_V2_LP_END]
+    sr r1, [ARC_V2_LP_START]
+    mov lp_count, r0
     rtie
 
diff --git a/system/libarc32_arduino101/common/atomic.h b/system/libarc32_arduino101/common/atomic.h
new file mode 100644
index 00000000..d30cfbe4
--- /dev/null
+++ b/system/libarc32_arduino101/common/atomic.h
@@ -0,0 +1,156 @@
+/* atomic operations */
+
+/*
+ * Copyright (c) 1997-2015, Wind River Systems, Inc.
+ *
+ * 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.
+ */
+
+#ifndef __ATOMIC_H__
+#define __ATOMIC_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef int atomic_t;
+typedef atomic_t atomic_val_t;
+
+extern atomic_val_t atomic_add(atomic_t *target, atomic_val_t value);
+extern atomic_val_t atomic_and(atomic_t *target, atomic_val_t value);
+extern atomic_val_t atomic_dec(atomic_t *target);
+extern atomic_val_t atomic_inc(atomic_t *target);
+extern atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value);
+extern atomic_val_t atomic_or(atomic_t *target, atomic_val_t value);
+extern atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value);
+extern atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value);
+extern atomic_val_t atomic_clear(atomic_t *target);
+extern atomic_val_t atomic_get(const atomic_t *target);
+extern atomic_val_t atomic_set(atomic_t *target, atomic_val_t value);
+extern int atomic_cas(atomic_t *target,
+					  atomic_val_t oldValue, atomic_val_t newValue);
+
+
+#define ATOMIC_INIT(i) {(i)}
+
+#define ATOMIC_BITS (sizeof(atomic_val_t) * 8)
+#define ATOMIC_MASK(bit) (1 << ((bit) & (ATOMIC_BITS - 1)))
+#define ATOMIC_ELEM(addr, bit) ((addr) + ((bit) / ATOMIC_BITS))
+
+/** @brief Test whether a bit is set
+ *
+ *  Test whether bit number bit is set or not.
+ *
+ *  Also works for an array of multiple atomic_t variables, in which
+ *  case the bit number may go beyond the number of bits in a single
+ *  atomic_t variable.
+ *
+ *  @param addr base address to start counting from
+ *  @param bit bit number counted from the base address
+ *
+ *  @return 1 if the bit was set, 0 if it wasn't
+ */
+static inline int atomic_test_bit(const atomic_t *addr, int bit)
+{
+	atomic_val_t val = atomic_get(ATOMIC_ELEM(addr, bit));
+
+	return (1 & (val >> (bit & (ATOMIC_BITS - 1))));
+}
+
+/** @brief Clear a bit and return its old value
+ *
+ *  Atomically clear a bit and return its old value.
+ *
+ *  Also works for an array of multiple atomic_t variables, in which
+ *  case the bit number may go beyond the number of bits in a single
+ *  atomic_t variable.
+ *
+ *  @param addr base address to start counting from
+ *  @param bit bit number counted from the base address
+ *
+ *  @return 1 if the bit was set, 0 if it wasn't
+ */
+static inline int atomic_test_and_clear_bit(atomic_t *addr, int bit)
+{
+	atomic_val_t mask = ATOMIC_MASK(bit);
+	atomic_val_t old;
+
+	old = atomic_and(ATOMIC_ELEM(addr, bit), ~mask);
+
+	return (old & mask) != 0;
+}
+
+/** @brief Set a bit and return its old value
+ *
+ *  Atomically set a bit and return its old value.
+ *
+ *  Also works for an array of multiple atomic_t variables, in which
+ *  case the bit number may go beyond the number of bits in a single
+ *  atomic_t variable.
+ *
+ *  @param addr base address to start counting from
+ *  @param bit bit number counted from the base address
+ *
+ *  @return 1 if the bit was set, 0 if it wasn't
+ */
+static inline int atomic_test_and_set_bit(atomic_t *addr, int bit)
+{
+	atomic_val_t mask = ATOMIC_MASK(bit);
+	atomic_val_t old;
+
+	old = atomic_or(ATOMIC_ELEM(addr, bit), mask);
+
+	return (old & mask) != 0;
+}
+
+/** @brief Clear a bit
+ *
+ *  Atomically clear a bit.
+ *
+ *  Also works for an array of multiple atomic_t variables, in which
+ *  case the bit number may go beyond the number of bits in a single
+ *  atomic_t variable.
+ *
+ *  @param addr base address to start counting from
+ *  @param bit bit number counted from the base address
+ */
+static inline void atomic_clear_bit(atomic_t *addr, int bit)
+{
+	atomic_val_t mask = ATOMIC_MASK(bit);
+
+	atomic_and(ATOMIC_ELEM(addr, bit), ~mask);
+}
+
+/** @brief Set a bit
+ *
+ *  Atomically set a bit.
+ *
+ *  Also works for an array of multiple atomic_t variables, in which
+ *  case the bit number may go beyond the number of bits in a single
+ *  atomic_t variable.
+ *
+ *  @param addr base address to start counting from
+ *  @param bit bit number counted from the base address
+ */
+static inline void atomic_set_bit(atomic_t *addr, int bit)
+{
+	atomic_val_t mask = ATOMIC_MASK(bit);
+
+	atomic_or(ATOMIC_ELEM(addr, bit), mask);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __ATOMIC_H__ */
diff --git a/system/libarc32_arduino101/common/misc/byteorder.h b/system/libarc32_arduino101/common/misc/byteorder.h
new file mode 100644
index 00000000..67cecbc3
--- /dev/null
+++ b/system/libarc32_arduino101/common/misc/byteorder.h
@@ -0,0 +1,44 @@
+/* byteorder.h - Byte order helpers */
+
+/*
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * 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.
+ */
+
+#define bswap_16(x) ((uint16_t) ((((x) >> 8) & 0xff) | (((x) & 0xff) << 8)))
+
+#define bswap_32(x) ((uint32_t) ((((x) >> 24) & 0xff) | (((x) >> 8) & 0xff00) \
+				| (((x) & 0xff00) << 8) | (((x) & 0xff) << 24)))
+
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+#define sys_le16_to_cpu(val) (val)
+#define sys_cpu_to_le16(val) (val)
+#define sys_be16_to_cpu(val) bswap_16(val)
+#define sys_cpu_to_be16(val) bswap_16(val)
+#define sys_le32_to_cpu(val) (val)
+#define sys_cpu_to_le32(val) (val)
+#define sys_be32_to_cpu(val) bswap_32(val)
+#define sys_cpu_to_be32(val) bswap_32(val)
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+#define sys_le16_to_cpu(val) bswap_16(val)
+#define sys_cpu_to_le16(val) bswap_16(val)
+#define sys_be16_to_cpu(val) (val)
+#define sys_cpu_to_be16(val) (val)
+#define sys_le32_to_cpu(val) bswap_32(val)
+#define sys_cpu_to_le32(val) bswap_32(val)
+#define sys_be32_to_cpu(val) (val)
+#define sys_cpu_to_be32(val) (val)
+#else
+#error "Unknown byte order"
+#endif
diff --git a/system/libarc32_arduino101/common/misc/util.h b/system/libarc32_arduino101/common/misc/util.h
index 626c2b69..ab45bbb1 100644
--- a/system/libarc32_arduino101/common/misc/util.h
+++ b/system/libarc32_arduino101/common/misc/util.h
@@ -44,6 +44,11 @@ extern "C" {
 
 #ifndef _ASMLANGUAGE
 
+#define ARRAY_SIZE(array) ((unsigned long)(sizeof(array) / sizeof((array)[0])))
+#define CONTAINER_OF(ptr, type, field) \
+	((type *)(((char *)(ptr)) - offsetof(type, field)))
+
+
 /* round "x" up/down to next multiple of "align" (which must be a power of 2) */
 #define ROUND_UP(x, align)                                   \
 	(((unsigned long)(x) + ((unsigned long)align - 1)) & \
diff --git a/system/libarc32_arduino101/drivers/atomic_native.c b/system/libarc32_arduino101/drivers/atomic_native.c
new file mode 100644
index 00000000..41bed7d4
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/atomic_native.c
@@ -0,0 +1,370 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ * Copyright (c) 2011-2014 Wind River Systems, Inc.
+ *
+ * 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.
+ */
+
+/**
+ * @file Atomic ops for ARC EM
+ *
+ * This module provides the atomic operators for ARC EM family processors
+ * which do not support native atomic operations.
+ *
+ * The atomic operations are guaranteed to be atomic with respect
+ * to interrupt service routines, and to operations performed by peer
+ * processors.
+ *
+ * (originally from x86's atomic.c)
+ */
+
+#include <atomic.h>
+//#include <toolchain.h>
+//#include <arch/arc/v2/irq.h>
+#include <interrupt.h>
+#define irq_lock() interrupt_lock()
+#define irq_unlock(key) interrupt_unlock(key)
+
+#if defined(__clang__)
+#define FUNC_NO_FP
+#else
+#define FUNC_NO_FP	 __attribute__((optimize("-fomit-frame-pointer")))
+#endif
+/**
+ *
+ * @brief Atomic compare-and-set primitive
+ *
+ * This routine provides the compare-and-set operator. If the original value at
+ * <target> equals <oldValue>, then <newValue> is stored at <target> and the
+ * function returns 1.
+ *
+ * If the original value at <target> does not equal <oldValue>, then the store
+ * is not done and the function returns 0.
+ *
+ * The reading of the original value at <target>, the comparison,
+ * and the write of the new value (if it occurs) all happen atomically with
+ * respect to both interrupts and accesses of other processors to <target>.
+ *
+ * @param target address to be tested
+ * @param old_value value to compare against
+ * @param new_value value to compare against
+ * @return Returns 1 if <new_value> is written, 0 otherwise.
+ */
+FUNC_NO_FP int atomic_cas(atomic_t *target, atomic_val_t old_value,
+			  atomic_val_t new_value)
+{
+	unsigned int key;
+	int ret = 0;
+
+	key = irq_lock();
+
+	if (*target == old_value) {
+		*target = new_value;
+		ret = 1;
+	}
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic addition primitive
+ *
+ * This routine provides the atomic addition operator. The <value> is
+ * atomically added to the value at <target>, placing the result at <target>,
+ * and the old value from <target> is returned.
+ *
+ * @param target memory location to add to
+ * @param value the value to add
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_add(atomic_t *target, atomic_val_t value)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target += value;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic subtraction primitive
+ *
+ * This routine provides the atomic subtraction operator. The <value> is
+ * atomically subtracted from the value at <target>, placing the result at
+ * <target>, and the old value from <target> is returned.
+ *
+ * @param target the memory location to subtract from
+ * @param value the value to subtract
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target -= value;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic increment primitive
+ *
+ * @param target memory location to increment
+ *
+ * This routine provides the atomic increment operator. The value at <target>
+ * is atomically incremented by 1, and the old value from <target> is returned.
+ *
+ * @return The value from <target> before the increment
+ */
+FUNC_NO_FP atomic_val_t atomic_inc(atomic_t *target)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	(*target)++;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic decrement primitive
+ *
+ * @param target memory location to decrement
+ *
+ * This routine provides the atomic decrement operator. The value at <target>
+ * is atomically decremented by 1, and the old value from <target> is returned.
+ *
+ * @return The value from <target> prior to the decrement
+ */
+FUNC_NO_FP atomic_val_t atomic_dec(atomic_t *target)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	(*target)--;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic get primitive
+ *
+ * @param target memory location to read from
+ *
+ * This routine provides the atomic get primitive to atomically read
+ * a value from <target>. It simply does an ordinary load.  Note that <target>
+ * is expected to be aligned to a 4-byte boundary.
+ *
+ * @return The value read from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_get(const atomic_t *target)
+{
+	return *target;
+}
+
+/**
+ *
+ * @brief Atomic get-and-set primitive
+ *
+ * This routine provides the atomic set operator. The <value> is atomically
+ * written at <target> and the previous value at <target> is returned.
+ *
+ * @param target the memory location to write to
+ * @param value the value to write
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_set(atomic_t *target, atomic_val_t value)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target = value;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic clear primitive
+ *
+ * This routine provides the atomic clear operator. The value of 0 is atomically
+ * written at <target> and the previous value at <target> is returned. (Hence,
+ * atomic_clear(pAtomicVar) is equivalent to atomic_set(pAtomicVar, 0).)
+ *
+ * @param target the memory location to write
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_clear(atomic_t *target)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target = 0;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic bitwise inclusive OR primitive
+ *
+ * This routine provides the atomic bitwise inclusive OR operator. The <value>
+ * is atomically bitwise OR'ed with the value at <target>, placing the result
+ * at <target>, and the previous value at <target> is returned.
+ *
+ * @param target the memory location to be modified
+ * @param value the value to OR
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_or(atomic_t *target, atomic_val_t value)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target |= value;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic bitwise exclusive OR (XOR) primitive
+ *
+ * This routine provides the atomic bitwise exclusive OR operator. The <value>
+ * is atomically bitwise XOR'ed with the value at <target>, placing the result
+ * at <target>, and the previous value at <target> is returned.
+ *
+ * @param target the memory location to be modified
+ * @param value the value to XOR
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target ^= value;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic bitwise AND primitive
+ *
+ * This routine provides the atomic bitwise AND operator. The <value> is
+ * atomically bitwise AND'ed with the value at <target>, placing the result
+ * at <target>, and the previous value at <target> is returned.
+ *
+ * @param target the memory location to be modified
+ * @param value the value to AND
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_and(atomic_t *target, atomic_val_t value)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target &= value;
+
+	irq_unlock(key);
+
+	return ret;
+}
+
+/**
+ *
+ * @brief Atomic bitwise NAND primitive
+ *
+ * This routine provides the atomic bitwise NAND operator. The <value> is
+ * atomically bitwise NAND'ed with the value at <target>, placing the result
+ * at <target>, and the previous value at <target> is returned.
+ *
+ * @param target the memory location to be modified
+ * @param value the value to NAND
+ *
+ * @return The previous value from <target>
+ */
+FUNC_NO_FP atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value)
+{
+	unsigned int key;
+	atomic_val_t ret;
+
+	key = irq_lock();
+
+	ret = *target;
+	*target = ~(*target & value);
+
+	irq_unlock(key);
+
+	return ret;
+}
diff --git a/system/libarc32_arduino101/drivers/bluetooth/att.h b/system/libarc32_arduino101/drivers/bluetooth/att.h
new file mode 100644
index 00000000..5752a079
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/bluetooth/att.h
@@ -0,0 +1,55 @@
+/** @file
+ *  @brief Attribute Protocol handling.
+ */
+
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+#ifndef __BT_ATT_H
+#define __BT_ATT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Error codes for Error response PDU */
+#define BT_ATT_ERR_INVALID_HANDLE		0x01
+#define BT_ATT_ERR_READ_NOT_PERMITTED		0x02
+#define BT_ATT_ERR_WRITE_NOT_PERMITTED		0x03
+#define BT_ATT_ERR_INVALID_PDU			0x04
+#define BT_ATT_ERR_AUTHENTICATION		0x05
+#define BT_ATT_ERR_NOT_SUPPORTED		0x06
+#define BT_ATT_ERR_INVALID_OFFSET		0x07
+#define BT_ATT_ERR_AUTHORIZATION		0x08
+#define BT_ATT_ERR_PREPARE_QUEUE_FULL		0x09
+#define BT_ATT_ERR_ATTRIBUTE_NOT_FOUND		0x0a
+#define BT_ATT_ERR_ATTRIBUTE_NOT_LONG		0x0b
+#define BT_ATT_ERR_ENCRYPTION_KEY_SIZE		0x0c
+#define BT_ATT_ERR_INVALID_ATTRIBUTE_LEN	0x0d
+#define BT_ATT_ERR_UNLIKELY			0x0e
+#define BT_ATT_ERR_INSUFFICIENT_ENCRYPTION	0x0f
+#define BT_ATT_ERR_UNSUPPORTED_GROUP_TYPE	0x10
+#define BT_ATT_ERR_INSUFFICIENT_RESOURCES	0x11
+
+/* Common Profile Error Codes (from CSS) */
+#define BT_ATT_ERR_CCC_IMPROPER_CONF		0xfd
+#define BT_ATT_ERR_PROCEDURE_IN_PROGRESS	0xfe
+#define BT_ATT_ERR_OUT_OF_RANGE			0xff
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BT_ATT_H */
diff --git a/system/libarc32_arduino101/drivers/bluetooth/bluetooth.h b/system/libarc32_arduino101/drivers/bluetooth/bluetooth.h
new file mode 100644
index 00000000..faf03b2f
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/bluetooth/bluetooth.h
@@ -0,0 +1,354 @@
+/** @file
+ *  @brief Bluetooth subsystem core APIs.
+ */
+
+/*
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * 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.
+ */
+#ifndef __BT_BLUETOOTH_H
+#define __BT_BLUETOOTH_H
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <bluetooth/hci.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @brief Callback for notifying that Bluetooth has been enabled.
+ *
+ *  @param err zero on success or (negative) error code otherwise.
+ */
+typedef void (*bt_ready_cb_t)(int err);
+
+/** @brief Enable Bluetooth
+ *
+ *  Enable Bluetooth. Must be the called before any calls that
+ *  require communication with the local Bluetooth hardware.
+ *
+ *  @param cb Callback to notify completion or NULL to perform the
+ *  enabling synchronously.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_enable(bt_ready_cb_t cb);
+
+/* Advertising API */
+
+/** Description of different data types that can be encoded into
+  * advertising data. Used to form arrays that are passed to the
+  * bt_le_adv_start() function.
+  */
+struct bt_data {
+	uint8_t type;
+	uint8_t data_len;
+	const uint8_t *data;
+};
+
+/** @brief Helper to declare elements of bt_data arrays
+ *
+ *  This macro is mainly for creating an array of struct bt_data
+ *  elements which is then passed to bt_le_adv_start().
+ *
+ *  @param _type Type of advertising data field
+ *  @param _data Pointer to the data field payload
+ *  @param _data_len Number of bytes behind the _data pointer
+ */
+#define BT_DATA(_type, _data, _data_len) \
+	{ \
+		.type = (_type), \
+		.data_len = (_data_len), \
+		.data = (_data), \
+	}
+
+/** @brief Helper to declare elements of bt_data arrays
+ *
+ *  This macro is mainly for creating an array of struct bt_data
+ *  elements which is then passed to bt_le_adv_start().
+ *
+ *  @param _type Type of advertising data field
+ *  @param _bytes Variable number of single-byte parameters
+ */
+#define BT_DATA_BYTES(_type, _bytes...) \
+	BT_DATA(_type, ((uint8_t []) { _bytes }), \
+		sizeof((uint8_t []) { _bytes }))
+
+/** Local advertising address type */
+enum {
+	/** Use local identity address for advertising. Unless a static
+	  * random address has been configured this will be the public
+	  * address.
+	  */
+	BT_LE_ADV_ADDR_IDENTITY,
+
+	/** Use local Non-resolvable Private Address (NRPA) for advertising */
+	BT_LE_ADV_ADDR_NRPA,
+};
+
+/** LE Advertising Parameters. */
+struct bt_le_adv_param {
+	/** Advertising type */
+	uint8_t  type;
+
+	/** Which type of own address to use for advertising */
+	uint8_t  addr_type;
+
+	/** Minimum Advertising Interval (N * 0.625) */
+	uint16_t interval_min;
+
+	/** Maximum Advertising Interval (N * 0.625) */
+	uint16_t interval_max;
+};
+
+/** Helper to declare advertising parameters inline
+  *
+  * @param _type      Advertising Type
+  * @param _addr_type Local address type to use for advertising
+  * @param _int_min   Minimum advertising interval
+  * @param _int_max   Maximum advertising interval
+  */
+#define BT_LE_ADV_PARAM(_type, _addr_type, _int_min, _int_max) \
+		(&(struct bt_le_adv_param) { \
+			.type = (_type), \
+			.addr_type = (_addr_type), \
+			.interval_min = (_int_min), \
+			.interval_max = (_int_max), \
+		 })
+
+#define BT_LE_ADV(t) BT_LE_ADV_PARAM(t, BT_LE_ADV_ADDR_IDENTITY, \
+				     BT_GAP_ADV_FAST_INT_MIN_2, \
+				     BT_GAP_ADV_FAST_INT_MAX_2)
+
+/** @brief Start advertising
+ *
+ *  Set advertisement data, scan response data, advertisement parameters
+ *  and start advertising.
+ *
+ *  @param param Advertising parameters.
+ *  @param ad Data to be used in advertisement packets.
+ *  @param ad_len Number of elements in ad
+ *  @param sd Data to be used in scan response packets.
+ *  @param sd_len Number of elements in sd
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_le_adv_start(const struct bt_le_adv_param *param,
+		    const struct bt_data *ad, size_t ad_len,
+		    const struct bt_data *sd, size_t sd_len);
+
+/** @brief Stop advertising
+ *
+ *  Stops ongoing advertising.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_le_adv_stop(void);
+
+/** @brief Define a type allowing user to implement a function that can
+ *  be used to get back active LE scan results.
+ *
+ *  A function of this type will be called back when user application
+ *  triggers active LE scan. The caller will populate all needed
+ *  parameters based on data coming from scan result.
+ *  Such function can be set by user when LE active scan API is used.
+ *
+ *  @param addr Advertiser LE address and type.
+ *  @param rssi Strength of advertiser signal.
+ *  @param adv_type Type of advertising response from advertiser.
+ *  @param adv_data Address of buffer containig advertiser data.
+ *  @param len Length of advertiser data contained in buffer.
+ */
+typedef void bt_le_scan_cb_t(const bt_addr_le_t *addr, int8_t rssi,
+			     uint8_t adv_type, const uint8_t *adv_data,
+			     uint8_t len);
+
+/** LE scan parameters */
+struct bt_le_scan_param {
+	/** Scan type (BT_HCI_LE_SCAN_ACTIVE or BT_HCI_LE_SCAN_PASSIVE) */
+	uint8_t  type;
+
+	/** Duplicate filtering (BT_HCI_LE_SCAN_FILTER_DUP_ENABLE or
+	 *  BT_HCI_LE_SCAN_FILTER_DUP_DISABLE)
+	 */
+	uint8_t  filter_dup;
+
+	/** Scan interval (N * 0.625 ms) */
+	uint16_t interval;
+
+	/** Scan window (N * 0.625 ms) */
+	uint16_t window;
+};
+
+/** Helper to declare scan parameters inline
+  *
+  * @param _type     Scan Type (BT_HCI_LE_SCAN_ACTIVE/BT_HCI_LE_SCAN_PASSIVE)
+  * @param _filter   Filter Duplicates
+  * @param _interval Scan Interval (N * 0.625 ms)
+  * @param _window   Scan Window (N * 0.625 ms)
+  */
+#define BT_LE_SCAN_PARAM(_type, _filter, _interval, _window) \
+		(&(struct bt_le_scan_param) { \
+			.type = (_type), \
+			.filter_dup = (_filter), \
+			.interval = (_interval), \
+			.window = (_window), \
+		 })
+
+/** Helper macro to enable active scanning to discover new devices. */
+#define BT_LE_SCAN_ACTIVE BT_LE_SCAN_PARAM(BT_HCI_LE_SCAN_ACTIVE, \
+					   BT_HCI_LE_SCAN_FILTER_DUP_ENABLE, \
+					   BT_GAP_SCAN_FAST_INTERVAL, \
+					   BT_GAP_SCAN_FAST_WINDOW)
+
+/** Helper macro to enable passive scanning to discover new devices.
+ *
+ * This macro should be used if information required for device identification
+ * (eg UUID) are known to be placed in Advertising Data.
+ */
+#define BT_LE_SCAN_PASSIVE BT_LE_SCAN_PARAM(BT_HCI_LE_SCAN_PASSIVE, \
+					    BT_HCI_LE_SCAN_FILTER_DUP_ENABLE, \
+					    BT_GAP_SCAN_FAST_INTERVAL, \
+					    BT_GAP_SCAN_FAST_WINDOW)
+
+/** @brief Start (LE) scanning
+ *
+ *  Start LE scanning with and provide results through the specified
+ *  callback.
+ *
+ *  @param param Scan parameters.
+ *  @param cb Callback to notify scan results.
+ *
+ *  @return Zero on success or error code otherwise, positive in case
+ *  of protocol error or negative (POSIX) in case of stack internal error
+ */
+int bt_le_scan_start(const struct bt_le_scan_param *param, bt_le_scan_cb_t cb);
+
+/** @brief Stop (LE) scanning.
+ *
+ *  Stops ongoing LE scanning.
+ *
+ *  @return Zero on success or error code otherwise, positive in case
+ *  of protocol error or negative (POSIX) in case of stack internal error
+ */
+int bt_le_scan_stop(void);
+
+/** @def BT_ADDR_STR_LEN
+ *
+ *  @brief Recommended length of user string buffer for Bluetooth address
+ *
+ *  @details The recommended length guarantee the output of address
+ *  conversion will not lose valuable information about address being
+ *  processed.
+ */
+#define BT_ADDR_STR_LEN 18
+
+/** @def BT_ADDR_LE_STR_LEN
+ *
+ *  @brief Recommended length of user string buffer for Bluetooth LE address
+ *
+ *  @details The recommended length guarantee the output of address
+ *  conversion will not lose valuable information about address being
+ *  processed.
+ */
+#define BT_ADDR_LE_STR_LEN 27
+
+/** @brief Converts binary Bluetooth address to string.
+ *
+ *  @param addr Address of buffer containing binary Bluetooth address.
+ *  @param str Address of user buffer with enough room to store formatted
+ *  string containing binary address.
+ *  @param len Length of data to be copied to user string buffer. Refer to
+ *  BT_ADDR_STR_LEN about recommended value.
+ *
+ *  @return Number of successfully formatted bytes from binary address.
+ */
+static inline int bt_addr_to_str(const bt_addr_t *addr, char *str, size_t len)
+{
+	return snprintf(str, len, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
+			addr->val[5], addr->val[4], addr->val[3],
+			addr->val[2], addr->val[1], addr->val[0]);
+}
+
+/** @brief Converts binary LE Bluetooth address to string.
+ *
+ *  @param addr Address of buffer containing binary LE Bluetooth address.
+ *  @param str Address of user buffer with enough room to store
+ *  formatted string containing binary LE address.
+ *  @param len Length of data to be copied to user string buffer. Refer to
+ *  BT_ADDR_LE_STR_LEN about recommended value.
+ *
+ *  @return Number of successfully formatted bytes from binary address.
+ */
+static inline int bt_addr_le_to_str(const bt_addr_le_t *addr, char *str,
+				    size_t len)
+{
+	char type[7];
+
+	switch (addr->type) {
+	case BT_ADDR_LE_PUBLIC:
+		strcpy(type, "public");
+		break;
+	case BT_ADDR_LE_RANDOM:
+		strcpy(type, "random");
+		break;
+	default:
+		sprintf(type, "0x%02x", addr->type);
+		break;
+	}
+
+	return snprintf(str, len, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X (%s)",
+			addr->val[5], addr->val[4], addr->val[3],
+			addr->val[2], addr->val[1], addr->val[0], type);
+}
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+/** @brief Enable/disable set controller in discoverable state.
+ *
+ *  Allows make local controller to listen on INQUIRY SCAN channel and responds
+ *  to devices making general inquiry. To enable this state it's mandatory
+ *  to first be in connectable state.
+ *
+ *  @param enable Value allowing/disallowing controller to become discoverable.
+ *
+ *  @return Negative if fail set to requested state or requested state has been
+ *  already set. Zero if done successfully.
+ */
+int bt_br_set_discoverable(bool enable);
+
+/** @brief Enable/disable set controller in connectable state.
+ *
+ *  Allows make local controller to be connectable. It means the controller
+ *  start listen to devices requests on PAGE SCAN channel. If disabled also
+ *  resets discoverability if was set.
+ *
+ *  @param enable Value allowing/disallowing controller to be connectable.
+ *
+ *  @return Negative if fail set to requested state or requested state has been
+ *  already set. Zero if done successfully.
+ */
+int bt_br_set_connectable(bool enable);
+#endif
+
+void bt_le_set_device_name(char *device_name, int len);
+void bt_le_set_mac_address(bt_addr_le_t bda);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BT_BLUETOOTH_H */
diff --git a/system/libarc32_arduino101/drivers/bluetooth/conn.h b/system/libarc32_arduino101/drivers/bluetooth/conn.h
new file mode 100644
index 00000000..1b710d6f
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/bluetooth/conn.h
@@ -0,0 +1,399 @@
+/** @file
+ *  @brief Bluetooth connection handling
+ */
+
+/*
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * 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.
+ */
+#ifndef __BT_CONN_H
+#define __BT_CONN_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(CONFIG_BLUETOOTH_CENTRAL) || defined(CONFIG_BLUETOOTH_PERIPHERAL)
+#include <stdbool.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+
+/** Opaque type representing a connection to a remote device */
+struct bt_conn;
+
+/** Connection parameters for LE connections */
+struct bt_le_conn_param {
+	uint16_t interval_min;
+	uint16_t interval_max;
+	uint16_t latency;
+	uint16_t timeout;
+};
+
+/** Helper to declare connection parameters inline
+  *
+  * @param int_min  Minimum Connection Interval (N * 1.25 ms)
+  * @param int_max  Maximum Connection Interval (N * 1.25 ms)
+  * @param lat      Connection Latency
+  * @param timeout  Supervision Timeout (N * 10 ms)
+  */
+#define BT_LE_CONN_PARAM(int_min, int_max, lat, to) \
+	(&(struct bt_le_conn_param) { \
+		.interval_min = (int_min), \
+		.interval_max = (int_max), \
+		.latency = (lat), \
+		.timeout = (to), \
+	 })
+
+/** Default LE connection parameters:
+  *   Connection Interval: 30-50 ms
+  *   Latency: 0
+  *   Timeout: 4 s
+  */
+#define BT_LE_CONN_PARAM_DEFAULT BT_LE_CONN_PARAM(BT_GAP_INIT_CONN_INT_MIN, \
+						  BT_GAP_INIT_CONN_INT_MAX, \
+						  0, 400)
+
+/** @brief Increment a connection's reference count.
+ *
+ *  Increment the reference count of a connection object.
+ *
+ *  @param conn Connection object.
+ *
+ *  @return Connection object with incremented reference count.
+ */
+struct bt_conn *bt_conn_ref(struct bt_conn *conn);
+
+/** @brief Decrement a connection's reference count.
+ *
+ *  Decrement the reference count of a connection object.
+ *
+ *  @param conn Connection object.
+ */
+void bt_conn_unref(struct bt_conn *conn);
+
+/** @brief Look up an existing connection by address.
+ *
+ *  Look up an existing connection based on the remote address.
+ *
+ *  @param peer Remote address.
+ *
+ *  @return Connection object or NULL if not found. The caller gets a
+ *  new reference to the connection object which must be released with
+ *  bt_conn_unref() once done using the object.
+ */
+struct bt_conn *bt_conn_lookup_addr_le(const bt_addr_le_t *peer);
+
+/** @brief Get destination (peer) address of a connection.
+ *
+ *  @param conn Connection object.
+ *
+ *  @return Destination address.
+ */
+const bt_addr_le_t *bt_conn_get_dst(const struct bt_conn *conn);
+
+/** Connection Type */
+enum {
+	BT_CONN_TYPE_LE, /** LE Connection Type */
+#if defined(CONFIG_BLUETOOTH_BREDR)
+	BT_CONN_TYPE_BR, /** BR/EDR Connection Type */
+#endif
+};
+
+/** LE Connection Info Structure */
+struct bt_conn_le_info {
+	const bt_addr_le_t *src; /** Source Address */
+	const bt_addr_le_t *dst; /** Destination Address */
+	uint16_t interval; /** Connection interval */
+	uint16_t latency; /** Connection slave latency */
+	uint16_t timeout; /** Connection supervision timeout */
+};
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+/** BR/EDR Connection Info Structure */
+struct bt_conn_br_info {
+	const bt_addr_t *dst; /** Destination BR/EDR address */
+};
+#endif
+
+/** Connection role (master or slave) */
+enum {
+	BT_CONN_ROLE_MASTER,
+	BT_CONN_ROLE_SLAVE,
+};
+
+/** Connection Info Structure */
+struct bt_conn_info {
+	/** Connection Type */
+	uint8_t type;
+
+	/** Connection Role */
+	uint8_t role;
+
+	union {
+		/** LE Connection specific Info */
+		struct bt_conn_le_info le;
+#if defined(CONFIG_BLUETOOTH_BREDR)
+		struct bt_conn_br_info br;
+#endif
+	};
+};
+
+/** @brief Get connection info
+ *
+ *  @param conn Connection object.
+ *  @param info Connection info object.
+ *
+ *  @return Zero on success or (negative) error code on failure.
+ */
+int bt_conn_get_info(const struct bt_conn *conn, struct bt_conn_info *info);
+
+/** @brief Update the connection parameters.
+ *
+ *  @param conn Connection object.
+ *  @param param Updated connection parameters.
+ *
+ *  @return Zero on success or (negative) error code on failure.
+ */
+int bt_conn_le_param_update(struct bt_conn *conn,
+			    const struct bt_le_conn_param *param);
+
+/** @brief Disconnect from a remote device or cancel pending connection.
+ *
+ *  Disconnect an active connection with the specified reason code or cancel
+ *  pending outgoing connection.
+ *
+ *  @param conn Connection to disconnect.
+ *  @param reason Reason code for the disconnection.
+ *
+ *  @return Zero on success or (negative) error code on failure.
+ */
+int bt_conn_disconnect(struct bt_conn *conn, uint8_t reason);
+
+#if defined(CONFIG_BLUETOOTH_CENTRAL)
+/** @brief Initiate an LE connection to a remote device.
+ *
+ *  Allows initiate new LE link to remote peer using its address.
+ *  Returns a new reference that the the caller is responsible for managing.
+ *
+ *  @param peer  Remote address.
+ *  @param param Initial connection parameters.
+ *
+ *  @return Valid connection object on success or NULL otherwise.
+ */
+struct bt_conn *bt_conn_create_le(const bt_addr_le_t *peer,
+				  const struct bt_le_conn_param *param);
+
+/** @brief Automatically connect to remote device if it's in range.
+ *
+ *  This function enables/disables automatic connection initiation.
+ *  Everytime the device looses the connection with peer, this connection
+ *  will be re-established if connectable advertisement from peer is received.
+ *
+ *  @param addr Remote Bluetooth address.
+ *  @param param If non-NULL, auto connect is enabled with the given
+ *  parameters. If NULL, auto connect is disabled.
+ *
+ *  @return Zero on success or error code otherwise.
+ */
+int bt_le_set_auto_conn(bt_addr_le_t *addr,
+			const struct bt_le_conn_param *param);
+#endif /* CONFIG_BLUETOOTH_CENTRAL */
+
+#if defined(CONFIG_BLUETOOTH_PERIPHERAL)
+/** @brief Initiate directed advertising to a remote device
+ *
+ *  Allows initiating a new LE connection to remote peer with the remote
+ *  acting in central role and the local device in peripheral role.
+ *
+ *  The advertising type must be either BT_LE_ADV_DIRECT_IND or
+ *  BT_LE_ADV_DIRECT_IND_LOW_DUTY.
+ *
+ *  In case of high duty cycle this will result in a callback with
+ *  connected() with a new connection or with an error.
+ *
+ *  The advertising may be cancelled with bt_conn_disconnect().
+ *
+ *  Returns a new reference that the the caller is responsible for managing.
+ *
+ *  @param peer  Remote address.
+ *  @param param Directed advertising parameters.
+ *
+ *  @return Valid connection object on success or NULL otherwise.
+ */
+struct bt_conn *bt_conn_create_slave_le(const bt_addr_le_t *peer,
+					const struct bt_le_adv_param *param);
+#endif /* CONFIG_BLUETOOTH_PERIPHERAL */
+
+/** Security level. */
+typedef enum __packed {
+	BT_SECURITY_LOW,    /** No encryption and no authentication. */
+	BT_SECURITY_MEDIUM, /** encryption and no authentication (no MITM). */
+	BT_SECURITY_HIGH,   /** encryption and authentication (MITM). */
+	BT_SECURITY_FIPS,   /** Authenticated LE Secure Connections and
+			     *  encryption.
+			     */
+} bt_security_t;
+
+#if defined(CONFIG_BLUETOOTH_SMP)
+/** @brief Set security level for a connection.
+ *
+ *  This function enable security (encryption) for a connection. If device is
+ *  already paired with sufficiently strong key encryption will be enabled. If
+ *  link is already encrypted with sufficiently strong key this function does
+ *  nothing.
+ *
+ *  If device is not paired pairing will be initiated. If device is paired and
+ *  keys are too weak but input output capabilities allow for strong enough keys
+ *  pairing will be initiated.
+ *
+ *  This function may return error if required level of security is not possible
+ *  to achieve due to local or remote device limitation (eg input output
+ *  capabilities).
+ *
+ *  @param conn Connection object.
+ *  @param sec Requested security level.
+ *
+ *  @return 0 on success or negative error
+ */
+int bt_conn_security(struct bt_conn *conn, bt_security_t sec);
+
+/** @brief Get encryption key size.
+ *
+ *  This function gets encryption key size.
+ *  If there is no security (encryption) enabled 0 will be returned.
+ *
+ *  @param conn Existing connection object.
+ *
+ *  @return Encryption key size.
+ */
+uint8_t bt_conn_enc_key_size(struct bt_conn *conn);
+
+/** @brief Clear device information (bonding, keys).
+ *
+ *  Clears all a bonding information (keys, etc). A bonded connection is
+ *  disconnected.
+ *  BT_ADDR_LE_ANY removes the of all bonded devices
+ *
+ *  @param addr identity address of a bonded device
+ *
+ *  @return 0 in success, error code otherwise
+ *
+ */
+int bt_conn_remove_info(const bt_addr_le_t *addr);
+#endif /* CONFIG_BLUETOOTH_SMP */
+
+/** Connection callback structure */
+struct bt_conn_cb {
+	void (*connected)(struct bt_conn *conn, uint8_t err);
+	void (*disconnected)(struct bt_conn *conn, uint8_t reason);
+	void (*le_param_updated)(struct bt_conn *conn, uint16_t interval,
+				 uint16_t latency, uint16_t timeout);
+#if defined(CONFIG_BLUETOOTH_SMP)
+	void (*identity_resolved)(struct bt_conn *conn,
+				  const bt_addr_le_t *rpa,
+				  const bt_addr_le_t *identity);
+	void (*security_changed)(struct bt_conn *conn, bt_security_t level);
+#endif
+	struct bt_conn_cb *_next;
+};
+
+/** @brief Register connection callbacks.
+ *
+ *  Register callbacks to monitor the state of connections.
+ *
+ *  @param cb Callback struct.
+ */
+void bt_conn_cb_register(struct bt_conn_cb *cb);
+
+#endif /* CONFIG_BLUETOOTH_CENTRAL || CONFIG_BLUETOOTH_PERIPHERAL */
+
+#if defined(CONFIG_BLUETOOTH_SMP) || defined(CONFIG_BLUETOOTH_BREDR)
+/** Authenticated pairing callback structure */
+struct bt_conn_auth_cb {
+	void (*passkey_display)(struct bt_conn *conn, unsigned int passkey);
+	void (*passkey_entry)(struct bt_conn *conn);
+	void (*passkey_confirm)(struct bt_conn *conn, unsigned int passkey);
+	void (*cancel)(struct bt_conn *conn);
+#if defined(CONFIG_BLUETOOTH_BREDR)
+	void (*pincode_entry)(struct bt_conn *conn, bool highsec);
+#endif
+};
+
+/** @brief Register authentication callbacks.
+ *
+ *  Register callbacks to handle authenticated pairing. Passing NULL unregisters
+ *  previous callbacks structure.
+ *
+ *  @param cb Callback struct.
+ *
+ *  @return Zero on success or negative error code otherwise
+ */
+int bt_conn_auth_cb_register(const struct bt_conn_auth_cb *cb);
+
+/** @brief Reply with entered passkey.
+ *
+ *  This function should be called only after passkey_entry callback from
+ *  bt_conn_auth_cb structure was called.
+ *
+ *  @param conn Connection object.
+ *  @param passkey Entered passkey.
+ *
+ *  @return Zero on success or negative error code otherwise
+ */
+int bt_conn_auth_passkey_entry(struct bt_conn *conn, unsigned int passkey);
+
+/** @brief Cancel ongoing authenticated pairing.
+ *
+ *  This function allows to cancel ongoing authenticated pairing.
+ *
+ *  @param conn Connection object.
+ *
+ *  @return Zero on success or negative error code otherwise
+ */
+int bt_conn_auth_cancel(struct bt_conn *conn);
+
+/** @brief Reply if passkey was confirmed by user.
+ *
+ *  This function should be called only after passkey_confirm callback from
+ *  bt_conn_auth_cb structure was called. If passkey is confirmed to match
+ *  then match should be true. Otherwise match should be false.
+ *
+ *  @param conn Connection object.
+ *  @param match True if passkey was confirmed to match, false otherwise.
+ *
+ *  @return Zero on success or negative error code otherwise
+ */
+int bt_conn_auth_passkey_confirm(struct bt_conn *conn, bool match);
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+/** @brief Reply with entered PIN code.
+ *
+ *  This function should be called only after PIN code callback from
+ *  bt_conn_auth_cb structure was called. It's for legacy 2.0 devices.
+ *
+ *  @param conn Connection object.
+ *  @param pin Entered PIN code.
+ *
+ *  @return Zero on success or negative error code otherwise
+ */
+int bt_conn_auth_pincode_entry(struct bt_conn *conn, const char *pin);
+#endif /* CONFIG_BLUETOOTH_BREDR */
+#endif /* CONFIG_BLUETOOTH_SMP || CONFIG_BLUETOOTH_BREDR */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BT_CONN_H */
diff --git a/system/libarc32_arduino101/drivers/bluetooth/gatt.h b/system/libarc32_arduino101/drivers/bluetooth/gatt.h
new file mode 100644
index 00000000..e9eac613
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/bluetooth/gatt.h
@@ -0,0 +1,1045 @@
+/** @file
+ *  @brief Generic Attribute Profile handling.
+ */
+
+/*
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * 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.
+ */
+#ifndef __BT_GATT_H
+#define __BT_GATT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(CONFIG_BLUETOOTH_CENTRAL) || defined(CONFIG_BLUETOOTH_PERIPHERAL)
+#include <stddef.h>
+#include <sys/types.h>
+#include <misc/util.h>
+#include <bluetooth/conn.h>
+#include <bluetooth/uuid.h>
+#include <bluetooth/att.h>
+
+/* GATT attribute permission bit field values */
+
+/** @def BT_GATT_PERM_READ
+ *  @brief Attribute read permission.
+ */
+#define BT_GATT_PERM_READ			0x01
+/** @def BT_GATT_PERM_WRITE
+ *  @brief Attribute write permission.
+ */
+#define BT_GATT_PERM_WRITE			0x02
+/** @def BT_GATT_PERM_READ_ENCRYPT
+ *  @brief Attribute read permission with encryption.
+ *
+ *  If set, requires encryption for read access.
+ */
+#define BT_GATT_PERM_READ_ENCRYPT		0x04
+/** @def BT_GATT_PERM_WRITE_ENCRYPT
+ *  @brief Attribute write permission with encryption.
+ *
+ *  If set, requires encryption for write access.
+ */
+#define BT_GATT_PERM_WRITE_ENCRYPT		0x08
+/** @def BT_GATT_PERM_READ_AUTHEN
+ *  @brief Attribute read permission with authentication.
+ *
+ *  If set, requires encryption using authenticated link-key for read access.
+ */
+#define BT_GATT_PERM_READ_AUTHEN		0x10
+/** @def BT_GATT_PERM_WRITE_AUTHEN
+ *  @brief Attribute write permission with authentication.
+ *
+ *  If set, requires encryption using authenticated link-key for write access.
+ */
+#define BT_GATT_PERM_WRITE_AUTHEN		0x20
+/** @def BT_GATT_PERM_READ_AUTHOR
+ *  @brief Attribute read permission with authorization.
+ *
+ *  If set, requires authorization for read access.
+ */
+#define BT_GATT_PERM_READ_AUTHOR		0x40
+/** @def BT_GATT_PERM_WRITE_AUTHOR
+ *  @brief Attribute write permission with authorization.
+ *
+ *  If set, requires authorization for write access.
+ */
+#define BT_GATT_PERM_WRITE_AUTHOR		0x80
+
+/* GATT attribute flush flags */
+/** @def BT_GATT_FLUSH_DISCARD
+ *  @brief Attribute flush discard flag.
+ */
+#define BT_GATT_FLUSH_DISCARD			0x00
+/** @def BT_GATT_FLUSH_DISCARD
+ *  @brief Attribute flush synchronize flag.
+ */
+#define BT_GATT_FLUSH_SYNC			0x01
+
+/**  @def BT_GATT_ERR
+  *  @brief Construct error return value for attribute read, write and
+  *         flush callbacks.
+  *
+  *  @param _att_err ATT error code
+  *
+  *  @return Appropriate error code for the attribute callbacks.
+  *
+  */
+#define BT_GATT_ERR(_att_err)                  (-(_att_err))
+
+/** @brief GATT Attribute structure. */
+struct bt_gatt_attr {
+	/** Attribute UUID */
+	const struct bt_uuid	*uuid;
+
+	/** Attribute read callback
+	 *
+	 *  @param conn   The connection that is requesting to read
+	 *  @param attr   The attribute that's being read
+	 *  @param buf    Buffer to place the read result in
+	 *  @param len    Length of data to read
+	 *  @param offset Offset to start reading from
+	 *
+	 *  @return Number fo bytes read, or in case of an error
+	 *          BT_GATT_ERR() with a specific ATT error code.
+	 */
+	ssize_t			(*read)(struct bt_conn *conn,
+					const struct bt_gatt_attr *attr,
+					void *buf, uint16_t len,
+					uint16_t offset);
+
+	/** Attribute write callback
+	 *
+	 *  @param conn   The connection that is requesting to write
+	 *  @param attr   The attribute that's being read
+	 *  @param buf    Buffer with the data to write
+	 *  @param len    Number of bytes in the buffer
+	 *  @param offset Offset to start writing from
+	 *
+	 *  @return Number of bytes written, or in case of an error
+	 *          BT_GATT_ERR() with a specific ATT error code.
+	 */
+	ssize_t			(*write)(struct bt_conn *conn,
+					 const struct bt_gatt_attr *attr,
+					 const void *buf, uint16_t len,
+					 uint16_t offset);
+
+	/** Attribute flush callback
+	 *
+	 *  If this callback is provided (non-NULL) every write
+	 *  operation will be followed by a call to it. The expectation
+	 *  is for the attribute implementation to only commit the write
+	 *  result once this is called.
+	 *
+	 *  @param conn   The connection that is requesting to write
+	 *  @param attr   The attribute that's being read
+	 *  @param flags  Flags (BT_GATT_FLUSH_*)
+	 *
+	 *  @return Number of bytes flushed, or in case of an error
+	 *          BT_GATT_ERR() with a specific ATT error code.
+	 */
+	ssize_t			(*flush)(struct bt_conn *conn,
+					 const struct bt_gatt_attr *attr,
+					 uint8_t flags);
+
+	/** Attribute user data */
+	void			*user_data;
+	/** Attribute handle */
+	uint16_t		handle;
+	/** Attribute permissions */
+	uint8_t			perm;
+#if defined(CONFIG_BLUETOOTH_GATT_DYNAMIC_DB)
+	struct bt_gatt_attr	*_next;
+#endif /* CONFIG_BLUETOOTH_GATT_DYNAMIC_DB */
+};
+
+/** @brief Service Attribute Value. */
+struct bt_gatt_service {
+	/** Service UUID. */
+	const struct bt_uuid	*uuid;
+	/** Service end handle. */
+	uint16_t		end_handle;
+};
+
+/** @brief Include Attribute Value. */
+struct bt_gatt_include {
+	/** Service UUID. */
+	const struct bt_uuid	*uuid;
+	/** Service start handle. */
+	uint16_t		start_handle;
+	/** Service end handle. */
+	uint16_t		end_handle;
+};
+
+/* Characteristic Properties Bit field values */
+
+/** @def BT_GATT_CHRC_BROADCAST
+ *  @brief Characteristic broadcast property.
+ *
+ *  If set, permits broadcasts of the Characteristic Value using Server
+ *  Characteristic Configuration Descriptor.
+ */
+#define BT_GATT_CHRC_BROADCAST			0x01
+/** @def BT_GATT_CHRC_READ
+ *  @brief Characteristic read property.
+ *
+ *  If set, permits reads of the Characteristic Value.
+ */
+#define BT_GATT_CHRC_READ			0x02
+/** @def BT_GATT_CHRC_WRITE_WITHOUT_RESP
+ *  @brief Characteristic write without response property.
+ *
+ *  If set, permits write of the Characteristic Value without response.
+ */
+#define BT_GATT_CHRC_WRITE_WITHOUT_RESP		0x04
+/** @def BT_GATT_CHRC_WRITE
+ *  @brief Characteristic write with response property.
+ *
+ *  If set, permits write of the Characteristic Value with response.
+ */
+#define BT_GATT_CHRC_WRITE			0x08
+/** @def BT_GATT_CHRC_NOTIFY
+ *  @brief Characteristic notify property.
+ *
+ *  If set, permits notifications of a Characteristic Value without
+ *  acknowledgment.
+ */
+#define BT_GATT_CHRC_NOTIFY			0x10
+/** @def BT_GATT_CHRC_INDICATE
+ *  @brief Characteristic indicate property.
+ *
+ * If set, permits indications of a Characteristic Value with acknowledgment.
+ */
+#define BT_GATT_CHRC_INDICATE			0x20
+/** @def BT_GATT_CHRC_AUTH
+ *  @brief Characteristic Authenticated Signed Writes property.
+ *
+ *  If set, permits signed writes to the Characteristic Value.
+ */
+#define BT_GATT_CHRC_AUTH			0x40
+/** @def BT_GATT_CHRC_EXT_PROP
+ *  @brief Characteristic Extended Properties property.
+ *
+ * If set, additional characteristic properties are defined in the
+ * Characteristic Extended Properties Descriptor.
+ */
+#define BT_GATT_CHRC_EXT_PROP			0x80
+
+/** @brief Characteristic Attribute Value. */
+struct bt_gatt_chrc {
+	/** Characteristic UUID. */
+	const struct bt_uuid	*uuid;
+	/** Characteristic properties. */
+	uint8_t			properties;
+};
+
+/* Characteristic Extended Properties Bit field values */
+#define BT_GATT_CEP_RELIABLE_WRITE		0x0001
+#define BT_GATT_CEP_WRITABLE_AUX		0x0002
+
+/** @brief Characteristic Extended Properties Attribute Value. */
+struct bt_gatt_cep {
+	/** Characteristic Extended properties */
+	uint16_t		properties;
+};
+
+/* Client Characteristic Configuration Values */
+
+/** @def BT_GATT_CCC_NOTIFY
+ *  @brief Client Characteristic Configuration Notification.
+ *
+ *  If set, changes to Characteristic Value shall be notified.
+ */
+#define BT_GATT_CCC_NOTIFY			0x0001
+/** @def BT_GATT_CCC_INDICATE
+ *  @brief Client Characteristic Configuration Indication.
+ *
+ *  If set, changes to Characteristic Value shall be indicated.
+ */
+#define BT_GATT_CCC_INDICATE			0x0002
+
+/* Client Characteristic Configuration Attribute Value */
+struct bt_gatt_ccc {
+	/** Client Characteristic Configuration flags */
+	uint16_t		flags;
+};
+
+/** @brief GATT Characteristic Presentation Format Attribute Value. */
+struct bt_gatt_cpf {
+	/** Format of the value of the characteristic */
+	uint8_t format;
+	/** Exponent field to determine how the value of this characteristic is further formatted */
+	int8_t exponent;
+	/** Unit of the characteristic */
+	uint16_t unit;
+	/** Name space of the description */
+	uint8_t name_space;
+	/** Description of the characteristic as defined in a higher layer profile */
+	uint16_t description;
+} __packed;
+
+/* Server API */
+
+/** @brief Register attribute database.
+ *
+ *  Register GATT attribute database table. Applications can make use of
+ *  macros such as BT_GATT_PRIMARY_SERVICE, BT_GATT_CHARACTERISTIC,
+ *  BT_GATT_DESCRIPTOR, etc.
+ *
+ *  @param attrs Database table containing the available attributes.
+ *  @param count Size of the database table.
+ *
+ * @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_register(struct bt_gatt_attr *attrs, size_t count);
+
+enum {
+	BT_GATT_ITER_STOP = 0,
+	BT_GATT_ITER_CONTINUE,
+};
+
+/** @brief Attribute iterator callback.
+ *
+ *  @param attr Attribute found.
+ *  @param user_data Data given.
+ *
+ *  @return BT_GATT_ITER_CONTINUE if should continue to the next attribute
+ *  or BT_GATT_ITER_STOP to stop.
+ */
+typedef uint8_t (*bt_gatt_attr_func_t)(const struct bt_gatt_attr *attr,
+				       void *user_data);
+
+/** @brief Attribute iterator.
+ *
+ *  Iterate attributes in the given range.
+ *
+ *  @param start_handle Start handle.
+ *  @param end_handle End handle.
+ *  @param func Callback function.
+ *  @param user_data Data to pass to the callback.
+ */
+void bt_gatt_foreach_attr(uint16_t start_handle, uint16_t end_handle,
+			  bt_gatt_attr_func_t func, void *user_data);
+
+/** @brief Iterate to the next attribute
+ *
+ *  Iterate to the next attribute following a given attribute.
+ *
+ *  @param attr Current Attribute.
+ *
+ *  @return The next attribute or NULL if it cannot be found.
+ */
+struct bt_gatt_attr *bt_gatt_attr_next(const struct bt_gatt_attr *attr);
+
+/** @brief Generic Read Attribute value helper.
+ *
+ *  Read attribute value storing the result into buffer.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute to read.
+ *  @param buf Buffer to store the value.
+ *  @param buf_len Buffer length.
+ *  @param offset Start offset.
+ *  @param value Attribute value.
+ *  @param value_len Length of the attribute value.
+ *
+ *  @return int number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read(struct bt_conn *conn, const struct bt_gatt_attr *attr,
+			  void *buf, uint16_t buf_len, uint16_t offset,
+			  const void *value, uint16_t value_len);
+
+/** @brief Read Service Attribute helper.
+ *
+ *  Read service attribute value storing the result into buffer after
+ *  enconding it.
+ *  NOTE: Only use this with attributes which user_data is a bt_uuid.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute to read.
+ *  @param buf Buffer to store the value read.
+ *  @param len Buffer length.
+ *  @param offset Start offset.
+ *
+ *  @return int number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read_service(struct bt_conn *conn,
+				  const struct bt_gatt_attr *attr,
+				  void *buf, uint16_t len, uint16_t offset);
+
+/** @def BT_GATT_SERVICE
+ *  @brief Generic Service Declaration Macro.
+ *
+ *  Helper macro to declare a service attribute.
+ *
+ *  @param _uuid Service attribute type.
+ *  @param _data Service attribute value.
+ */
+#define BT_GATT_SERVICE(_uuid, _service)				\
+{									\
+	.uuid = _uuid,							\
+	.perm = BT_GATT_PERM_READ,					\
+	.read = bt_gatt_attr_read_service,				\
+	.user_data = _service,						\
+}
+
+/** @def BT_GATT_PRIMARY_SERVICE
+ *  @brief Primary Service Declaration Macro.
+ *
+ *  Helper macro to declare a primary service attribute.
+ *
+ *  @param _service Service attribute value.
+ */
+#define BT_GATT_PRIMARY_SERVICE(_service)				\
+{									\
+	.uuid = BT_UUID_GATT_PRIMARY,					\
+	.perm = BT_GATT_PERM_READ,					\
+	.read = bt_gatt_attr_read_service,				\
+	.user_data = _service,						\
+}
+
+/** @def BT_GATT_SECONDARY_SERVICE
+ *  @brief Secondary Service Declaration Macro.
+ *
+ *  Helper macro to declare a secondary service attribute.
+ *
+ *  @param _service Service attribute value.
+ */
+#define BT_GATT_SECONDARY_SERVICE(_service)				\
+{									\
+	.uuid = BT_UUID_GATT_SECONDARY,					\
+	.perm = BT_GATT_PERM_READ,					\
+	.read = bt_gatt_attr_read_service,				\
+	.user_data = _service,						\
+}
+
+/** @brief Read Include Attribute helper.
+ *
+ *  Read include service attribute value storing the result into buffer after
+ *  enconding it.
+ *  NOTE: Only use this with attributes which user_data is a bt_gatt_include.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute to read.
+ *  @param buf Buffer to store the value read.
+ *  @param len Buffer length.
+ *  @param offset Start offset.
+ *
+ *  @return int number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read_included(struct bt_conn *conn,
+				   const struct bt_gatt_attr *attr,
+				   void *buf, uint16_t len, uint16_t offset);
+
+/** @def BT_GATT_INCLUDE_SERVICE
+ *  @brief Include Service Declaration Macro.
+ *
+ *  Helper macro to declare a include service attribute.
+ *
+ *  @param _service Service attribute value.
+ */
+#define BT_GATT_INCLUDE_SERVICE(_service)				\
+{									\
+	.uuid = BT_UUID_GATT_INCLUDE,					\
+	.perm = BT_GATT_PERM_READ,					\
+	.read = bt_gatt_attr_read_included,				\
+	.user_data = _service,						\
+}
+
+/** @brief Read Characteristic Attribute helper.
+ *
+ *  Read characteristic attribute value storing the result into buffer after
+ *  enconding it.
+ *  NOTE: Only use this with attributes which user_data is a bt_gatt_chrc.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute to read.
+ *  @param buf Buffer to store the value read.
+ *  @param len Buffer length.
+ *  @param offset Start offset.
+ *
+ *  @return number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn,
+			       const struct bt_gatt_attr *attr, void *buf,
+			       uint16_t len, uint16_t offset);
+
+/** @def BT_GATT_CHARACTERISTIC
+ *  @brief Characteristic Declaration Macro.
+ *
+ *  Helper macro to declare a characteristic attribute.
+ *
+ *  @param _uuid Characteristic attribute uuid.
+ *  @param _props Characteristic attribute properties.
+ */
+#define BT_GATT_CHARACTERISTIC(_uuid, _props)				\
+{									\
+	.uuid = BT_UUID_GATT_CHRC,					\
+	.perm = BT_GATT_PERM_READ,					\
+	.read = bt_gatt_attr_read_chrc,					\
+	.user_data = (&(struct bt_gatt_chrc) { .uuid = _uuid,		\
+					       .properties = _props, }),\
+}
+
+/** @brief GATT CCC configuration entry. */
+struct bt_gatt_ccc_cfg {
+	/** Config peer address. */
+	bt_addr_le_t		peer;
+	/** Config peer value. */
+	uint16_t		value;
+	/** Config valid flag. */
+	uint8_t			valid;
+};
+
+/* Internal representation of CCC value */
+struct _bt_gatt_ccc {
+	struct bt_gatt_ccc_cfg	*cfg;
+	size_t			cfg_len;
+	uint16_t		value;
+	void			(*cfg_changed)(uint16_t value);
+};
+
+/** @brief Read Client Characteristic Configuration Attribute helper.
+ *
+ *  Read CCC attribute value storing the result into buffer after
+ *  enconding it.
+ *  NOTE: Only use this with attributes which user_data is a _bt_gatt_ccc.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute to read.
+ *  @param buf Buffer to store the value read.
+ *  @param len Buffer length.
+ *  @param offset Start offset.
+ *
+ *  @return number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read_ccc(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset);
+
+/** @brief Write Client Characteristic Configuration Attribute helper.
+ *
+ *  Write value in the buffer into CCC attribute.
+ *  NOTE: Only use this with attributes which user_data is a _bt_gatt_ccc.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute to read.
+ *  @param buf Buffer to store the value read.
+ *  @param len Buffer length.
+ *  @param offset Start offset.
+ *
+ *  @return number of bytes written in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_write_ccc(struct bt_conn *conn,
+			       const struct bt_gatt_attr *attr, const void *buf,
+			       uint16_t len, uint16_t offset);
+
+/** @def BT_GATT_CCC
+ *  @brief Client Characteristic Configuration Declaration Macro.
+ *
+ *  Helper macro to declare a CCC attribute.
+ *
+ *  @param _cfg Initial configuration.
+ *  @param _cfg_changed Configuration changed callback.
+ */
+#define BT_GATT_CCC(_cfg, _cfg_changed)					\
+{									\
+	.uuid = BT_UUID_GATT_CCC,					\
+	.perm = BT_GATT_PERM_READ | BT_GATT_PERM_WRITE,			\
+	.read = bt_gatt_attr_read_ccc,					\
+	.write = bt_gatt_attr_write_ccc,				\
+	.user_data = (&(struct _bt_gatt_ccc) { .cfg = _cfg,		\
+					       .cfg_len = ARRAY_SIZE(_cfg), \
+					       .cfg_changed = _cfg_changed, }),\
+}
+
+/** @brief Read Characteristic Extended Properties Attribute helper
+ *
+ *  Read CEP attribute value storing the result into buffer after
+ *  encoding it.
+ *  NOTE: Only use this with attributes which user_data is a bt_gatt_cep.
+ *
+ *  @param conn Connection object
+ *  @param attr Attribute to read
+ *  @param buf Buffer to store the value read
+ *  @param len Buffer length
+ *  @param offset Start offset
+ *
+ *  @return number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read_cep(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset);
+
+/** @def BT_GATT_CEP
+ *  @brief Characteristic Extended Properties Declaration Macro.
+ *
+ *  Helper macro to declare a CEP attribute.
+ *
+ *  @param _value Descriptor attribute value.
+ */
+#define BT_GATT_CEP(_value)						\
+{									\
+	.uuid = BT_UUID_GATT_CEP,					\
+	.perm = BT_GATT_PERM_READ,					\
+	.read = bt_gatt_attr_read_cep,					\
+	.user_data = _value,						\
+}
+
+/** @brief Read Characteristic User Description Descriptor Attribute helper
+ *
+ *  Read CUD attribute value storing the result into buffer after
+ *  encoding it.
+ *  NOTE: Only use this with attributes which user_data is a NULL-terminated C string.
+ *
+ *  @param conn Connection object
+ *  @param attr Attribute to read
+ *  @param buf Buffer to store the value read
+ *  @param len Buffer length
+ *  @param offset Start offset
+ *
+ *  @return number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read_cud(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset);
+
+/** @def BT_GATT_CUD
+ *  @brief Characteristic User Format Descriptor Declaration Macro.
+ *
+ *  Helper macro to declare a CUD attribute.
+ *
+ *  @param _value User description NULL-terminated C string.
+ *  @param _perm Descriptor attribute access permissions.
+ */
+#define BT_GATT_CUD(_value, _perm)					\
+{									\
+	.uuid = BT_UUID_GATT_CUD,					\
+	.perm = _perm,							\
+	.read = bt_gatt_attr_read_cud,					\
+	.user_data = _value,						\
+}
+
+/** @brief Read Characteristic Presentation format Descriptor Attribute helper
+ *
+ *  Read CPF attribute value storing the result into buffer after
+ *  encoding it.
+ *  NOTE: Only use this with attributes which user_data is a bt_gatt_pf.
+ *
+ *  @param conn Connection object
+ *  @param attr Attribute to read
+ *  @param buf Buffer to store the value read
+ *  @param len Buffer length
+ *  @param offset Start offset
+ *
+ *  @return number of bytes read in case of success or negative values in
+ *  case of error.
+ */
+ssize_t bt_gatt_attr_read_cpf(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset);
+
+/** @def BT_GATT_CPF
+ *  @brief Characteristic Presentation Format Descriptor Declaration Macro.
+ *
+ *  Helper macro to declare a CPF attribute.
+ *
+ *  @param _value Descriptor attribute value.
+ */
+#define BT_GATT_CPF(_value)						\
+{									\
+	.uuid = BT_UUID_GATT_CPF,					\
+	.perm = BT_GATT_PERM_READ,					\
+	.read = bt_gatt_attr_read_cpf,					\
+	.user_data = _value,						\
+}
+
+/** @def BT_GATT_DESCRIPTOR
+ *  @brief Descriptor Declaration Macro.
+ *
+ *  Helper macro to declare a descriptor attribute.
+ *
+ *  @param _uuid Descriptor attribute uuid.
+ *  @param _perm Descriptor attribute access permissions.
+ *  @param _read Descriptor attribute read callback.
+ *  @param _write Descriptor attribute write callback.
+ *  @param _value Descriptor attribute value.
+ */
+#define BT_GATT_DESCRIPTOR(_uuid, _perm, _read, _write, _value)		\
+{									\
+	.uuid = _uuid,							\
+	.perm = _perm,							\
+	.read = _read,							\
+	.write = _write,						\
+	.user_data = _value,						\
+}
+
+/** @def BT_GATT_LONG_DESCRIPTOR
+ *  @brief Descriptor Declaration Macro.
+ *
+ *  Helper macro to declare a descriptor attribute.
+ *
+ *  @param _uuid Descriptor attribute uuid.
+ *  @param _perm Descriptor attribute access permissions.
+ *  @param _read Descriptor attribute read callback.
+ *  @param _write Descriptor attribute write callback.
+ *  @param _flush Descriptor attribute flush callback.
+ *  @param _value Descriptor attribute value.
+ */
+#define BT_GATT_LONG_DESCRIPTOR(_uuid, _perm, _read, _write, _flush, _value) \
+{									\
+	.uuid = _uuid,							\
+	.perm = _perm,							\
+	.read = _read,							\
+	.write = _write,						\
+	.flush = _flush,						\
+	.user_data = _value,						\
+}
+
+/** @brief Notify sent callback
+ *
+ *  This means that the complete attribute has been sent. This does not mean it
+ *  has been received however (use indicate for this).
+ *  This shall be used to flow control the callee to avoid flooding the ble
+ *  controller.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute object.
+ *  @param err 0 if none
+ */
+typedef void (*bt_gatt_notify_sent_func_t)(struct bt_conn *conn, struct bt_gatt_attr *attr,
+					   uint8_t err);
+
+/** @brief Notify attribute value change.
+ *
+ *  Send notification of attribute value change, if connection is NULL notify
+ *  all peer that have notification enabled via CCC otherwise do a direct
+ *  notification only the given connection.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute object.
+ *  @param value Attribute value.
+ *  @param len Attribute value length.
+ *  @param cb callback function called when send is complete (or NULL)
+ */
+int bt_gatt_notify(struct bt_conn *conn, const struct bt_gatt_attr *attr,
+		   const void *data, uint16_t len,
+		   bt_gatt_notify_sent_func_t cb);
+
+/** @brief Indication complete result callback.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute object.
+ *  @param err: 0 success, error in the other case
+ */
+typedef void (*bt_gatt_indicate_func_t)(struct bt_conn *conn,
+					const struct bt_gatt_attr *attr,
+					int err);
+
+/** @brief GATT Indicate Value parameters */
+struct bt_gatt_indicate_params {
+	/** Indicate Attribute object*/
+	const struct bt_gatt_attr *attr;
+	/** Indicate Value callback */
+	bt_gatt_indicate_func_t func;
+	/** Indicate Value data*/
+	const void *data;
+	/** Indicate Value length*/
+	uint16_t len;
+};
+
+/** @brief Indicate attribute value change.
+ *
+ *  Send an indication of attribute value change.
+ *  Note: This function should only be called if CCC is declared with
+ *  BT_GATT_CCC otherwise it cannot find a valid peer configuration.
+ *
+ *  Note: This procedure is asynchronous therefore the parameters need to
+ *  remains valid while it is active.
+ *
+ *  @param conn Connection object.
+ *  @param params Indicate parameters.
+ *
+ *  @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_indicate(struct bt_conn *conn,
+		     struct bt_gatt_indicate_params *params);
+
+#if defined(CONFIG_BLUETOOTH_GATT_CLIENT)
+/* Client API */
+
+/** @brief Response callback function
+ *
+ *  @param conn Connection object.
+ *  @param err Error code.
+ */
+typedef void (*bt_gatt_rsp_func_t)(struct bt_conn *conn, uint8_t err);
+
+/** @brief Exchange MTU
+ *
+ * This client procedure can be used to set the MTU to the maximum possible
+ * size the buffers can hold.
+ * NOTE: Shall only be used once per connection.
+ *
+ *  @param conn Connection object.
+ *  @param func Exchange MTU Response callback function.
+ */
+int bt_gatt_exchange_mtu(struct bt_conn *conn, bt_gatt_rsp_func_t func);
+
+struct bt_gatt_discover_params;
+
+/** @brief Discover attribute callback function.
+ *
+ *  @param conn Connection object.
+ *  @param attr Attribute found.
+ *  @param params Discovery parameters given.
+ *
+ *  If discovery procedure has completed this callback will be called with
+ *  attr set to NULL. This will not happen if procedure was stopped by returning
+ *  BT_GATT_ITER_STOP.
+ *
+ *  @return BT_GATT_ITER_CONTINUE if should continue attribute discovery
+ *  or BT_GATT_ITER_STOP to stop discovery procedure.
+ */
+typedef uint8_t (*bt_gatt_discover_func_t)(struct bt_conn *conn,
+					   const struct bt_gatt_attr *attr,
+					   struct bt_gatt_discover_params *params);
+
+enum {
+	BT_GATT_DISCOVER_PRIMARY,
+	BT_GATT_DISCOVER_SECONDARY,
+	BT_GATT_DISCOVER_INCLUDE,
+	BT_GATT_DISCOVER_CHARACTERISTIC,
+	BT_GATT_DISCOVER_DESCRIPTOR,
+};
+
+/** @brief GATT Discover Attributes parameters */
+struct bt_gatt_discover_params {
+	/** Discover UUID type */
+	struct bt_uuid *uuid;
+	/** Discover attribute callback */
+	bt_gatt_discover_func_t func;
+	/** Discover start handle */
+	uint16_t start_handle;
+	/** Discover end handle */
+	uint16_t end_handle;
+	/** Discover type */
+	uint8_t type;
+};
+
+/** @brief GATT Discover function
+ *
+ *  This procedure is used by a client to discover attributes on a server.
+ *
+ *  Primary Service Discovery: Procedure allows to discover specific Primary
+ *                             Service based on UUID.
+ *  Include Service Discovery: Procedure allows to discover all Include Services
+ *                             within specified range.
+ *  Characteristic Discovery:  Procedure allows to discover all characteristics
+ *                             within specified handle range as well as
+ *                             discover characteristics with specified UUID.
+ *  Descriptors Discovery:     Procedure allows to discover all characteristic
+ *                             descriptors within specified range.
+ *
+ *  For each attribute found the callback is called which can then decide
+ *  whether to continue discovering or stop.
+ *
+ *  Note: This procedure is asynchronous therefore the parameters need to
+ *  remains valid while it is active.
+ *
+ *  @param conn Connection object.
+ *  @param params Discover parameters.
+ *
+ *  @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_discover(struct bt_conn *conn,
+		     struct bt_gatt_discover_params *params);
+
+struct bt_gatt_read_params;
+
+/** @brief Read callback function
+ *
+ *  @param conn Connection object.
+ *  @param err Error code.
+ *  @param params Read parameters used.
+ *  @param data Attribute value data. NULL means read has completed.
+ *  @param length Attribute value length.
+ */
+typedef uint8_t (*bt_gatt_read_func_t)(struct bt_conn *conn, int err,
+				       struct bt_gatt_read_params *params,
+				       const void *data, uint16_t length);
+
+/** @brief GATT Read parameters */
+struct bt_gatt_read_params {
+	/** Read attribute callback */
+	bt_gatt_read_func_t func;
+	/** Handles count.
+	 * If equals to 1 single.handle and single.offset are used.
+	 * If >1 Read Multiple Characteristic Values is performed and handles
+	 * are used.
+	 */
+	size_t handle_count;
+	union {
+		struct {
+			/** Attribute handle */
+			uint16_t handle;
+			/** Attribute data offset */
+			uint16_t offset;
+		} single;
+		/** Handles to read in Read Multiple Characteristic Values */
+		uint16_t *handles;
+	};
+};
+
+/** @brief Read Attribute Value by handle
+ *
+ *  This procedure read the attribute value and return it to the callback.
+ *
+ *  Note: This procedure is asynchronous therefore the parameters need to
+ *  remains valid while it is active.
+ *
+ *  @param conn Connection object.
+ *  @param params Read parameters.
+ *
+ *  @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_read(struct bt_conn *conn, struct bt_gatt_read_params *params);
+
+/** @brief Write Response callback function
+ *
+ *  @param conn Connection object.
+ *  @param err  Error code.
+ *  @param data Data pointer in the write request.
+ */
+typedef void (*bt_gatt_write_rsp_func_t)(struct bt_conn *conn, uint8_t err, const void *data);
+
+/** @brief Write Attribute Value by handle
+ *
+ * This procedure write the attribute value and return the result in the
+ * callback.
+ *
+ * @param conn Connection object.
+ * @param handle Attribute handle.
+ * @param offset Attribute data offset.
+ * @param data Data to be written.
+ * @param length Data length.
+ * @param func Callback function.
+ *
+ * @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_write(struct bt_conn *conn, uint16_t handle, uint16_t offset,
+		  const void *data, uint16_t length, bt_gatt_write_rsp_func_t func);
+
+/** @brief Write Attribute Value by handle without response
+ *
+ * This procedure write the attribute value without requiring an
+ * acknowledgement that the write was successfully performed
+ *
+ * @param conn Connection object.
+ * @param handle Attribute handle.
+ * @param data Data to be written.
+ * @param length Data length.
+ * @param sign Whether to sign data
+ *
+ * @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_write_without_response(struct bt_conn *conn, uint16_t handle,
+				   const void *data, uint16_t length,
+				   bool sign);
+
+struct bt_gatt_subscribe_params;
+
+/** @brief Notification callback function
+ *
+ *  @param conn Connection object.
+ *  @param params Subscription parameters.
+ *  @param data Attribute value data. If NULL then subscription was removed.
+ *  @param length Attribute value length.
+ */
+typedef uint8_t (*bt_gatt_notify_func_t)(struct bt_conn *conn,
+					 struct bt_gatt_subscribe_params *params,
+					 const void *data, uint16_t length);
+
+/** @brief GATT Subscribe parameters */
+struct bt_gatt_subscribe_params {
+	bt_addr_le_t _peer;
+	/** Notification value callback */
+	bt_gatt_notify_func_t notify;
+	/** Subscribe value handle */
+	uint16_t value_handle;
+	/** Subscribe CCC handle */
+	uint16_t ccc_handle;
+	/** Subscribe value */
+	uint16_t value;
+	struct bt_gatt_subscribe_params *_next;
+};
+
+/** @brief Subscribe Attribute Value Notification
+ *
+ *  This procedure subscribe to value notification using the Client
+ *  Characteristic Configuration handle.
+ *  If notification received subscribe value callback is called to return
+ *  notified value. One may then decide whether to unsubscribe directly from
+ *  this callback. Notification callback with NULL data will not be called if
+ *  subscription was removed by this method.
+ *
+ *  Note: This procedure is asynchronous therefore the parameters need to
+ *  remains valid while it is active.
+ *
+ *  @param conn Connection object.
+ *  @param params Subscribe parameters.
+ *
+ *  @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_subscribe(struct bt_conn *conn,
+		      struct bt_gatt_subscribe_params *params);
+
+/** @brief Unsubscribe Attribute Value Notification
+ *
+ * This procedure unsubscribe to value notification using the Client
+ * Characteristic Configuration handle. Notification callback with NULL data
+ * will not be called if subscription was removed by this call.
+ *
+ * @param conn Connection object.
+ * @param params Subscribe parameters.
+ *
+ * @return 0 in case of success or negative value in case of error.
+ */
+int bt_gatt_unsubscribe(struct bt_conn *conn,
+			struct bt_gatt_subscribe_params *params);
+
+/** @brief Cancel GATT pending request
+ *
+ *  @param conn Connection object.
+ */
+void bt_gatt_cancel(struct bt_conn *conn);
+
+#endif /* CONFIG_BLUETOOTH_GATT_CLIENT */
+#endif /* CONFIG_BLUETOOTH_CENTRAL || CONFIG_BLUETOOTH_PERIPHERAL */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BT_GATT_H */
diff --git a/system/libarc32_arduino101/drivers/bluetooth/hci.h b/system/libarc32_arduino101/drivers/bluetooth/hci.h
new file mode 100644
index 00000000..f7ff52c1
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/bluetooth/hci.h
@@ -0,0 +1,683 @@
+/* hci.h - Bluetooth Host Control Interface definitions */
+
+/*
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * 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.
+ */
+#ifndef __BT_HCI_H
+#define __BT_HCI_H
+
+//#include <toolchain.h>
+#include <stdint.h>
+#include <string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BT_ADDR_LE_PUBLIC  0x00
+#define BT_ADDR_LE_RANDOM  0x01
+
+typedef struct {
+	uint8_t  val[6];
+} bt_addr_t;
+
+typedef struct {
+	uint8_t  type;
+	uint8_t  val[6];
+} bt_addr_le_t;
+
+#define BT_ADDR_ANY    (&(bt_addr_t) {{0, 0, 0, 0, 0, 0} })
+#define BT_ADDR_LE_ANY (&(bt_addr_le_t) { 0, {0, 0, 0, 0, 0, 0} })
+
+static inline int bt_addr_cmp(const bt_addr_t *a, const bt_addr_t *b)
+{
+	return memcmp(a, b, sizeof(*a));
+}
+
+static inline int bt_addr_le_cmp(const bt_addr_le_t *a, const bt_addr_le_t *b)
+{
+	return memcmp(a, b, sizeof(*a));
+}
+
+static inline void bt_addr_copy(bt_addr_t *dst, const bt_addr_t *src)
+{
+	memcpy(dst, src, sizeof(*dst));
+}
+
+static inline void bt_addr_le_copy(bt_addr_le_t *dst, const bt_addr_le_t *src)
+{
+	memcpy(dst, src, sizeof(*dst));
+}
+
+/* HCI Error Codes */
+#define BT_HCI_ERR_UNKNOWN_CONN_ID		0x02
+#define BT_HCI_ERR_AUTHENTICATION_FAIL		0x05
+#define BT_HCI_ERR_INSUFFICIENT_RESOURCES	0x0d
+#define BT_HCI_ERR_REMOTE_USER_TERM_CONN	0x13
+#define BT_HCI_ERR_PAIRING_NOT_ALLOWED		0x18
+#define BT_HCI_ERR_UNSUPP_REMOTE_FEATURE	0x1a
+#define BT_HCI_ERR_INVALID_LL_PARAMS		0x1e
+#define BT_HCI_ERR_UNSPECIFIED			0x1f
+#define BT_HCI_ERR_PAIRING_NOT_SUPPORTED	0x29
+#define BT_HCI_ERR_UNACCEPT_CONN_PARAMS		0x3b
+#define BT_HCI_ERR_DIRECTED_ADV_TIMEOUT		0x3c
+
+/* EIR/AD data type definitions */
+#define BT_DATA_FLAGS			0x01 /* AD flags */
+#define BT_DATA_UUID16_SOME		0x02 /* 16-bit UUID, more available */
+#define BT_DATA_UUID16_ALL		0x03 /* 16-bit UUID, all listed */
+#define BT_DATA_UUID32_SOME		0x04 /* 32-bit UUID, more available */
+#define BT_DATA_UUID32_ALL		0x05 /* 32-bit UUID, all listed */
+#define BT_DATA_UUID128_SOME		0x06 /* 128-bit UUID, more available */
+#define BT_DATA_UUID128_ALL		0x07 /* 128-bit UUID, all listed */
+#define BT_DATA_NAME_SHORTENED		0x08 /* Shortened name */
+#define BT_DATA_NAME_COMPLETE		0x09 /* Complete name */
+#define BT_DATA_TX_POWER		0x0a /* Tx Power */
+#define BT_DATA_SOLICIT16		0x14 /* Solicit UUIDs, 16-bit */
+#define BT_DATA_SOLICIT128		0x15 /* Solicit UUIDs, 128-bit */
+#define BT_DATA_SVC_DATA16		0x16 /* Service data, 16-bit UUID */
+#define BT_DATA_GAP_APPEARANCE		0x19 /* GAP appearance */
+#define BT_DATA_SOLICIT32		0x1f /* Solicit UUIDs, 32-bit */
+#define BT_DATA_SVC_DATA32		0x20 /* Service data, 32-bit UUID */
+#define BT_DATA_SVC_DATA128		0x21 /* Service data, 128-bit UUID */
+#define BT_DATA_MANUFACTURER_DATA	0xff /* Manufacturer Specific Data */
+
+#define BT_LE_AD_LIMITED		0x01 /* Limited Discoverable */
+#define BT_LE_AD_GENERAL		0x02 /* General Discoverable */
+#define BT_LE_AD_NO_BREDR		0x04 /* BR/EDR not supported */
+
+struct bt_hci_evt_hdr {
+	uint8_t  evt;
+	uint8_t  len;
+} __packed;
+
+#define BT_ACL_START_NO_FLUSH		0x00
+#define BT_ACL_CONT			0x01
+#define BT_ACL_START			0x02
+
+#define bt_acl_handle(h)		((h) & 0x0fff)
+#define bt_acl_flags(h)			((h) >> 12)
+#define bt_acl_handle_pack(h, f)	((h) | ((f) << 12))
+
+struct bt_hci_acl_hdr {
+	uint16_t handle;
+	uint16_t len;
+} __packed;
+
+struct bt_hci_cmd_hdr {
+	uint16_t opcode;
+	uint8_t  param_len;
+} __packed;
+
+/* LMP features */
+#define BT_LMP_NO_BREDR				0x20
+#define BT_LMP_LE				0x40
+
+/* LE features */
+#define BT_HCI_LE_ENCRYPTION			0x01
+#define BT_HCI_LE_CONN_PARAM_REQ_PROC		0x02
+#define BT_HCI_LE_SLAVE_FEATURES		0x08
+
+/* Bonding/authentication types */
+#define BT_HCI_NO_BONDING			0x00
+#define BT_HCI_NO_BONDING_MITM			0x01
+#define BT_HCI_DEDICATED_BONDING		0x02
+#define BT_HCI_DEDICATED_BONDING_MITM		0x03
+#define BT_HCI_GENERAL_BONDING			0x04
+#define BT_HCI_GENERAL_BONDING_MITM		0x05
+
+/* I/O capabilities */
+#define BT_IO_DISPLAY_ONLY			0x00
+#define BT_IO_DISPLAY_YESNO			0x01
+#define BT_IO_KEYBOARD_ONLY			0x02
+#define BT_IO_NO_INPUT_OUTPUT			0x03
+
+/* Defined GAP timers */
+#define BT_GAP_SCAN_FAST_INTERVAL		0x0060	/* 60 ms    */
+#define BT_GAP_SCAN_FAST_WINDOW			0x0030  /* 30 ms    */
+#define BT_GAP_SCAN_SLOW_INTERVAL_1		0x0800  /* 1.28 s   */
+#define BT_GAP_SCAN_SLOW_WINDOW_1		0x0012  /* 11.25 ms */
+#define BT_GAP_SCAN_SLOW_INTERVAL_2		0x1000  /* 2.56 s   */
+#define BT_GAP_SCAN_SLOW_WINDOW_2		0x0012  /* 11.25 ms */
+#define BT_GAP_ADV_FAST_INT_MIN_1		0x0030	/* 30 ms    */
+#define BT_GAP_ADV_FAST_INT_MAX_1		0x0060  /* 60 ms    */
+#define BT_GAP_ADV_FAST_INT_MIN_2		0x00a0  /* 100 ms   */
+#define BT_GAP_ADV_FAST_INT_MAX_2		0x00f0  /* 150 ms   */
+#define BT_GAP_ADV_SLOW_INT_MIN			0x0640  /* 1 s      */
+#define BT_GAP_ADV_SLOW_INT_MAX			0x0780  /* 1.2 s    */
+#define BT_GAP_INIT_CONN_INT_MIN		0x0018  /* 30 ms    */
+#define BT_GAP_INIT_CONN_INT_MAX		0x0028  /* 50 ms    */
+
+/* HCI BR/EDR link types */
+#define BT_HCI_SCO				0x00
+#define BT_HCI_ACL				0x01
+#define BT_HCI_ESCO				0x02
+
+/* OpCode Group Fields */
+#define BT_OGF_LINK_CTRL			0x01
+#define BT_OGF_BASEBAND				0x03
+#define BT_OGF_INFO				0x04
+#define BT_OGF_LE				0x08
+
+/* Construct OpCode from OGF and OCF */
+#define BT_OP(ogf, ocf)				((ocf) | ((ogf) << 10))
+
+#define BT_HCI_OP_DISCONNECT			BT_OP(BT_OGF_LINK_CTRL, 0x0006)
+struct bt_hci_cp_disconnect {
+	uint16_t handle;
+	uint8_t  reason;
+} __packed;
+
+#define BT_HCI_OP_ACCEPT_CONN_REQ		BT_OP(BT_OGF_LINK_CTRL, 0x0009)
+struct bt_hci_cp_accept_conn_req {
+	bt_addr_t bdaddr;
+	uint8_t   role;
+} __packed;
+
+#define BT_HCI_OP_REJECT_CONN_REQ		BT_OP(BT_OGF_LINK_CTRL, 0x000a)
+struct bt_hci_cp_reject_conn_req {
+	bt_addr_t bdaddr;
+	uint8_t   reason;
+} __packed;
+
+#define BT_HCI_OP_LINK_KEY_REPLY		BT_OP(BT_OGF_LINK_CTRL, 0x000b)
+struct bt_hci_cp_link_key_reply {
+	bt_addr_t bdaddr;
+	uint8_t   link_key[16];
+} __packed;
+
+#define BT_HCI_OP_LINK_KEY_NEG_REPLY		BT_OP(BT_OGF_LINK_CTRL, 0x000c)
+struct bt_hci_cp_link_key_neg_reply {
+	bt_addr_t bdaddr;
+} __packed;
+
+#define BT_HCI_OP_PIN_CODE_REPLY		BT_OP(BT_OGF_LINK_CTRL, 0x000d)
+struct bt_hci_cp_pin_code_reply {
+	bt_addr_t bdaddr;
+	uint8_t   pin_len;
+	uint8_t   pin_code[16];
+} __packed;
+struct bt_hci_rp_pin_code_reply {
+	uint8_t   status;
+	bt_addr_t bdaddr;
+} __packed;
+
+#define BT_HCI_OP_PIN_CODE_NEG_REPLY		BT_OP(BT_OGF_LINK_CTRL, 0x000e)
+struct bt_hci_cp_pin_code_neg_reply {
+	bt_addr_t bdaddr;
+} __packed;
+struct bt_hci_rp_pin_code_neg_reply {
+	uint8_t   status;
+	bt_addr_t bdaddr;
+} __packed;
+
+#define BT_HCI_OP_IO_CAPABILITY_REPLY		BT_OP(BT_OGF_LINK_CTRL, 0x002b)
+struct bt_hci_cp_io_capability_reply {
+	bt_addr_t bdaddr;
+	uint8_t   capability;
+	uint8_t   oob_data;
+	uint8_t   authentication;
+} __packed;
+
+#define BT_HCI_OP_IO_CAPABILITY_NEG_REPLY	BT_OP(BT_OGF_LINK_CTRL, 0x0034)
+struct bt_hci_cp_io_capability_neg_reply {
+	bt_addr_t bdaddr;
+	uint8_t   reason;
+} __packed;
+
+#define BT_HCI_OP_SET_EVENT_MASK		BT_OP(BT_OGF_BASEBAND, 0x0001)
+struct bt_hci_cp_set_event_mask {
+	uint8_t  events[8];
+} __packed;
+
+#define BT_HCI_OP_RESET				BT_OP(BT_OGF_BASEBAND, 0x0003)
+
+#define BT_HCI_OP_WRITE_SCAN_ENABLE		BT_OP(BT_OGF_BASEBAND, 0x001a)
+#define BT_BREDR_SCAN_DISABLED			0x00
+#define BT_BREDR_SCAN_INQUIRY			0x01
+#define BT_BREDR_SCAN_PAGE			0x02
+
+#define BT_HCI_CTL_TO_HOST_FLOW_ENABLE		0x01
+#define BT_HCI_OP_SET_CTL_TO_HOST_FLOW		BT_OP(BT_OGF_BASEBAND, 0x0031)
+
+#define BT_HCI_OP_HOST_BUFFER_SIZE		BT_OP(BT_OGF_BASEBAND, 0x0033)
+struct bt_hci_cp_host_buffer_size {
+	uint16_t acl_mtu;
+	uint8_t  sco_mtu;
+	uint16_t acl_pkts;
+	uint16_t sco_pkts;
+} __packed;
+
+struct bt_hci_handle_count {
+	uint16_t handle;
+	uint16_t count;
+} __packed;
+
+#define BT_HCI_OP_HOST_NUM_COMPLETED_PACKETS	BT_OP(BT_OGF_BASEBAND, 0x0035)
+struct bt_hci_cp_host_num_completed_packets {
+	uint8_t  num_handles;
+	struct bt_hci_handle_count h[0];
+} __packed;
+
+#define BT_HCI_OP_WRITE_SSP_MODE		BT_OP(BT_OGF_BASEBAND, 0x0056)
+struct bt_hci_cp_write_ssp_mode {
+	uint8_t mode;
+} __packed;
+
+#define BT_HCI_OP_LE_WRITE_LE_HOST_SUPP		BT_OP(BT_OGF_BASEBAND, 0x006d)
+struct bt_hci_cp_write_le_host_supp {
+	uint8_t  le;
+	uint8_t  simul;
+} __packed;
+
+#define BT_HCI_OP_READ_LOCAL_VERSION_INFO	BT_OP(BT_OGF_INFO, 0x0001)
+struct bt_hci_rp_read_local_version_info {
+	uint8_t  status;
+	uint8_t  hci_version;
+	uint16_t hci_revision;
+	uint8_t  lmp_version;
+	uint16_t manufacturer;
+	uint16_t lmp_subversion;
+} __packed;
+
+#define BT_HCI_OP_READ_SUPPORTED_COMMANDS	BT_OP(BT_OGF_INFO, 0x0002)
+struct bt_hci_rp_read_supported_commands {
+	uint8_t  status;
+	uint8_t  commands[36];
+} __packed;
+
+#define BT_HCI_OP_READ_LOCAL_FEATURES		BT_OP(BT_OGF_INFO, 0x0003)
+struct bt_hci_rp_read_local_features {
+	uint8_t  status;
+	uint8_t  features[8];
+} __packed;
+
+#define BT_HCI_OP_READ_BUFFER_SIZE		BT_OP(BT_OGF_INFO, 0x0005)
+struct bt_hci_rp_read_buffer_size {
+	uint8_t  status;
+	uint16_t acl_max_len;
+	uint8_t  sco_max_len;
+	uint16_t acl_max_num;
+	uint16_t sco_max_num;
+} __packed;
+
+#define BT_HCI_OP_READ_BD_ADDR			BT_OP(BT_OGF_INFO, 0x0009)
+struct bt_hci_rp_read_bd_addr {
+	uint8_t   status;
+	bt_addr_t bdaddr;
+} __packed;
+
+#define BT_HCI_OP_LE_SET_EVENT_MASK		BT_OP(BT_OGF_LE, 0x0001)
+struct bt_hci_cp_le_set_event_mask {
+	uint8_t events[8];
+} __packed;
+struct bt_hci_rp_le_set_event_mask {
+	uint8_t status;
+} __packed;
+
+#define BT_HCI_OP_LE_READ_BUFFER_SIZE		BT_OP(BT_OGF_LE, 0x0002)
+struct bt_hci_rp_le_read_buffer_size {
+	uint8_t  status;
+	uint16_t le_max_len;
+	uint8_t  le_max_num;
+} __packed;
+
+#define BT_HCI_OP_LE_READ_LOCAL_FEATURES	BT_OP(BT_OGF_LE, 0x0003)
+struct bt_hci_rp_le_read_local_features {
+	uint8_t  status;
+	uint8_t  features[8];
+} __packed;
+
+#define BT_HCI_OP_LE_SET_RANDOM_ADDRESS		BT_OP(BT_OGF_LE, 0x0005)
+
+/* Advertising types */
+#define BT_LE_ADV_IND				0x00
+#define BT_LE_ADV_DIRECT_IND			0x01
+#define BT_LE_ADV_SCAN_IND			0x02
+#define BT_LE_ADV_NONCONN_IND			0x03
+#define BT_LE_ADV_DIRECT_IND_LOW_DUTY		0x04
+/* Needed in advertising reports when getting info about */
+#define BT_LE_ADV_SCAN_RSP			0x04
+
+#define BT_HCI_OP_LE_SET_ADV_PARAMETERS		BT_OP(BT_OGF_LE, 0x0006)
+struct bt_hci_cp_le_set_adv_parameters {
+	uint16_t     min_interval;
+	uint16_t     max_interval;
+	uint8_t      type;
+	uint8_t      own_addr_type;
+	bt_addr_le_t direct_addr;
+	uint8_t      channel_map;
+	uint8_t      filter_policy;
+} __packed;
+
+#define BT_HCI_OP_LE_SET_ADV_DATA		BT_OP(BT_OGF_LE, 0x0008)
+struct bt_hci_cp_le_set_adv_data {
+	uint8_t  len;
+	uint8_t  data[31];
+} __packed;
+
+#define BT_HCI_OP_LE_SET_SCAN_RSP_DATA		BT_OP(BT_OGF_LE, 0x0009)
+struct bt_hci_cp_le_set_scan_rsp_data {
+	uint8_t  len;
+	uint8_t  data[31];
+} __packed;
+
+#define BT_HCI_LE_ADV_DISABLE			0x00
+#define BT_HCI_LE_ADV_ENABLE			0x01
+
+#define BT_HCI_OP_LE_SET_ADV_ENABLE		BT_OP(BT_OGF_LE, 0x000a)
+struct bt_hci_cp_le_set_adv_enable {
+	uint8_t  enable;
+} __packed;
+
+/* Scan types */
+#define BT_HCI_OP_LE_SET_SCAN_PARAMS		BT_OP(BT_OGF_LE, 0x000b)
+#define BT_HCI_LE_SCAN_PASSIVE			0x00
+#define BT_HCI_LE_SCAN_ACTIVE			0x01
+
+struct bt_hci_cp_le_set_scan_params {
+	uint8_t  scan_type;
+	uint16_t interval;
+	uint16_t window;
+	uint8_t  addr_type;
+	uint8_t  filter_policy;
+} __packed;
+
+#define BT_HCI_OP_LE_SET_SCAN_ENABLE		BT_OP(BT_OGF_LE, 0x000c)
+
+#define BT_HCI_LE_SCAN_DISABLE			0x00
+#define BT_HCI_LE_SCAN_ENABLE			0x01
+
+#define BT_HCI_LE_SCAN_FILTER_DUP_DISABLE	0x00
+#define BT_HCI_LE_SCAN_FILTER_DUP_ENABLE	0x01
+
+struct bt_hci_cp_le_set_scan_enable {
+	uint8_t  enable;
+	uint8_t  filter_dup;
+} __packed;
+
+#define BT_HCI_OP_LE_CREATE_CONN		BT_OP(BT_OGF_LE, 0x000d)
+struct bt_hci_cp_le_create_conn {
+	uint16_t     scan_interval;
+	uint16_t     scan_window;
+	uint8_t      filter_policy;
+	bt_addr_le_t peer_addr;
+	uint8_t      own_addr_type;
+	uint16_t     conn_interval_min;
+	uint16_t     conn_interval_max;
+	uint16_t     conn_latency;
+	uint16_t     supervision_timeout;
+	uint16_t     min_ce_len;
+	uint16_t     max_ce_len;
+} __packed;
+
+#define BT_HCI_OP_LE_CREATE_CONN_CANCEL		BT_OP(BT_OGF_LE, 0x000e)
+
+#define BT_HCI_OP_LE_CONN_UPDATE		BT_OP(BT_OGF_LE, 0x0013)
+struct hci_cp_le_conn_update {
+	uint16_t handle;
+	uint16_t conn_interval_min;
+	uint16_t conn_interval_max;
+	uint16_t conn_latency;
+	uint16_t supervision_timeout;
+	uint16_t min_ce_len;
+	uint16_t max_ce_len;
+} __packed;
+
+#define BT_HCI_OP_LE_READ_REMOTE_FEATURES	BT_OP(BT_OGF_LE, 0x0016)
+struct bt_hci_cp_le_read_remote_features {
+	uint16_t  handle;
+} __packed;
+
+#define BT_HCI_OP_LE_ENCRYPT			BT_OP(BT_OGF_LE, 0x0017)
+struct bt_hci_cp_le_encrypt {
+	uint8_t  key[16];
+	uint8_t  plaintext[16];
+} __packed;
+struct bt_hci_rp_le_encrypt {
+	uint8_t  status;
+	uint8_t  enc_data[16];
+} __packed;
+
+#define BT_HCI_OP_LE_RAND			BT_OP(BT_OGF_LE, 0x0018)
+struct bt_hci_rp_le_rand {
+	uint8_t  status;
+	uint8_t  rand[8];
+} __packed;
+
+#define BT_HCI_OP_LE_START_ENCRYPTION		BT_OP(BT_OGF_LE, 0x0019)
+struct bt_hci_cp_le_start_encryption {
+	uint16_t handle;
+	uint64_t rand;
+	uint16_t ediv;
+	uint8_t  ltk[16];
+} __packed;
+
+#define BT_HCI_OP_LE_LTK_REQ_REPLY		BT_OP(BT_OGF_LE, 0x001a)
+struct bt_hci_cp_le_ltk_req_reply {
+	uint16_t handle;
+	uint8_t  ltk[16];
+} __packed;
+
+#define BT_HCI_OP_LE_LTK_REQ_NEG_REPLY		BT_OP(BT_OGF_LE, 0x001b)
+struct bt_hci_cp_le_ltk_req_neg_reply {
+	uint16_t handle;
+} __packed;
+
+#define BT_HCI_OP_LE_CONN_PARAM_REQ_REPLY	BT_OP(BT_OGF_LE, 0x0020)
+struct bt_hci_cp_le_conn_param_req_reply {
+	uint16_t handle;
+	uint16_t interval_min;
+	uint16_t interval_max;
+	uint16_t latency;
+	uint16_t timeout;
+	uint16_t min_ce_len;
+	uint16_t max_ce_len;
+} __packed;
+
+#define BT_HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY	BT_OP(BT_OGF_LE, 0x0021)
+struct bt_hci_cp_le_conn_param_req_neg_reply {
+	uint16_t handle;
+	uint8_t  reason;
+} __packed;
+
+#define BT_HCI_OP_LE_P256_PUBLIC_KEY		BT_OP(BT_OGF_LE, 0x0025)
+
+#define BT_HCI_OP_LE_GENERATE_DHKEY		BT_OP(BT_OGF_LE, 0x0026)
+struct bt_hci_cp_le_generate_dhkey {
+	uint8_t key[64];
+} __packed;
+
+/* Event definitions */
+
+#define BT_HCI_EVT_CONN_COMPLETE		0x03
+struct bt_hci_evt_conn_complete {
+	uint8_t   status;
+	uint16_t  handle;
+	bt_addr_t bdaddr;
+	uint8_t   link_type;
+	uint8_t   encr_enabled;
+} __packed;
+
+#define BT_HCI_EVT_CONN_REQUEST			0x04
+struct bt_hci_evt_conn_request {
+	bt_addr_t bdaddr;
+	uint8_t   dev_class[3];
+	uint8_t   link_type;
+} __packed;
+
+#define BT_HCI_EVT_DISCONN_COMPLETE		0x05
+struct bt_hci_evt_disconn_complete {
+	uint8_t  status;
+	uint16_t handle;
+	uint8_t  reason;
+} __packed;
+
+#define BT_HCI_EVT_ENCRYPT_CHANGE		0x08
+struct bt_hci_evt_encrypt_change {
+	uint8_t  status;
+	uint16_t handle;
+	uint8_t  encrypt;
+} __packed;
+
+#define BT_HCI_EVT_CMD_COMPLETE			0x0e
+struct hci_evt_cmd_complete {
+	uint8_t  ncmd;
+	uint16_t opcode;
+} __packed;
+
+#define BT_HCI_EVT_CMD_STATUS			0x0f
+struct bt_hci_evt_cmd_status {
+	uint8_t  status;
+	uint8_t  ncmd;
+	uint16_t opcode;
+} __packed;
+
+#define BT_HCI_EVT_NUM_COMPLETED_PACKETS	0x13
+struct bt_hci_evt_num_completed_packets {
+	uint8_t  num_handles;
+	struct bt_hci_handle_count h[0];
+} __packed;
+
+#define BT_HCI_EVT_PIN_CODE_REQ			0x16
+struct bt_hci_evt_pin_code_req {
+	bt_addr_t bdaddr;
+} __packed;
+
+#define BT_HCI_EVT_LINK_KEY_REQ			0x17
+struct bt_hci_evt_link_key_req {
+	bt_addr_t bdaddr;
+} __packed;
+
+/* Link Key types */
+#define BT_LK_COMBINATION			0x00
+#define BT_LK_LOCAL_UNIT			0x01
+#define BT_LK_REMOTE_UNIT			0x02
+#define BT_LK_DEBUG_COMBINATION			0x03
+#define BT_LK_UNAUTH_COMBINATION_P192		0x04
+#define BT_LK_AUTH_COMBINATION_P192		0x05
+#define BT_LK_CHANGED_COMBINATION		0x06
+#define BT_LK_UNAUTH_COMBINATION_P256		0x07
+#define BT_LK_AUTH_COMBINATION_P256		0x08
+
+#define BT_HCI_EVT_LINK_KEY_NOTIFY		0x18
+struct bt_hci_ev_link_key_notify {
+	bt_addr_t bdaddr;
+	uint8_t   link_key[16];
+	uint8_t   key_type;
+} __packed;
+
+#define BT_HCI_EVT_ENCRYPT_KEY_REFRESH_COMPLETE	0x30
+struct bt_hci_evt_encrypt_key_refresh_complete {
+	uint8_t  status;
+	uint16_t handle;
+} __packed;
+
+#define BT_HCI_EVT_IO_CAPA_REQ			0x31
+struct bt_hci_evt_io_capa_req {
+	bt_addr_t bdaddr;
+} __packed;
+
+#define BT_HCI_EVT_IO_CAPA_RESP			0x32
+struct bt_hci_evt_io_capa_resp {
+	bt_addr_t bdaddr;
+	uint8_t   capability;
+	uint8_t   oob_data;
+	uint8_t   authentication;
+} __packed;
+
+#define BT_HCI_EVT_SSP_COMPLETE			0x36
+struct bt_hci_evt_ssp_complete {
+	uint8_t   status;
+	bt_addr_t bdaddr;
+} __packed;
+
+#define BT_HCI_EVT_LE_META_EVENT		0x3e
+struct bt_hci_evt_le_meta_event {
+	uint8_t  subevent;
+} __packed;
+
+#define BT_HCI_ROLE_MASTER			0x00
+#define BT_HCI_ROLE_SLAVE			0x01
+
+#define BT_HCI_EVT_LE_CONN_COMPLETE		0x01
+struct bt_hci_evt_le_conn_complete {
+	uint8_t      status;
+	uint16_t     handle;
+	uint8_t      role;
+	bt_addr_le_t peer_addr;
+	uint16_t     interval;
+	uint16_t     latency;
+	uint16_t     supv_timeout;
+	uint8_t      clock_accuracy;
+} __packed;
+
+#define BT_HCI_EVT_LE_ADVERTISING_REPORT	0x02
+struct bt_hci_ev_le_advertising_info {
+	uint8_t      evt_type;
+	bt_addr_le_t addr;
+	uint8_t      length;
+	uint8_t      data[0];
+} __packed;
+
+#define BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE	0x03
+struct bt_hci_evt_le_conn_update_complete {
+	uint8_t  status;
+	uint16_t handle;
+	uint16_t interval;
+	uint16_t latency;
+	uint16_t supv_timeout;
+} __packed;
+
+#define BT_HCI_EV_LE_REMOTE_FEAT_COMPLETE	0x04
+struct bt_hci_ev_le_remote_feat_complete {
+	uint8_t  status;
+	uint16_t handle;
+	uint8_t  features[8];
+} __packed;
+
+#define BT_HCI_EVT_LE_LTK_REQUEST		0x05
+struct bt_hci_evt_le_ltk_request {
+	uint16_t handle;
+	uint64_t rand;
+	uint16_t ediv;
+} __packed;
+
+#define BT_HCI_EVT_LE_CONN_PARAM_REQ		0x06
+struct bt_hci_evt_le_conn_param_req {
+	uint16_t handle;
+	uint16_t interval_min;
+	uint16_t interval_max;
+	uint16_t latency;
+	uint16_t timeout;
+} __packed;
+
+#define BT_HCI_EVT_LE_P256_PUBLIC_KEY_COMPLETE	0x08
+struct bt_hci_evt_le_p256_public_key_complete {
+	uint8_t status;
+	uint8_t key[64];
+} __packed;
+
+#define BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE	0x09
+struct bt_hci_evt_le_generate_dhkey_complete {
+	uint8_t status;
+	uint8_t dhkey[32];
+} __packed;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BT_HCI_H */
diff --git a/system/libarc32_arduino101/drivers/bluetooth/uuid.h b/system/libarc32_arduino101/drivers/bluetooth/uuid.h
new file mode 100644
index 00000000..a54108ba
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/bluetooth/uuid.h
@@ -0,0 +1,463 @@
+/** @file
+ *  @brief Bluetooth UUID handling
+ */
+
+/*
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * 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.
+ */
+#ifndef __BT_UUID_H
+#define __BT_UUID_H
+
+#include <misc/util.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @brief Bluetooth UUID types */
+enum {
+	BT_UUID_TYPE_16,
+	BT_UUID_TYPE_128,
+};
+
+/** @brief This is a 'tentative' type and should be used as a pointer only */
+struct bt_uuid {
+	uint8_t type;
+};
+
+struct bt_uuid_16 {
+	struct bt_uuid uuid;
+	uint16_t val;
+};
+
+struct bt_uuid_128 {
+	struct bt_uuid uuid;
+	uint8_t val[16];
+};
+
+#define BT_UUID_INIT_16(value)		\
+{					\
+	.uuid.type = BT_UUID_TYPE_16,	\
+	.val = (value),			\
+}
+
+#define BT_UUID_INIT_128(value...)	\
+{					\
+	.uuid.type = BT_UUID_TYPE_128,	\
+	.val = { value },		\
+}
+
+#define BT_UUID_DECLARE_16(value) \
+	((struct bt_uuid *) (&(struct bt_uuid_16) BT_UUID_INIT_16(value)))
+#define BT_UUID_DECLARE_128(value...) \
+	((struct bt_uuid *) (&(struct bt_uuid_128) BT_UUID_INIT_128(value)))
+
+#define BT_UUID_16(__u) CONTAINER_OF(__u, struct bt_uuid_16, uuid)
+#define BT_UUID_128(__u) CONTAINER_OF(__u, struct bt_uuid_128, uuid)
+
+/** @def BT_UUID_GAP
+ *  @brief Generic Access
+ */
+#define BT_UUID_GAP				BT_UUID_DECLARE_16(0x1800)
+#define BT_UUID_GAP_VAL				0x1800
+/** @def BT_UUID_GATT
+ *  @brief Generic Attribute
+ */
+#define BT_UUID_GATT				BT_UUID_DECLARE_16(0x1801)
+#define BT_UUID_GATT_VAL			0x1801
+/** @def BT_UUID_CTS
+ *  @brief Current Time Service
+ */
+#define BT_UUID_CTS				BT_UUID_DECLARE_16(0x1805)
+#define BT_UUID_CTS_VAL				0x1805
+/** @def BT_UUID_DIS
+ *  @brief Device Information Service
+ */
+#define BT_UUID_DIS				BT_UUID_DECLARE_16(0x180a)
+#define BT_UUID_DIS_VAL				0x180a
+/** @def BT_UUID_HRS
+ *  @brief Heart Rate Service
+ */
+#define BT_UUID_HRS				BT_UUID_DECLARE_16(0x180d)
+#define BT_UUID_HRS_VAL				0x180d
+/** @def BT_UUID_BAS
+ *  @brief Battery Service
+ */
+#define BT_UUID_BAS				BT_UUID_DECLARE_16(0x180f)
+#define BT_UUID_BAS_VAL				0x180f
+/** @def BT_UUID_RSCS
+ *  @brief Running Speed and Cadence Service
+ */
+#define BT_UUID_RSCS				BT_UUID_DECLARE_16(0x1814)
+#define BT_UUID_RSCS_VAL			0x1814
+/** @def BT_UUID_CSC
+ *  @brief Cycling Speed and Cadence Service
+ */
+#define BT_UUID_CSC				BT_UUID_DECLARE_16(0x1816)
+#define BT_UUID_CSC_VAL				0x1816
+/** @def BT_UUID_ESS
+ *  @brief Environmental Sensing Service
+ */
+#define BT_UUID_ESS				BT_UUID_DECLARE_16(0x181a)
+#define BT_UUID_ESS_VAL				0x181a
+/** @def BT_UUID_IPSS
+ *  @brief IP Support Service
+ */
+#define BT_UUID_IPSS				BT_UUID_DECLARE_16(0x1820)
+#define BT_UUID_IPSS_VAL			0x1820
+/** @def BT_UUID_LNS
+ *  @brief Location and Navigation Support Service
+ */
+#define BT_UUID_LNS				BT_UUID_DECLARE_16(0x1819)
+#define BT_UUID_LNS_VAL				0x1819
+/** @def BT_UUID_GATT_PRIMARY
+ *  @brief GATT Primary Service
+ */
+#define BT_UUID_GATT_PRIMARY			BT_UUID_DECLARE_16(0x2800)
+#define BT_UUID_GATT_PRIMARY_VAL		0x2800
+/** @def BT_UUID_GATT_SECONDARY
+ *  @brief GATT Secondary Service
+ */
+#define BT_UUID_GATT_SECONDARY			BT_UUID_DECLARE_16(0x2801)
+#define BT_UUID_GATT_SECONDARY_VAL		0x2801
+/** @def BT_UUID_GATT_INCLUDE
+ *  @brief GATT Include Service
+ */
+#define BT_UUID_GATT_INCLUDE			BT_UUID_DECLARE_16(0x2802)
+#define BT_UUID_GATT_INCLUDE_VAL		0x2802
+/** @def BT_UUID_GATT_CHRC
+ *  @brief GATT Characteristic
+ */
+#define BT_UUID_GATT_CHRC			BT_UUID_DECLARE_16(0x2803)
+#define BT_UUID_GATT_CHRC_VAL			0x2803
+/** @def BT_UUID_GATT_CEP
+ *  @brief GATT Characteristic Extended Properties
+ */
+#define BT_UUID_GATT_CEP			BT_UUID_DECLARE_16(0x2900)
+#define BT_UUID_GATT_CEP_VAL			0x2900
+/** @def BT_UUID_GATT_CUD
+ *  @brief GATT Characteristic User Description
+ */
+#define BT_UUID_GATT_CUD			BT_UUID_DECLARE_16(0x2901)
+#define BT_UUID_GATT_CUD_VAL			0x2901
+/** @def BT_UUID_GATT_CCC
+ *  @brief GATT Client Characteristic Configuration
+ */
+#define BT_UUID_GATT_CCC			BT_UUID_DECLARE_16(0x2902)
+#define BT_UUID_GATT_CCC_VAL			0x2902
+/** @def BT_UUID_GATT_SCC
+ *  @brief GATT Server Characteristic Configuration
+ */
+#define BT_UUID_GATT_SCC			BT_UUID_DECLARE_16(0x2903)
+#define BT_UUID_GATT_SCC_VAL			0x2903
+/** @def BT_UUID_GATT_CPF
+ *  @brief GATT Characteristic Presentation Format
+ */
+#define BT_UUID_GATT_CPF			BT_UUID_DECLARE_16(0x2904)
+#define BT_UUID_GATT_CPF_VAL			0x2904
+/** @def BT_UUID_VALID_RANGE
+ *  @brief Valid Range Descriptor
+ */
+#define BT_UUID_VALID_RANGE			BT_UUID_DECLARE_16(0x2906)
+#define BT_UUID_VALID_RANGE_VAL			0x2906
+/** @def BT_UUID_ES_CONFIGURATION
+ *  @brief Environmental Sensing Configuration Descriptor
+ */
+#define BT_UUID_ES_CONFIGURATION		BT_UUID_DECLARE_16(0x290b)
+#define BT_UUID_ES_CONFIGURATION_VAL		0x290b
+/** @def BT_UUID_ES_MEASUREMENT
+ *  @brief Environmental Sensing Measurement Descriptor
+ */
+#define BT_UUID_ES_MEASUREMENT			BT_UUID_DECLARE_16(0x290c)
+#define BT_UUID_ES_MEASUREMENT_VAL		0x290c
+/** @def BT_UUID_ES_TRIGGER_SETTING
+ *  @brief Environmental Sensing Trigger Setting Descriptor
+ */
+#define BT_UUID_ES_TRIGGER_SETTING		BT_UUID_DECLARE_16(0x290d)
+#define BT_UUID_ES_TRIGGER_SETTING_VAL		0x290d
+/** @def BT_UUID_GAP_DEVICE_NAME
+ *  @brief GAP Characteristic Device Name
+ */
+#define BT_UUID_GAP_DEVICE_NAME			BT_UUID_DECLARE_16(0x2a00)
+#define BT_UUID_GAP_DEVICE_NAME_VAL		0x2a00
+/** @def BT_UUID_GAP_APPEARANCE
+ *  @brief GAP Characteristic Appearance
+ */
+#define BT_UUID_GAP_APPEARANCE			BT_UUID_DECLARE_16(0x2a01)
+#define BT_UUID_GAP_APPEARANCE_VAL		0x2a01
+/** @def BT_UUID_GAP_PPCP
+ *  @brief GAP Characteristic Peripheral Preferred Connection Parameters
+ */
+#define BT_UUID_GAP_PPCP			BT_UUID_DECLARE_16(0x2a04)
+#define BT_UUID_GAP_PPCP_VAL			0x2a04
+/** @def BT_UUID_BAS_BATTERY_LEVEL
+ *  @brief BAS Characteristic Battery Level
+ */
+#define BT_UUID_BAS_BATTERY_LEVEL		BT_UUID_DECLARE_16(0x2a19)
+#define BT_UUID_BAS_BATTERY_LEVEL_VAL		0x2a19
+/** @def BT_UUID_DIS_SYSTEM_ID
+ *  @brief DIS Characteristic System ID
+ */
+#define BT_UUID_DIS_SYSTEM_ID			BT_UUID_DECLARE_16(0x2a23)
+#define BT_UUID_DIS_SYSTEM_ID_VAL		0x2a23
+/** @def BT_UUID_DIS_MODEL_NUMBER
+ *  @brief DIS Characteristic Model Number String
+ */
+#define BT_UUID_DIS_MODEL_NUMBER		BT_UUID_DECLARE_16(0x2a24)
+#define BT_UUID_DIS_MODEL_NUMBER_VAL		0x2a24
+/** @def BT_UUID_DIS_SERIAL_NUMBER
+ *  @brief DIS Characteristic Serial Number String
+ */
+#define BT_UUID_DIS_SERIAL_NUMBER		BT_UUID_DECLARE_16(0x2a25)
+#define BT_UUID_DIS_SERIAL_NUMBER_VAL		0x2a25
+/** @def BT_UUID_DIS_FIRMWARE_REVISION
+ *  @brief DIS Characteristic Firmware Revision String
+ */
+#define BT_UUID_DIS_FIRMWARE_REVISION		BT_UUID_DECLARE_16(0x2a26)
+#define BT_UUID_DIS_FIRMWARE_REVISION_VAL	0x2a26
+/** @def BT_UUID_DIS_HARDWARE_REVISION
+ *  @brief DIS Characteristic Hardware Revision String
+ */
+#define BT_UUID_DIS_HARDWARE_REVISION		BT_UUID_DECLARE_16(0x2a27)
+#define BT_UUID_DIS_HARDWARE_REVISION_VAL	0x2a27
+/** @def BT_UUID_DIS_SOFTWARE_REVISION
+ *  @brief DIS Characteristic Software Revision String
+ */
+#define BT_UUID_DIS_SOFTWARE_REVISION		BT_UUID_DECLARE_16(0x2a28)
+#define BT_UUID_DIS_SOFTWARE_REVISION_VAL	0x2a28
+/** @def BT_UUID_DIS_MANUFACTURER_NAME
+ *  @brief DIS Characteristic Manufacturer Name String
+ */
+#define BT_UUID_DIS_MANUFACTURER_NAME		BT_UUID_DECLARE_16(0x2a29)
+#define BT_UUID_DIS_MANUFACTURER_NAME_VAL	0x2a29
+/** @def BT_UUID_DIS_PNP_ID
+ *  @brief DIS Characteristic PnP ID
+ */
+#define BT_UUID_DIS_PNP_ID			BT_UUID_DECLARE_16(0x2a50)
+#define BT_UUID_DIS_PNP_ID_VAL			0x2a50
+/** @def BT_UUID_RSC_MEASUREMENT
+ *  @brief RSC Characteristic measurement ID
+ */
+#define BT_UUID_RSC_MEASUREMENT			BT_UUID_DECLARE_16(0x2a53)
+#define BT_UUID_RSC_MEASUREMENT_VAL		0x2a53
+/** @def BT_UUID_RSC_FEATURE
+ *  @brief RSC Characteristic feature ID
+ */
+#define BT_UUID_RSC_FEATURE			BT_UUID_DECLARE_16(0x2a54)
+#define BT_UUID_RSC_FEATURE_VAL			0x2a54
+/** @def BT_UUID_CTS_CURRENT_TIME
+ *  @brief CTS Characteristic Current Time
+ */
+#define BT_UUID_CTS_CURRENT_TIME		BT_UUID_DECLARE_16(0x2a2b)
+#define BT_UUID_CTS_CURRENT_TIME_VAL		0x2a2b
+/** @def BT_UUID_MAGN_DECLINATION
+ *  @brief Magnetic Declination Characteristic
+ */
+#define BT_UUID_MAGN_DECLINATION		BT_UUID_DECLARE_16(0x2a2c)
+#define BT_UUID_MAGN_DECLINATION_VAL		0x2a2c
+/** @def BT_UUID_HRS_MEASUREMENT
+ *  @brief HRS Characteristic Measurement Interval
+ */
+#define BT_UUID_HRS_MEASUREMENT			BT_UUID_DECLARE_16(0x2a37)
+#define BT_UUID_HRS_MEASUREMENT_VAL		0x2a37
+/** @def BT_UUID_HRS_BODY_SENSOR
+ *  @brief HRS Characteristic Body Sensor Location
+ */
+#define BT_UUID_HRS_BODY_SENSOR			BT_UUID_DECLARE_16(0x2a38)
+#define BT_UUID_HRS_BODY_SENSOR_VAL		0x2a38
+/** @def BT_UUID_HRS_CONTROL_POINT
+ *  @brief HRS Characteristic Control Point
+ */
+#define BT_UUID_HRS_CONTROL_POINT		BT_UUID_DECLARE_16(0x2a39)
+#define BT_UUID_HRS_CONTROL_POINT_VAL		0x2a39
+/** @def BT_UUID_CSC_MEASUREMENT
+ *  @brief CSC Measurement Characteristic
+ */
+#define BT_UUID_CSC_MEASUREMENT			BT_UUID_DECLARE_16(0x2a5b)
+#define BT_UUID_CSC_MEASUREMENT_VAL		0x2a5b
+/** @def BT_UUID_CSC_FEATURE
+ *  @brief CSC Feature Characteristic
+ */
+#define BT_UUID_CSC_FEATURE			BT_UUID_DECLARE_16(0x2a5c)
+#define BT_UUID_CSC_FEATURE_VAL			0x2a5c
+/** @def BT_UUID_SENSOR_LOCATION
+ *  @brief Sensor Location Characteristic
+ */
+#define BT_UUID_SENSOR_LOCATION			BT_UUID_DECLARE_16(0x2a5d)
+#define BT_UUID_SENSOR_LOCATION_VAL		0x2a5d
+/** @def BT_UUID_SC_CONTROL_POINT
+ *  @brief SC Control Point Characteristic
+ */
+#define BT_UUID_SC_CONTROL_POINT		BT_UUID_DECLARE_16(0x2a55)
+#define BT_UUID_SC_CONTROL_POINT_VAl		0x2a55
+/** @def BT_UUID_LNS_CONTROL_POINT
+ *  @brief LNS Control Point Characteristic
+ */
+#define BT_UUID_LNS_CONTROL_POINT		BT_UUID_DECLARE_16(0x2a6B)
+#define BT_UUID_LNS_CONTROL_POINT_VAL		0x2a6B
+/** @def BT_UUID_LNS_LOCATION_SPEED
+ *  @brief LNS Characteristic Location and Speed
+ */
+#define BT_UUID_LNS_LOCATION_SPEED		BT_UUID_DECLARE_16(0x2a67)
+#define BT_UUID_LNS_LOCATION_SPEED_VAL		0x2a67
+/** @def BT_UUID_LNS_FEATURE
+ *  @brief LNS Characteristic Feature
+ */
+#define BT_UUID_LNS_FEATURE			BT_UUID_DECLARE_16(0x2a6a)
+#define BT_UUID_LNS_FEATURE_VAL			0x2a6a
+/** @def BT_UUID_ELEVATION
+ *  @brief Elevation Characteristic
+ */
+#define BT_UUID_ELEVATION			BT_UUID_DECLARE_16(0x2a6c)
+#define BT_UUID_ELEVATION_VAL			0x2a6c
+/** @def BT_UUID_PRESSURE
+ *  @brief Pressure Characteristic
+ */
+#define BT_UUID_PRESSURE			BT_UUID_DECLARE_16(0x2a6d)
+#define BT_UUID_PRESSURE_VAL			0x2a6d
+/** @def BT_UUID_TEMPERATURE
+ *  @brief Temperature Characteristic
+ */
+#define BT_UUID_TEMPERATURE			BT_UUID_DECLARE_16(0x2a6e)
+#define BT_UUID_TEMPERATURE_VAL			0x2a6e
+/** @def BT_UUID_HUMIDITY
+ *  @brief Humidity Characteristic
+ */
+#define BT_UUID_HUMIDITY			BT_UUID_DECLARE_16(0x2a6f)
+#define BT_UUID_HUMIDITY_VAL			0x2a6f
+/** @def BT_UUID_TRUE_WIND_SPEED
+ *  @brief True Wind Speed Characteristic
+ */
+#define BT_UUID_TRUE_WIND_SPEED			BT_UUID_DECLARE_16(0x2a70)
+#define BT_UUID_TRUE_WIND_SPEED_VAL		0x2a70
+/** @def BT_UUID_TRUE_WIND_DIR
+ *  @brief True Wind Direction Characteristic
+ */
+#define BT_UUID_TRUE_WIND_DIR			BT_UUID_DECLARE_16(0x2a71)
+#define BT_UUID_TRUE_WIND_DIR_VAL		0x2a71
+/** @def BT_UUID_APPARENT_WIND_SPEED
+ *  @brief Apparent Wind Speed Characteristic
+ */
+#define BT_UUID_APPARENT_WIND_SPEED		BT_UUID_DECLARE_16(0x2a72)
+#define BT_UUID_APPARENT_WIND_SPEED_VAL		0x2a72
+/** @def BT_UUID_APPARENT_WIND_DIR
+ *  @brief Apparent Wind Direction Characteristic
+ */
+#define BT_UUID_APPARENT_WIND_DIR		BT_UUID_DECLARE_16(0x2a73)
+#define BT_UUID_APPARENT_WIND_DIR_VAL		0x2a73
+/** @def BT_UUID_GUST_FACTOR
+ *  @brief Gust Factor Characteristic
+ */
+#define BT_UUID_GUST_FACTOR			BT_UUID_DECLARE_16(0x2a74)
+#define BT_UUID_GUST_FACTOR_VAL			0x2a74
+/** @def BT_UUID_POLLEN_CONCENTRATION
+ *  @brief Pollen Concentration Characteristic
+ */
+#define BT_UUID_POLLEN_CONCENTRATION		BT_UUID_DECLARE_16(0x2a75)
+#define BT_UUID_POLLEN_CONCENTRATION_VAL	0x2a75
+/** @def BT_UUID_UV_INDEX
+ *  @brief UV Index Characteristic
+ */
+#define BT_UUID_UV_INDEX			BT_UUID_DECLARE_16(0x2a76)
+#define BT_UUID_UV_INDEX_VAL			0x2a76
+/** @def BT_UUID_IRRADIANCE
+ *  @brief Irradiance Characteristic
+ */
+#define BT_UUID_IRRADIANCE			BT_UUID_DECLARE_16(0x2a77)
+#define BT_UUID_IRRADIANCE_VAL			0x2a77
+/** @def BT_UUID_RAINFALL
+ *  @brief Rainfall Characteristic
+ */
+#define BT_UUID_RAINFALL			BT_UUID_DECLARE_16(0x2a78)
+#define BT_UUID_RAINFALL_VAL			0x2a78
+/** @def BT_UUID_WIND_CHILL
+ *  @brief Wind Chill Characteristic
+ */
+#define BT_UUID_WIND_CHILL			BT_UUID_DECLARE_16(0x2a79)
+#define BT_UUID_WIND_CHILL_VAL			0x2a79
+/** @def BT_UUID_HEAT_INDEX
+ *  @brief Heat Index Characteristic
+ */
+#define BT_UUID_HEAT_INDEX			BT_UUID_DECLARE_16(0x2a7a)
+#define BT_UUID_HEAT_INDEX_VAL			0x2a7a
+/** @def BT_UUID_DEW_POINT
+ *  @brief Dew Point Characteristic
+ */
+#define BT_UUID_DEW_POINT			BT_UUID_DECLARE_16(0x2a7b)
+#define BT_UUID_DEW_POINT_VAL			0x2a7b
+/** @def BT_UUID_DESC_VALUE_CHANGED
+ *  @brief Descriptor Value Changed Characteristic
+ */
+#define BT_UUID_DESC_VALUE_CHANGED		BT_UUID_DECLARE_16(0x2a7d)
+#define BT_UUID_DESC_VALUE_CHANGED_VAL		0x2a7d
+/** @def BT_UUID_MAGN_FLUX_DENSITY_2D
+ *  @brief Magnetic Flux Density - 2D Characteristic
+ */
+#define BT_UUID_MAGN_FLUX_DENSITY_2D		BT_UUID_DECLARE_16(0x2aa0)
+#define BT_UUID_MAGN_FLUX_DENSITY_2D_VAL	0x2aa0
+/** @def BT_UUID_MAGN_FLUX_DENSITY_3D
+ *  @brief Magnetic Flux Density - 3D Characteristic
+ */
+#define BT_UUID_MAGN_FLUX_DENSITY_3D		BT_UUID_DECLARE_16(0x2aa1)
+#define BT_UUID_MAGN_FLUX_DENSITY_3D_VAL	0x2aa1
+/** @def BT_UUID_BAR_PRESSURE_TREND
+ *  @brief Barometric Pressure Trend Characteristic
+ */
+#define BT_UUID_BAR_PRESSURE_TREND		BT_UUID_DECLARE_16(0x2aa3)
+#define BT_UUID_BAR_PRESSURE_TREND_VAL		0x2aa3
+
+/** @brief Compare Bluetooth UUIDs.
+ *
+ *  Compares 2 Bluetooth UUIDs, if the types are different both UUIDs are
+ *  first converted to 128 bits format before comparing.
+ *
+ *  @param u1 First Bluetooth UUID to compare
+ *  @param u2 Second Bluetooth UUID to compare
+ *
+ *  @return negative value if @a u1 < @a u2, 0 if @a u1 == @a u2, else positive
+ */
+int bt_uuid_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2);
+
+#if defined(CONFIG_BLUETOOTH_DEBUG)
+/** @brief Convert Bluetooth UUID to string.
+ *
+ *  Converts Bluetooth UUID to string. UUID has to be in 16 bits or 128 bits
+ *  format.
+ *
+ *  @param uuid Bluetooth UUID
+ *  @param str pointer where to put converted string
+ *  @param len length of str
+ *
+ *  @return N/A
+ */
+void bt_uuid_to_str(const struct bt_uuid *uuid, char *str, size_t len);
+
+/** @brief Convert Bluetooth UUID to string in place.
+ *
+ *  Converts Bluetooth UUID to string in place. UUID has to be in 16 bits or
+ *  128 bits format.
+ *
+ *  @param uuid Bluetooth UUID
+ *
+ *  @return String representation of the UUID given
+ */
+const char *bt_uuid_str(const struct bt_uuid *uuid);
+#endif /* CONFIG_BLUETOOTH_DEBUG */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BT_UUID_H */
diff --git a/system/libarc32_arduino101/drivers/ipc_uart_ns16550.c b/system/libarc32_arduino101/drivers/ipc_uart_ns16550.c
index a497b6cf..accd8b2c 100644
--- a/system/libarc32_arduino101/drivers/ipc_uart_ns16550.c
+++ b/system/libarc32_arduino101/drivers/ipc_uart_ns16550.c
@@ -49,14 +49,38 @@
 
 #define IPC_UART_HDR_REQUEST_LEN (IPC_HEADER_LEN+sizeof(uint32_t)) /* ipc header + request len */
 
-struct ipc_uart_channels channels[IPC_UART_MAX_CHANNEL] = { {0, 0, NULL},};
-static uint16_t send_counter = 0;
-static uint16_t received_counter = 0;
-static uint8_t * ipc_uart_tx = NULL;
-static uint8_t * ipc_uart_rx = NULL;
-static uint8_t ipc_uart_tx_state = 0;
+enum {
+	STATUS_TX_IDLE = 0,
+	STATUS_TX_BUSY,
+	STATUS_TX_DONE,
+};
+
+enum {
+	STATUS_RX_IDLE = 0,
+	STATUS_RX_HDR,
+	STATUS_RX_DATA
+};
+
+struct ipc_uart {
+	uint8_t *tx_data;
+	uint8_t *rx_ptr;
+	struct ipc_uart_channels channels[IPC_UART_MAX_CHANNEL];
+	struct ipc_uart_header tx_hdr;
+	struct ipc_uart_header rx_hdr;
+	uint16_t send_counter;
+	uint16_t rx_size;
+	uint8_t tx_state;
+	uint8_t rx_state;
+	uint8_t uart_enabled;
+	/* protect against multiple wakelock and wake assert calls */
+	uint8_t tx_wakelock_acquired;
+	/* TODO: remove once IRQ will take a parameter */
+	//struct td_device *device;
+	void (*tx_cb)(bool wake_state, void *); /*!< Callback to be called to set wake state when TX is starting or ending */
+	void *tx_cb_param;              /*!< tx_cb function parameter */
+};
 
-static void * ble_cfw_channel;
+static struct ipc_uart ipc = {};
 
 static const struct uart_init_info uart_dev_info[] = {
 	{
@@ -79,187 +103,246 @@ static const struct uart_init_info uart_dev_info[] = {
 	},
 };
 
-void uart_ipc_close_channel(int channel_id)
+void ipc_uart_close_channel(int channel_id)
 {
-	channels[channel_id].state = IPC_CHANNEL_STATE_CLOSED;
-	channels[channel_id].cb = NULL;
-	channels[channel_id].index = channel_id;
+	ipc.channels[channel_id].state = IPC_CHANNEL_STATE_CLOSED;
+	ipc.channels[channel_id].cb = NULL;
+	ipc.channels[channel_id].index = channel_id;
+
+	ipc.uart_enabled = 0;
+	ipc.tx_wakelock_acquired = 0;
 }
 
-void uart_ipc_disable(int num)
+void ipc_uart_ns16550_disable(int num)
 {
 	int i;
 	for (i = 0; i < IPC_UART_MAX_CHANNEL; i++)
-		uart_ipc_close_channel(i);
+		ipc_uart_close_channel(i);
+	if (ipc.tx_cb)
+		ipc.tx_cb(0, ipc.tx_cb_param);
+    
 	UART_IRQ_TX_DISABLE(num);
 	UART_IRQ_RX_DISABLE(num);
 }
 
-void uart_ipc_init(int num)
+void ipc_uart_init(int num)
 {
 	int i;
 	(void)num;
-	uint8_t c;
 
 	for (i = 0; i < IPC_UART_MAX_CHANNEL; i++)
-		uart_ipc_close_channel(i);
+		ipc_uart_close_channel(i);
 
-	pr_info(LOG_MODULE_IPC, "uart_ipc_init(nr: %d), baudrate %d, options:"
-			"0x%x, irq: %d",IPC_UART,
+	pr_info(LOG_MODULE_IPC, "%s(nr: %d), baudrate %d, options:"
+			"0x%x, irq: %d",__FUNCTION__, IPC_UART,
 			uart_dev_info[IPC_UART].baud_rate,
 			uart_dev_info[IPC_UART].options,
 			uart_dev_info[IPC_UART].irq);
 	uart_init(IPC_UART, &uart_dev_info[IPC_UART]);
-	/* Drain RX FIFOs (no need to disable IRQ at this stage) */
-	while (uart_poll_in(IPC_UART, &c) != -1);
-	uart_int_connect(IPC_UART, uart_ipc_isr, NULL, NULL);
-
-	UART_IRQ_RX_ENABLE(IPC_UART);
-}
-
-void uart_ipc_set_channel(void * ipc_channel)
-{
-	ble_cfw_channel = ipc_channel;
+	
+	ipc.uart_enabled = 0;
+	ipc.tx_wakelock_acquired = 0;
+
+	/* Initialize the reception pointer */
+	ipc.rx_size = sizeof(ipc.rx_hdr);
+	ipc.rx_ptr = (uint8_t *)&ipc.rx_hdr;
+	ipc.rx_state = STATUS_RX_IDLE;
 }
 
-void * uart_ipc_get_channel(void)
+static void ipc_uart_push_frame(uint16_t len, uint8_t *p_data)
 {
-	return ble_cfw_channel;
-}
-
-void uart_ipc_push_frame(void) {
-	void * frame;
-	OS_ERR_TYPE error = E_OS_OK;
-
-	if (NULL == ipc_uart_rx)
-		return;
-	int len = IPC_FRAME_GET_LEN(ipc_uart_rx);
-	int channel = IPC_FRAME_GET_CHANNEL(ipc_uart_rx);
-	uint8_t cpu_id = IPC_FRAME_GET_SRC(ipc_uart_rx);
-
-	pr_debug(LOG_MODULE_IPC, "%s: received frame: len %d, channel %d, src "
-			"%d", __func__, len, channel, cpu_id);
-
-	if (channels[channel].cb != NULL) {
-		frame = balloc(len, &error);
-		if (error != E_OS_OK) {
-			pr_error(LOG_MODULE_IPC, "NO MEM: error: %d size: %d",
-					error, len);
-		} else {
-			memcpy(frame, &ipc_uart_rx[IPC_HEADER_LEN], len);
-
-			channels[channel].cb(cpu_id, channel, len, frame);
-		}
+	//pr_debug(LOG_MODULE_IPC, "push_frame: received:frame len: %d, p_data: "
+	//	 "len %d, src %d, channel %d", ipc.rx_hdr.len, len,
+	//	 ipc.rx_hdr.src_cpu_id,
+	//	 ipc.rx_hdr.channel);
+    //pr_debug(LOG_MODULE_IPC,"data[0 - 1]: %x-%x", p_data[0], p_data[1]);
+
+	if ((ipc.rx_hdr.channel < IPC_UART_MAX_CHANNEL) &&
+	    (ipc.channels[ipc.rx_hdr.channel].cb != NULL)) {
+		ipc.channels[ipc.rx_hdr.channel].cb(ipc.rx_hdr.channel,
+						    IPC_MSG_TYPE_MESSAGE,
+						    len,
+						    p_data);
+	} else {
+		bfree(p_data);
+		pr_error(LOG_MODULE_IPC, "uart_ipc: bad channel %d",
+			 ipc.rx_hdr.channel);
 	}
-	if (ipc_uart_rx)
-		bfree(ipc_uart_rx);
-	ipc_uart_rx = NULL;
 }
 
-void uart_ipc_isr()
+void ipc_uart_isr()
 {
-	uint8_t *p_rx;
-	uint8_t *p_tx;
-
-	while (UART_IRQ_HW_UPDATE(IPC_UART) && UART_IRQ_IS_PENDING(IPC_UART)) {
-		if (UART_IRQ_ERR_DETECTED(IPC_UART)) {
-			uint8_t c;
-			if (UART_BREAK_CHECK(IPC_UART)){
-				panic();
-			}
-			UART_POLL_IN(IPC_UART, &c);
-		} else if (UART_IRQ_RX_READY(IPC_UART)) {
-			int received;
-			if (received_counter < 2) {
-				if (NULL == ipc_uart_rx)
-					ipc_uart_rx =
-						balloc(IPC_UART_MAX_PAYLOAD, NULL);
-				p_rx = ipc_uart_rx;
-				received = UART_FIFO_READ(IPC_UART,
-						&p_rx[received_counter],
-						1);
-				received_counter += received;
-			} else {
-				p_rx = ipc_uart_rx;
-				received = UART_FIFO_READ(IPC_UART,
-						&p_rx[received_counter],
-						IPC_FRAME_GET_LEN(p_rx) +
-						IPC_HEADER_LEN
-						- received_counter);
-				received_counter += received;
-				if (received_counter == IPC_FRAME_GET_LEN(p_rx)
-						+ IPC_HEADER_LEN) {
+    /* TODO: remove once IRQ supports parameter */
+    uint8_t *p_tx;
+
+    while (UART_IRQ_HW_UPDATE(IPC_UART) && 
+           UART_IRQ_IS_PENDING(IPC_UART)) {
+        if (UART_IRQ_ERR_DETECTED(IPC_UART))
+        {
+            uint8_t c;
+            if (UART_BREAK_CHECK(IPC_UART)) {
+                panic(-1);
+            }
+            UART_POLL_IN(IPC_UART, &c);
+        }
+        if (UART_IRQ_RX_READY(IPC_UART)) {
+            int rx_cnt;
+
+            while ((rx_cnt =
+                    UART_FIFO_READ(IPC_UART,
+                               ipc.rx_ptr,
+                               ipc.rx_size)) != 0)
+            {
+                if ((ipc.uart_enabled) &&
+                    (ipc.rx_state == STATUS_RX_IDLE)) {
+                    /* acquire wakelock until frame is fully received */
+                    //pm_wakelock_acquire(&info->rx_wl);
+                    ipc.rx_state = STATUS_RX_HDR;
+                }
+
+                /* Until UART has enabled at least one channel, data should be discarded */
+                if (ipc.uart_enabled) {
+                    ipc.rx_size -= rx_cnt;
+                    ipc.rx_ptr += rx_cnt;
+                }
+
+                if (ipc.rx_size == 0) {
+                    if (ipc.rx_state == STATUS_RX_HDR) {
+    //pr_error(0, "%s-%d", __FUNCTION__, ipc.rx_hdr.len);
+                        ipc.rx_ptr = balloc(
+                            ipc.rx_hdr.len, NULL);
+                        
+                            //pr_debug(
+                            //  LOG_MODULE_IPC,
+                            //  "ipc_uart_isr: rx_ptr is %p",
+                            //  ipc.rx_ptr);
+                        ipc.rx_size = ipc.rx_hdr.len;
+                        ipc.rx_state = STATUS_RX_DATA;
+                    } else {
 #ifdef IPC_UART_DBG_RX
-					for(int i = 0; i < received_counter; i++) {
-						pr_debug(LOG_MODULE_IPC, "%s: %d byte is %d", __func__, i, p_rx[i]);
-					}
+                        uint8_t *p_rx = ipc.rx_ptr -
+                                ipc.rx_hdr.len;
+                        for (int i = 0;
+                             i < ipc.rx_hdr.len;
+                             i++) {
+                            pr_debug(
+                                LOG_MODULE_IPC,
+                                "ipc_uart_isr: %d byte is %d",
+                                i, p_rx[i]);
+                        }
 #endif
-					received_counter = 0;
-					uart_ipc_push_frame();
-				}
-			}
-		} else if (UART_IRQ_TX_READY(IPC_UART)) {
-			int transmitted;
-			if (ipc_uart_tx_state == STATUS_TX_IDLE) {
-				uint8_t lsr = UART_LINE_STATUS(IPC_UART);
-				UART_IRQ_TX_DISABLE(IPC_UART);
-
-				pr_debug(LOG_MODULE_IPC, "ipc_isr_tx: disable TXint, LSR: 0x%2x\n",
-						lsr);
-				/* wait for FIFO AND THR being empty! */
-				while ((lsr & BOTH_EMPTY) != BOTH_EMPTY) {
-					lsr = UART_LINE_STATUS(IPC_UART);
-				}
-				return;
-			}
-			if(NULL == ipc_uart_tx){
-				pr_warning(LOG_MODULE_IPC, "%s: Bad Tx data",__func__);
-				return;
-			}
-			p_tx = ipc_uart_tx;
-			transmitted = UART_FIFO_FILL(IPC_UART, &p_tx[send_counter],
-					IPC_FRAME_GET_LEN(p_tx) +
-					IPC_HEADER_LEN - send_counter);
-			send_counter += transmitted;
-			if (send_counter == IPC_FRAME_GET_LEN(p_tx) +
-					IPC_HEADER_LEN) {
-				send_counter = 0;
+
+                        ipc_uart_push_frame(
+                            ipc.rx_hdr.len,
+                            ipc.rx_ptr -
+                            ipc.rx_hdr.len);
+                        ipc.rx_size = sizeof(ipc.rx_hdr);
+                        ipc.rx_ptr =
+                            (uint8_t *)&ipc.rx_hdr;
+                        ipc.rx_state = STATUS_RX_IDLE;
+                    }
+                }
+            }
+        } 
+        if (UART_IRQ_TX_READY(IPC_UART)) {
+            int tx_len;
+
+            if (ipc.tx_state == STATUS_TX_DONE) {
+                uint8_t lsr = UART_LINE_STATUS(IPC_UART);
+                ipc.tx_state = STATUS_TX_IDLE;
+                UART_IRQ_TX_DISABLE(IPC_UART);
+                
+                /* wait for FIFO AND THR being empty! */
+                while ((lsr & BOTH_EMPTY) != BOTH_EMPTY) {
+                    lsr = UART_LINE_STATUS(IPC_UART);
+                }
+
+                /* No more TX activity, send event and release wakelock */
+                if (ipc.tx_cb) {
+                    ipc.tx_cb(0, ipc.tx_cb_param);
+                }
+                //pm_wakelock_release(&info->tx_wl);
+                ipc.tx_wakelock_acquired = 0;
+                return;
+            }
+            if (NULL == ipc.tx_data) {
+                pr_warning(LOG_MODULE_IPC,
+                       "ipc_uart_isr: Bad Tx data");
+                return;
+            }
+
+            if (!ipc.tx_wakelock_acquired) {
+                ipc.tx_wakelock_acquired = 1;
+                /* Starting TX activity, send wake assert event and acquire wakelock */
+                if (ipc.tx_cb) {
+                    ipc.tx_cb(1, ipc.tx_cb_param);
+                }
+                //pm_wakelock_acquire(&info->tx_wl);
+            }
+            if (ipc.send_counter < sizeof(ipc.tx_hdr)) {
+                p_tx = (uint8_t *)&ipc.tx_hdr +
+                       ipc.send_counter;
+                tx_len = sizeof(ipc.tx_hdr) - ipc.send_counter;
+            } else {
+                p_tx = ipc.tx_data +
+                       (ipc.send_counter - sizeof(ipc.tx_hdr));
+                tx_len = ipc.tx_hdr.len -
+                     (ipc.send_counter - sizeof(ipc.tx_hdr));
+            }
+            ipc.send_counter += UART_FIFO_FILL(IPC_UART, 
+                                               p_tx,
+                                               tx_len);
+
+            if (ipc.send_counter ==
+                (ipc.tx_hdr.len + sizeof(ipc.tx_hdr))) {
+                ipc.send_counter = 0;
 #ifdef IPC_UART_DBG_TX
-				pr_debug(LOG_MODULE_IPC, "%s: sent IPC FRAME "
-						"len %d", __func__,
-						IPC_FRAME_GET_LEN(p_tx));
-				for (int i = 0; i < send_counter; i++) {
-					pr_debug(LOG_MODULE_IPC, "%s: %d  sent "
-							"byte is %d",
-							__func__, i, p_tx[i]);
-				}
+                pr_debug(
+                    LOG_MODULE_IPC,
+                    "ipc_uart_isr: sent IPC FRAME "
+                    "len %d", ipc.tx_hdr.len);
 #endif
-				bfree(ipc_uart_tx);
-				ipc_uart_tx = NULL;
 
-				ipc_uart_tx_state = STATUS_TX_IDLE;
+                p_tx = ipc.tx_data;
+                ipc.tx_data = NULL;
+                ipc.tx_state = STATUS_TX_DONE;
+
+                /* free sent message and pull send next frame one in the queue */
+                if (ipc.channels[ipc.tx_hdr.channel].cb)
+                {
+                    ipc.channels[ipc.tx_hdr.channel].cb(
+                        ipc.tx_hdr.channel,
+                        IPC_MSG_TYPE_FREE,
+                        ipc.tx_hdr.len,
+                        p_tx);
+                }
+                else
+                {
+                    bfree(p_tx);
+                }
+                
 #ifdef IPC_UART_DBG_TX
-				uint8_t lsr = UART_LINE_STATUS(IPC_UART);
-				pr_info(LOG_MODULE_IPC, "ipc_isr_tx: tx_idle LSR: 0x%2x\n",
-						lsr);
+                uint8_t lsr = UART_LINE_STATUS(IPC_UART);//(info->uart_num);
+                pr_debug(LOG_MODULE_IPC,
+                     "ipc_isr_tx: tx_idle LSR: 0x%2x\n",
+                     lsr);
 #endif
-			}
-		} else {
-			pr_warning(LOG_MODULE_IPC, "%s: Unknown ISR src",
-					__func__);
-		}
-	}
+            }
+        }
+        
+    }
 }
 
-void *uart_ipc_channel_open(int channel_id,
-			void (*cb) (uint8_t, int, int, void *))
+void *ipc_uart_channel_open(int channel_id,
+			    int (*cb)(int, int, int, void *))
 {
 	struct ipc_uart_channels *chan;
+	uint8_t c;
 
-	if (channel_id > IPC_UART_MAX_CHANNEL - 1)
+	if (channel_id > (IPC_UART_MAX_CHANNEL - 1))
 		return NULL;
 
-	chan = &channels[channel_id];
+	chan = &ipc.channels[channel_id];
 
 	if (chan->state != IPC_CHANNEL_STATE_CLOSED)
 		return NULL;
@@ -267,74 +350,48 @@ void *uart_ipc_channel_open(int channel_id,
 	chan->state = IPC_CHANNEL_STATE_OPEN;
 	chan->cb = cb;
 
+	ipc.uart_enabled = 1;
+	ipc.tx_wakelock_acquired = 0;
+	
+    pr_debug(LOG_MODULE_IPC, "%s: open chan success", __FUNCTION__);
+    
+	/* Drain RX FIFOs (no need to disable IRQ at this stage) */
+	while (uart_poll_in(IPC_UART, &c) != -1);
+	uart_int_connect(IPC_UART, ipc_uart_isr, NULL, NULL);
+	
+	UART_IRQ_RX_ENABLE(IPC_UART);
+	
 	return chan;
 }
 
-int uart_ipc_send_message(void *handle, int len, void *p_data)
+int ipc_uart_ns16550_send_pdu(void *handle, int len, void *p_data)
 {
-	struct ipc_uart_channels *chan = (struct ipc_uart_channels *) handle;
-
-	int flags = interrupt_lock();
-	if (ipc_uart_tx_state == STATUS_TX_BUSY) {
-		interrupt_unlock(flags);
-		return IPC_UART_ERROR_WRONG_STATE;
-	}
-	ipc_uart_tx_state = STATUS_TX_BUSY;
-	interrupt_unlock(flags);
-
-	uint8_t *p_tx = ipc_uart_tx = balloc(len + IPC_UART_HDR_REQUEST_LEN,
-			NULL);
-
-	/* Adding size of the message request field*/
-	int size = len + sizeof(uint32_t);
+	struct ipc_uart_channels *chan = (struct ipc_uart_channels *)handle;
 
-	/* length = cfw_message size + message request field*/
-	IPC_FRAME_SET_LEN(p_tx, size);
-	IPC_FRAME_SET_CHANNEL(p_tx, chan->index);
-	IPC_FRAME_SET_SRC(p_tx, get_cpu_id());
-	IPC_FRAME_SET_REQUEST(p_tx, IPC_MSG_TYPE_MESSAGE);
+    //pr_debug(LOG_MODULE_IPC, "%s: %d", __FUNCTION__, ipc.tx_state);
 
-	/* IPC_HEADER + request_ID + cfw_message */
-	/* copy cfw_message within IPC frame*/
-	memcpy(IPC_FRAME_DATA(p_tx), p_data, len);
+	if (ipc.tx_state == STATUS_TX_BUSY) {
+		return IPC_UART_TX_BUSY;
+	}
+	
+	/* It is eventually possible to be in DONE state (sending last bytes of previous message),
+	 * so we move immediately to BUSY and configure the next frame */
+	ipc.tx_state = STATUS_TX_BUSY;
 
-	pr_debug(LOG_MODULE_IPC, "%s: tx: channel %d, len %d, request 0x%x",
-			__func__, p_tx[2], len, p_tx[4]);
+	ipc.tx_hdr.len = len;
+	ipc.tx_hdr.channel = chan->index;
+	ipc.tx_hdr.src_cpu_id = 0;
+	ipc.tx_data = p_data;
 
+	/* Enable the interrupt (ready will expire if it was disabled) */
 	UART_IRQ_TX_ENABLE(IPC_UART);
 
 	return IPC_UART_ERROR_OK;
 }
 
-int uart_ipc_send_sync_resp(int channel, int request_id, int param1, int param2,
-			    void * ptr)
+void ipc_uart_ns16550_set_tx_cb(void (*cb)(bool, void *), void *param)
 {
-	if (ipc_uart_tx_state == STATUS_TX_BUSY)
-		return IPC_UART_ERROR_WRONG_STATE;
-	ipc_uart_tx_state = STATUS_TX_BUSY;
-
-	uint8_t *p_tx = ipc_uart_tx = balloc(IPC_UART_HDR_REQUEST_LEN + 12,
-			NULL);
-
-	IPC_FRAME_SET_LEN(p_tx, 16);
-	IPC_FRAME_SET_CHANNEL(p_tx, channel);
-	IPC_FRAME_SET_SRC(ipc_uart_tx, get_cpu_id());
-
-	IPC_FRAME_SET_REQUEST(p_tx, request_id);
-	SYNC_FRAME_SET_PARAM1(p_tx, param1);
-	SYNC_FRAME_SET_PARAM2(p_tx, param2);
-	SYNC_FRAME_SET_PTR(p_tx, ptr);
-
-#ifdef IPC_UART_DBG_SYNC_RESP
-	for (int i = 0; i < 20; i++) {
-		pr_debug(LOG_MODULE_IPC, "%s: IPC sync resp %d byte : %d",
-				__func__,i, p_tx[i]);
-	}
-	pr_debug(LOG_MODULE_IPC, "%s: tx: channel %d, request %xh", __func__,
-			p_tx[2], p_tx[4]);
-#endif
-
-	UART_IRQ_TX_ENABLE(IPC_UART);
-
-	return IPC_UART_ERROR_OK;
+	ipc.tx_cb = cb;
+	ipc.tx_cb_param = param;
 }
+
diff --git a/system/libarc32_arduino101/drivers/ipc_uart_ns16550.h b/system/libarc32_arduino101/drivers/ipc_uart_ns16550.h
index 7fc04670..d71a61b9 100644
--- a/system/libarc32_arduino101/drivers/ipc_uart_ns16550.h
+++ b/system/libarc32_arduino101/drivers/ipc_uart_ns16550.h
@@ -33,19 +33,71 @@
 
 #define IPC_UART 0
 
-enum {
-	STATUS_TX_IDLE = 0,
-	STATUS_TX_BUSY,
-};
 
-enum {
+/** IPC UART return codes */
+enum IPC_UART_RESULT_CODES {
 	IPC_UART_ERROR_OK = 0,
-	IPC_UART_ERROR_WRONG_STATE,
 	IPC_UART_ERROR_DATA_TO_BIG,
+	IPC_UART_TX_BUSY /**< A transmission is already ongoing, message is NOT sent */
+};
+
+
+/**
+ * Channel list
+ */
+enum ipc_channels {
+	RPC_CHANNEL=0, /**< RPC channel */
+	IPC_UART_MAX_CHANNEL = 4
+};
+
+/**
+ * Channel state
+ */
+enum ipc_channel_state {
+	IPC_CHANNEL_STATE_CLOSED = 0,
+	IPC_CHANNEL_STATE_OPEN
+};
+
+/**
+ * Definitions valid for NONE sync IPC UART headers
+ * |len|channel|cpu_id|request|payload|
+ *
+ * len = len(request)+len(payload)
+ */
+
+/**
+ * @note this structure must be self-aligned and self-packed
+ */
+struct ipc_uart_header {
+	uint16_t len;       /**< Length of IPC message, (request + payload) */
+	uint8_t channel;    /**< Channel number of IPC message. */
+	uint8_t src_cpu_id; /**< CPU id of IPC sender. */
 };
 
-void uart_ipc_isr();
-void uart_ipc_push_frame(void);
-void uart_ipc_close_channel(int channel_id);
+/**
+ * IPC channel description
+ */
+struct ipc_uart_channels {
+	uint16_t index; /**< Channel number */
+	uint16_t state; /**< @ref ipc_channel_state */
+	int (*cb)(int chan, int request, int len, void *data);
+	/**< Callback of the channel.
+	 * @param chan Channel index used
+	 * @param request Request id (defined in ipc_requests.h)
+	 * @param len Payload size
+	 * @param data Pointer to data
+	 */
+};
+
+void ipc_uart_init(int num);
+void ipc_uart_isr();
+//static void ipc_uart_push_frame(uint16_t len, uint8_t *p_data);
+void ipc_uart_ns16550_disable(int num);
+void ipc_uart_close_channel(int channel_id);
+void ipc_uart_ns16550_set_tx_cb(void (*cb)(bool, void *), void *param);
+int ipc_uart_ns16550_send_pdu(void *handle, int len, void *p_data);
+void *ipc_uart_channel_open(int channel_id,
+			    int (*cb)(int, int, int, void *));
+
 
 #endif /* _IPC_UART_NS16550_H_ */
diff --git a/system/libarc32_arduino101/drivers/ns16550.c b/system/libarc32_arduino101/drivers/ns16550.c
index 386c6b22..575d43af 100644
--- a/system/libarc32_arduino101/drivers/ns16550.c
+++ b/system/libarc32_arduino101/drivers/ns16550.c
@@ -340,7 +340,7 @@ unsigned char uart_poll_out(
 	)
 {
 	/* wait for transmitter to ready to accept a character */
-	while ((INBYTE(LSR(which)) & LSR_TEMT) == 0)
+	while ((INBYTE(LSR(which)) & LSR_THRE) == 0)
 		;
 
 	OUTBYTE(THR(which), outChar);
@@ -352,6 +352,8 @@ unsigned char uart_poll_out(
 *
 * uart_fifo_fill - fill FIFO with data
 *
+* It is up to the caller to make sure that FIFO capcity is not exceeded
+*
 * RETURNS: number of bytes sent
 */
 
@@ -362,8 +364,9 @@ int uart_fifo_fill(int which, /* UART on which to send */
 {
 	int i;
 
-	for (i = 0; i < size && (INBYTE(LSR(which)) &
-			LSR_BOTH_EMPTY) != 0; i++) {
+	for (i = 0; i < size  && (INBYTE(LSR(which)) &
+			LSR_BOTH_EMPTY) != 0; i++) 
+	{
 		OUTBYTE(THR(which), txData[i]);
 	}
 	return i;
@@ -640,6 +643,19 @@ uint8_t uart_tx_complete(int which)
 	return INBYTE(LSR(which)) & LSR_TEMT;
 }
 
+/*******************************************************************************
+*
+* uart_tx_complete - check if tx holding register is empty
+*
+* RETURNS: zero if register is non-empty, 
+*          non-zero if register is empty (ready to receive new data)
+*/
+
+uint8_t uart_tx_ready(int which)
+{
+	return INBYTE(LSR(which)) & LSR_THRE;
+}
+
 /*******************************************************************************
 *
 * uart_loop_enable - enable loopback
diff --git a/system/libarc32_arduino101/drivers/rpc/rpc.h b/system/libarc32_arduino101/drivers/rpc/rpc.h
new file mode 100644
index 00000000..5d45d463
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/rpc/rpc.h
@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef RPC_H_
+#define RPC_H_
+
+#include <stdint.h>
+
+/** Identifiers of the signature supported by the RPC */
+enum {
+	SIG_TYPE_NONE = 1,
+	SIG_TYPE_S,
+	SIG_TYPE_P,
+	SIG_TYPE_S_B,
+	SIG_TYPE_B_B_P,
+	SIG_TYPE_S_P,
+	SIG_TYPE_S_B_P,
+	SIG_TYPE_S_B_B_P
+};
+
+/**
+ * RPC memory allocation function, must be implemented by the user of the RPC.
+ *
+ * This function is called by the RPC mechanism to allocate a buffer for transmission
+ * of a serialized function.  The function should not fail.
+ *
+ * @param length Length of the buffer to allocate
+ *
+ * @return Pointer to the allocated buffer, the allocation shall not fail, error must
+ * be handled internally
+ */
+uint8_t * rpc_alloc_cb(uint16_t length);
+
+/**
+ * RPC transmission function, must be implemented by the user of the RPC.
+ *
+ * @param p_buf Pointer to the buffer allocated for transmission by @ref rpc_alloc_cb
+ * @param length Length of the buffer to transmit
+ */
+void rpc_transmit_cb(uint8_t * p_buf, uint16_t length);
+
+/**
+ * RPC serialization function to serialize a function that does not require any parameter.
+ *
+ * @param fn_index Index of the function
+ */
+void rpc_serialize_none(uint8_t fn_index);
+
+/**
+ * RPC serialization function to serialize a function that expects a structure as parameter.
+ *
+ * @param fn_index Index of the function
+ * @param struct_data Pointer to the structure to serialize
+ * @param struct_length Length of the structure to serialize
+ */
+void rpc_serialize_s(uint8_t fn_index, const void * struct_data, uint8_t struct_length);
+
+/**
+ * RPC serialization function to serialize a function that expects a structure as parameter.
+ *
+ * @param fn_index Index of the function
+ * @param struct_data Pointer to the structure to serialize
+ * @param struct_length Length of the structure to serialize
+ * @param p_priv Pointer to serialize
+ */
+void rpc_serialize_s_p(uint8_t fn_index, const void * struct_data, uint8_t struct_length, void * p_priv);
+
+/**
+ * RPC serialization function to serialize a function that expects a pointer as parameter.
+ *
+ * @param fn_index Index of the function
+ * @param p_priv Pointer to serialize
+ */
+void rpc_serialize_p(uint8_t fn_index, void * p_priv);
+
+/**
+ * RPC serialization function to serialize a function that expects a structure
+ * and a buffer as parameters.
+ *
+ * @param fn_index Index of the function
+ * @param struct_data Pointer to the structure to serialize
+ * @param struct_length Length of the structure to serialize
+ * @param vbuf Pointer to the buffer to serialize
+ * @param vbuf_length Length of the buffer to serialize
+ */
+void rpc_serialize_s_b(uint8_t fn_index, const void * struct_data, uint8_t struct_length, const void * vbuf, uint16_t vbuf_length);
+
+/**
+ * RPC serialization function to serialize a function that expects a structure
+ * and a buffer as parameters.
+ *
+ * @param fn_index Index of the function
+ * @param vbuf1 Pointer to the buffer1 to serialize
+ * @param vbuf1_length Length of the buffer1 to serialize
+ * @param vbuf2 Pointer to the buffer2 to serialize
+ * @param vbuf2_length Length of the buffer2 to serialize
+ * @param p_priv Pointer to serialize
+ */
+void rpc_serialize_b_b_p(uint8_t fn_index, const void * vbuf1, uint16_t vbuf1_length,
+		const void * vbuf2, uint16_t vbuf2_length, void * p_priv);
+
+/**
+ * RPC serialization function to serialize a function that expects a structure
+ * and a buffer as parameters.
+ *
+ * @param fn_index Index of the function
+ * @param struct_data Pointer to the structure to serialize
+ * @param struct_length Length of the structure to serialize
+ * @param vbuf Pointer to the buffer to serialize
+ * @param vbuf_length Length of the buffer to serialize
+ * @param p_priv Pointer to serialize
+ */
+void rpc_serialize_s_b_p(uint8_t fn_index, const void * struct_data, uint8_t struct_length,
+		const void * vbuf, uint16_t vbuf_length, void * p_priv);
+
+/**
+ * RPC serialization function to serialize a function that expects a structure
+ * and a buffer as parameters.
+ *
+ * @param fn_index Index of the function
+ * @param struct_data Pointer to the structure to serialize
+ * @param struct_length Length of the structure to serialize
+ * @param vbuf1 Pointer to the buffer1 to serialize
+ * @param vbuf1_length Length of the buffer1 to serialize
+ * @param vbuf2 Pointer to the buffer2 to serialize
+ * @param vbuf2_length2 Length of the buffer2 to serialize
+ * @param p_priv Pointer to serialize
+ */
+void rpc_serialize_s_b_b_p(uint8_t fn_index, const void * struct_data, uint8_t struct_length,
+		const void * vbuf1, uint16_t vbuf1_length, const void * vbuf2, uint16_t vbuf2_length, void * p_priv);
+
+/** RPC deserialization function, shall be invoked when a buffer is received over the transport interface.
+ *
+ * @param p_buf Pointer to the received buffer
+ * @param length Length of the received buffer
+ */
+void rpc_deserialize(const uint8_t * p_buf, uint16_t length);
+
+#endif /* RPC_H_*/
diff --git a/system/libarc32_arduino101/drivers/rpc/rpc_deserialize.c b/system/libarc32_arduino101/drivers/rpc/rpc_deserialize.c
new file mode 100644
index 00000000..e3f568d0
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/rpc/rpc_deserialize.c
@@ -0,0 +1,457 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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 <stdlib.h>
+#include <string.h>
+
+#include "rpc.h"
+
+extern void panic(int err);
+
+/* Include the functions offered */
+#if defined(CONFIG_QUARK_SE_BLE_CORE)
+#include "rpc_functions_to_ble_core.h"
+#elif defined(CONFIG_SOC_QUARK_SE)
+#include "rpc_functions_to_quark.h"
+#elif defined(LINUX_HOST_RUNTIME)
+// for the host compilation (to simulate connection to BLE controller)
+#include "rpc_functions_to_ble_core.h"
+#else
+#error "File is compiled but should not"
+#endif
+
+/* Build the list of prototypes and check that list are made only of matching signatures */
+#define FN_SIG_NONE(__fn) void __fn(void);
+LIST_FN_SIG_NONE
+#undef FN_SIG_NONE
+
+#define FN_SIG_S(__fn, __s) void __fn(__s p_s);
+LIST_FN_SIG_S
+#undef FN_SIG_S
+
+#define FN_SIG_P(__fn, __type) void __fn(__type p_priv);
+LIST_FN_SIG_P
+#undef FN_SIG_P
+
+#define FN_SIG_S_B(__fn, __s, __type, __length) void __fn(__s p_s, __type p_buf, __length length);
+LIST_FN_SIG_S_B
+#undef FN_SIG_S_B
+
+#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2, __type3) \
+	void __fn(__type1 p_buf1, __length1 length1, __type2 p_buf2, __length2 length2, __type3 p_priv);
+LIST_FN_SIG_B_B_P
+#undef FN_SIG_B_B_P
+
+#define FN_SIG_S_P(__fn, __s, __type) void __fn(__s p_s, __type p_priv);
+LIST_FN_SIG_S_P
+#undef FN_SIG_S_P
+
+#define FN_SIG_S_B_P(__fn, __s,  __type, __length, __type_ptr) \
+	void __fn(__s p_s, __type p_buf, __length length, __type_ptr p_priv);
+LIST_FN_SIG_S_B_P
+#undef FN_SIG_S_B_P
+
+#define FN_SIG_S_B_B_P(__fn, __s,  __type1, __length1, __type2, __length2, __type_ptr) \
+	void __fn(__s p_s, __type1 p_buf1, __length1 length1, __type2 p_buf2, __length2 length2, __type_ptr p_priv);
+LIST_FN_SIG_S_B_B_P
+#undef FN_SIG_S_B_B_P
+
+
+
+/* 1 - define the size check arrays */
+#define FN_SIG_NONE(__fn)
+
+#define FN_SIG_S(__fn, __s) sizeof(*((__s)0)),
+
+#define FN_SIG_P(__fn, __type)
+
+#define FN_SIG_S_B(__fn, __s, __type, __length) sizeof(*((__s)0)),
+
+#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2, __type3) sizeof(*((__s)0)),
+
+#define FN_SIG_S_P(__fn, __s, __type) sizeof(*((__s)0)),
+
+#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr) sizeof(*((__s)0)),
+
+#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2, __length2, __type3) sizeof(*((__s)0)),
+
+static uint8_t m_size_s[] = { LIST_FN_SIG_S };
+static uint8_t m_size_s_b[] = { LIST_FN_SIG_S_B };
+static uint8_t m_size_s_p[] = { LIST_FN_SIG_S_P };
+static uint8_t m_size_s_b_p[] = { LIST_FN_SIG_S_B_P };
+static uint8_t m_size_s_b_b_p[] = { LIST_FN_SIG_S_B_B_P };
+
+#undef FN_SIG_NONE
+#undef FN_SIG_S
+#undef FN_SIG_P
+#undef FN_SIG_S_B
+#undef FN_SIG_B_B_P
+#undef FN_SIG_S_P
+#undef FN_SIG_S_B_P
+#undef FN_SIG_S_B_B_P
+
+/* 2- build the enumerations list */
+#define FN_SIG_NONE(__fn)                                                          fn_index_##__fn,
+#define FN_SIG_S(__fn, __s)                                                        FN_SIG_NONE(__fn)
+#define FN_SIG_P(__fn, __type)                                                     FN_SIG_NONE(__fn)
+#define FN_SIG_S_B(__fn, __s, __type, __length)                                    FN_SIG_NONE(__fn)
+#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2, __type3)        FN_SIG_NONE(__fn)
+#define FN_SIG_S_P(__fn, __s, __type)                                              FN_SIG_NONE(__fn)
+#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr)                      FN_SIG_NONE(__fn)
+#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2, __length2, __type3) FN_SIG_NONE(__fn)
+
+/* Build the list of function indexes in the deserialization array */
+enum { LIST_FN_SIG_NONE fn_none_index_max };
+enum { LIST_FN_SIG_S fn_s_index_max };
+enum { LIST_FN_SIG_P fn_p_index_max };
+enum { LIST_FN_SIG_S_B fn_s_b_index_max };
+enum { LIST_FN_SIG_B_B_P fn_b_b_p_index_max };
+enum { LIST_FN_SIG_S_P fn_s_p_index_max };
+enum { LIST_FN_SIG_S_B_P fn_s_b_p_index_max };
+enum { LIST_FN_SIG_S_B_B_P fn_s_b_b_p_index_max };
+
+#undef FN_SIG_NONE
+#undef FN_SIG_S
+#undef FN_SIG_P
+#undef FN_SIG_S_B
+#undef FN_SIG_B_B_P
+#undef FN_SIG_S_P
+#undef FN_SIG_S_B_P
+#undef FN_SIG_S_B_B_P
+
+/* 3- build the array */
+#define FN_SIG_NONE(__fn)                                                             [fn_index_##__fn] = (void*)__fn,
+#define FN_SIG_S(__fn, __s)                                                           FN_SIG_NONE(__fn)
+#define FN_SIG_P(__fn, __type)                                                        FN_SIG_NONE(__fn)
+#define FN_SIG_S_B(__fn, __s, __type, __length)                                       FN_SIG_NONE(__fn)
+#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2, __type3)           FN_SIG_NONE(__fn)
+#define FN_SIG_S_P(__fn, __s, __type)                                                 FN_SIG_NONE(__fn)
+#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr)                         FN_SIG_NONE(__fn)
+#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2, __length2, __type3)    FN_SIG_NONE(__fn)
+
+static void (*m_fct_none[])(void) = { LIST_FN_SIG_NONE };
+static void (*m_fct_s[])(void * structure) = { LIST_FN_SIG_S };
+static void (*m_fct_p[])(void * pointer) = { LIST_FN_SIG_P };
+static void (*m_fct_s_b[])(void * structure, void * buffer, uint8_t length) = { LIST_FN_SIG_S_B };
+static void (*m_fct_b_b_p[])(void * buffer1, uint8_t length1, void * buffer2, uint8_t length2, void * pointer) = { LIST_FN_SIG_B_B_P };
+static void (*m_fct_s_p[])(void * structure, void * pointer) = { LIST_FN_SIG_S_P };
+static void (*m_fct_s_b_p[])(void * structure, void * buffer, uint8_t length, void * pointer) = { LIST_FN_SIG_S_B_P };
+static void (*m_fct_s_b_b_p[])(void * structure, void * buffer1, uint8_t length1, void * buffer2, uint8_t length2, void * pointer) = { LIST_FN_SIG_S_B_B_P };
+
+static const uint8_t * deserialize_struct(const uint8_t *p, const uint8_t **pp_struct, uint8_t *p_struct_length) {
+	uint8_t struct_length;
+
+	struct_length = *p++;
+	*pp_struct = p;
+	*p_struct_length = struct_length;
+
+	return p + struct_length;
+}
+
+static const uint8_t * deserialize_buf(const uint8_t *p, const uint8_t **pp_buf, uint16_t *p_buflen) {
+	uint8_t b;
+	uint16_t buflen;
+
+	/* Get the current byte */
+	b = *p++;
+	buflen = b & 0x7F;
+	if (b & 0x80) {
+		/* Get the current byte */
+		b = *p++;
+		buflen += (uint16_t)b << 7;
+	}
+
+	/* Return the values */
+	*pp_buf = p;
+	*p_buflen = buflen;
+	p += buflen;
+	return p;
+}
+
+static void deserialize_none(uint8_t fn_index, const uint8_t * p_buf, uint16_t length) {
+	(void)p_buf;
+	if (length != 0)
+		panic(-1);
+	m_fct_none[fn_index]();
+}
+
+static void deserialize_s(uint8_t fn_index, const uint8_t * p_buf, uint16_t length) {
+	const uint8_t *p_struct_data;
+	uint8_t struct_length;
+	const uint8_t *p;
+
+	p = deserialize_struct(p_buf, &p_struct_data, &struct_length);
+
+	if ((length != (p - p_buf)) ||
+	   (struct_length != m_size_s[fn_index]))
+		panic(-1);
+
+	{
+		/* Always align structures on word boundary */
+		uintptr_t struct_data[(struct_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+
+		memcpy(struct_data, p_struct_data, struct_length);
+
+		m_fct_s[fn_index](struct_data);
+	}
+}
+
+static void deserialize_p(uint8_t fn_index, const uint8_t * p_buf, uint16_t length) {
+	uintptr_t p_priv;
+
+	if (length != 4)
+		panic(-1);
+
+	/* little endian conversion */
+	p_priv = p_buf[0] | (p_buf[1] << 8) | (p_buf[2] << 16) | (p_buf[3] << 24);
+
+	m_fct_p[fn_index]((void *)p_priv);
+}
+
+static void deserialize_s_b(uint8_t fn_index, const uint8_t * p_buf, uint16_t length) {
+	const uint8_t *p_struct_data;
+	uint8_t struct_length;
+	const uint8_t *p_vbuf;
+	uint16_t vbuf_length;
+	const uint8_t *p;
+
+	p = deserialize_struct(p_buf, &p_struct_data, &struct_length);
+	p = deserialize_buf(p, &p_vbuf, &vbuf_length);
+
+	if ((length != (p - p_buf)) ||
+	    (struct_length != m_size_s_b[fn_index]))
+		panic(-1);
+
+	{
+		/* Always align structures on word boundary */
+		uintptr_t struct_data[(struct_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		uintptr_t vbuf[(vbuf_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		void * buf = NULL;
+
+		memcpy(struct_data, p_struct_data, struct_length);
+
+		if (vbuf_length) {
+			memcpy(vbuf, p_vbuf, vbuf_length);
+			buf = vbuf;
+		}
+
+		m_fct_s_b[fn_index](struct_data, buf, vbuf_length);
+	}
+}
+
+static void deserialize_b_b_p(uint8_t fn_index, const uint8_t * p_buf, uint16_t length) {
+	const uint8_t *p_vbuf1;
+	uint16_t vbuf1_length;
+	const uint8_t *p_vbuf2;
+	uint16_t vbuf2_length;
+	uintptr_t p_priv;
+	const uint8_t *p;
+
+	p = deserialize_buf(p_buf, &p_vbuf1, &vbuf1_length);
+	p = deserialize_buf(p, &p_vbuf2, &vbuf2_length);
+	p += 4;
+
+	if (length != (p - p_buf))
+		panic(-1);
+
+	{
+		/* Always align structures on word boundary */
+		uintptr_t vbuf1[(vbuf1_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		uintptr_t vbuf2[(vbuf2_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		void * buf1 = NULL;
+		void * buf2 = NULL;
+
+		if (vbuf1_length) {
+			memcpy(vbuf1, p_vbuf1, vbuf1_length);
+			buf1 = vbuf1;
+		}
+
+		if (vbuf2_length) {
+			memcpy(vbuf2, p_vbuf2, vbuf2_length);
+			buf2 = vbuf2;
+		}
+		p = p_vbuf2 + vbuf2_length;
+
+		/* little endian conversion */
+		p_priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
+
+		m_fct_b_b_p[fn_index](buf1, vbuf1_length, buf2, vbuf2_length, (void *)p_priv);
+	}
+}
+
+static void deserialize_s_p(uint8_t fn_index, const uint8_t * p_buf, uint16_t length)
+{
+	const uint8_t *p_struct_data;
+	uint8_t struct_length;
+	uintptr_t p_priv;
+	const uint8_t *p;
+
+	p = deserialize_struct(p_buf, &p_struct_data, &struct_length);
+	p += 4;
+
+	if ((length != (p - p_buf)) ||
+	    (struct_length != m_size_s_p[fn_index]))
+		panic(-1);
+
+	{
+		/* Always align structures on word boundary */
+		uintptr_t struct_data[(struct_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+
+		memcpy(struct_data, p_struct_data, struct_length);
+		p = p_struct_data + struct_length;
+
+		/* little endian conversion */
+		p_priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
+
+		m_fct_s_p[fn_index](struct_data, (void *)p_priv);
+	}
+}
+
+static void deserialize_s_b_p(uint8_t fn_index, const uint8_t * p_buf, uint16_t length)
+{
+	const uint8_t *p_struct_data;
+	uint8_t struct_length;
+	const uint8_t *p_vbuf;
+	uint16_t vbuf_length;
+	uintptr_t p_priv;
+	const uint8_t *p;
+
+	p = deserialize_struct(p_buf, &p_struct_data, &struct_length);
+	p = deserialize_buf(p, &p_vbuf, &vbuf_length);
+	p += 4;
+
+	if ((length != (p - p_buf)) ||
+	    (struct_length != m_size_s_b_p[fn_index]))
+		panic(-1);
+
+	{
+		/* Always align structures on word boundary */
+		uintptr_t struct_data[(struct_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		uintptr_t vbuf[(vbuf_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		void * buf = NULL;
+
+		memcpy(struct_data, p_struct_data, struct_length);
+
+		if (vbuf_length) {
+			memcpy(vbuf, p_vbuf, vbuf_length);
+			buf = vbuf;
+		}
+		p = p_vbuf + vbuf_length;
+
+		/* little endian conversion */
+		p_priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
+
+		m_fct_s_b_p[fn_index](struct_data, buf, vbuf_length, (void *)p_priv);
+	}
+}
+
+static void deserialize_s_b_b_p(uint8_t fn_index, const uint8_t * p_buf, uint16_t length) {
+	const uint8_t *p_struct_data;
+	uint8_t struct_length;
+	const uint8_t *p_vbuf1;
+	uint16_t vbuf1_length;
+	const uint8_t *p_vbuf2;
+	uint16_t vbuf2_length;
+	uintptr_t p_priv;
+	const uint8_t *p;
+
+	p = deserialize_struct(p_buf, &p_struct_data, &struct_length);
+	p = deserialize_buf(p, &p_vbuf1, &vbuf1_length);
+	p = deserialize_buf(p, &p_vbuf2, &vbuf2_length);
+	p += 4;
+	if ((length != (p - p_buf)) ||
+		 (struct_length != m_size_s_b_b_p[fn_index]))
+		panic(-1);
+
+	{
+		/* Always align structures on word boundary */
+		uintptr_t struct_data[(struct_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		uintptr_t vbuf1[(vbuf1_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		uintptr_t vbuf2[(vbuf2_length + (sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
+		void * buf1 = NULL;
+		void * buf2 = NULL;
+
+		memcpy(struct_data, p_struct_data, struct_length);
+
+		if (vbuf1_length) {
+			memcpy(vbuf1, p_vbuf1, vbuf1_length);
+			buf1 = vbuf1;
+		}
+		if (vbuf2_length) {
+			memcpy(vbuf2, p_vbuf2, vbuf2_length);
+			buf2 = vbuf2;
+		}
+
+		p = p_vbuf2 + vbuf2_length;
+
+		/* little endian conversion */
+		p_priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
+
+		m_fct_s_b_b_p[fn_index](struct_data, buf1, vbuf1_length, buf2, vbuf2_length, (void *)p_priv);
+	}
+}
+
+void rpc_deserialize(const uint8_t * p_buf, uint16_t length) {
+
+	uint8_t fn_index;
+	uint8_t sig_type;
+
+	if (NULL != p_buf) {
+		sig_type = p_buf[0];
+		fn_index = p_buf[1];
+
+		p_buf += 2;
+		length -= 2;
+
+		switch(sig_type) {
+		case SIG_TYPE_NONE:
+			if (sizeof(m_fct_none))
+				deserialize_none(fn_index, p_buf, length);
+			break;
+		case SIG_TYPE_S:
+			if (sizeof(m_fct_s))
+				deserialize_s(fn_index, p_buf, length);
+			break;
+		case SIG_TYPE_P:
+			if (sizeof(m_fct_p))
+				deserialize_p(fn_index, p_buf, length);
+			break;
+		case SIG_TYPE_S_B:
+			if (sizeof(m_fct_s_b))
+				deserialize_s_b(fn_index, p_buf, length);
+			break;
+		case SIG_TYPE_B_B_P:
+			if (sizeof(m_fct_b_b_p))
+				deserialize_b_b_p(fn_index, p_buf, length);
+			break;
+		case SIG_TYPE_S_P:
+			if (sizeof(m_fct_s_p))
+				deserialize_s_p(fn_index, p_buf, length);
+			break;
+		case SIG_TYPE_S_B_P:
+			if (sizeof(m_fct_s_b_p))
+				deserialize_s_b_p(fn_index, p_buf, length);
+			break;
+		case SIG_TYPE_S_B_B_P:
+			if (sizeof(m_fct_s_b_b_p))
+				deserialize_s_b_b_p(fn_index, p_buf, length);
+			break;
+		default:
+			panic(-1);
+			break;
+		}
+	}
+}
diff --git a/system/libarc32_arduino101/drivers/rpc/rpc_functions_to_ble_core.h b/system/libarc32_arduino101/drivers/rpc/rpc_functions_to_ble_core.h
new file mode 100644
index 00000000..5f742208
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/rpc/rpc_functions_to_ble_core.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef RPC_FUNCTIONS_TO_BLE_CORE_H_
+#define RPC_FUNCTIONS_TO_BLE_CORE_H_
+
+#include "gap_internal.h"
+#include "gatt_internal.h"
+
+/* declare the list of functions sorted by signature */
+#define LIST_FN_SIG_NONE						\
+	FN_SIG_NONE(nble_gap_start_adv_req)				\
+	FN_SIG_NONE(nble_gap_stop_scan_req)
+
+#define LIST_FN_SIG_S							\
+	FN_SIG_S(nble_gap_set_adv_data_req,				\
+		 struct nble_gap_ad_data_params *)			\
+	FN_SIG_S(nble_gap_set_adv_params_req,				\
+		 struct nble_gap_adv_params *)				\
+	FN_SIG_S(nble_gap_start_scan_req,				\
+		 const struct nble_gap_scan_params *)			\
+	FN_SIG_S(nble_gap_sm_config_req,				\
+		 const struct nble_gap_sm_config_params *)		\
+	FN_SIG_S(nble_gap_sm_passkey_reply_req,				\
+		 const struct nble_gap_sm_key_reply_req_params *)	\
+	FN_SIG_S(nble_gap_sm_bond_info_req,				\
+		 const struct nble_gap_sm_bond_info_param *)		\
+	FN_SIG_S(nble_gap_sm_security_req,				\
+		 const struct nble_gap_sm_security_params *)		\
+	FN_SIG_S(nble_gap_sm_clear_bonds_req,				\
+		 const struct nble_gap_sm_clear_bond_req_params *)	\
+	FN_SIG_S(nble_set_bda_req, const struct nble_set_bda_params *)	\
+	FN_SIG_S(nble_gap_conn_update_req,				\
+		 const struct nble_gap_connect_update_params *)		\
+	FN_SIG_S(nble_gattc_discover_req,				\
+		const struct nble_discover_params *)			\
+	FN_SIG_S(nble_gatts_wr_reply_req,				\
+		const struct nble_gatts_wr_reply_params *)		\
+	FN_SIG_S(nble_uas_rssi_calibrate_req,				\
+		const struct nble_uas_rssi_calibrate *)			\
+	FN_SIG_S(nble_gap_service_write_req,				\
+		   const struct nble_gap_service_write_params *)	\
+	FN_SIG_S(nble_gap_disconnect_req,				\
+		   const struct nble_gap_disconnect_req_params *)	\
+	FN_SIG_S(nble_gattc_read_req,					\
+		   const struct ble_gattc_read_params *)		\
+	FN_SIG_S(nble_gap_tx_power_req,					\
+		   const struct nble_gap_tx_power_params *)		\
+	FN_SIG_S(nble_get_version_req,					\
+		   const struct nble_gap_get_version_param *)
+
+#define LIST_FN_SIG_P							\
+	FN_SIG_P(nble_gap_dtm_init_req, void *)				\
+	FN_SIG_P(nble_gap_read_bda_req, void *)				\
+	FN_SIG_P(nble_gap_stop_adv_req, void *)				\
+	FN_SIG_P(nble_gap_cancel_connect_req, void *)
+
+#define LIST_FN_SIG_S_B							\
+	FN_SIG_S_B(nble_gatt_register_req,				\
+		   const struct nble_gatt_register_req *,		\
+		   uint8_t *, uint16_t)					\
+	FN_SIG_S_B(nble_gatt_send_notif_req,				\
+		   const struct nble_gatt_send_notif_params *,		\
+		   const uint8_t *, uint16_t)				\
+	FN_SIG_S_B(nble_gatt_send_ind_req,				\
+		   const struct nble_gatt_send_ind_params *,		\
+		   const uint8_t *, uint8_t)				\
+	FN_SIG_S_B(nble_gatts_rd_reply_req,				\
+		   const struct nble_gatts_rd_reply_params *,		\
+		   uint8_t *, uint16_t)					\
+	FN_SIG_S_B(nble_gattc_write_req,				\
+		   const struct ble_gattc_write_params *,		\
+		   const uint8_t *, uint8_t)				\
+	FN_SIG_S_B(nble_gattc_read_multiple_req,				\
+		   const struct ble_gattc_read_multiple_params *,	\
+		   const uint16_t *, uint16_t)
+
+#define LIST_FN_SIG_B_B_P
+
+#define LIST_FN_SIG_S_P							\
+	FN_SIG_S_P(nble_gap_connect_req,				\
+		   const struct nble_gap_connect_req_params *, void *)	\
+	FN_SIG_S_P(nble_gap_set_rssi_report_req,			\
+		   const struct nble_rssi_report_params *, void *)	\
+	FN_SIG_S_P(nble_gap_dbg_req,					\
+		   const struct nble_debug_params *,			\
+		   void *)
+
+#define LIST_FN_SIG_S_B_P
+
+#define LIST_FN_SIG_S_B_B_P
+
+#endif /* RPC_FUNCTIONS_TO_BLE_CORE_H_ */
diff --git a/system/libarc32_arduino101/drivers/rpc/rpc_functions_to_quark.h b/system/libarc32_arduino101/drivers/rpc/rpc_functions_to_quark.h
new file mode 100644
index 00000000..b08005e6
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/rpc/rpc_functions_to_quark.h
@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef RPC_FUNCTIONS_TO_QUARK_H_
+#define RPC_FUNCTIONS_TO_QUARK_H_
+
+#include "gap_internal.h"
+#include "gatt_internal.h"
+#include "gap_internal.h"
+
+/* declare the list of functions sorted by signature */
+#define LIST_FN_SIG_NONE					\
+	FN_SIG_NONE(on_nble_up)
+
+#define LIST_FN_SIG_S						\
+	FN_SIG_S(on_nble_get_version_rsp,			\
+		 const struct nble_version_response *)		\
+	FN_SIG_S(on_nble_gap_connect_evt,			\
+		 const struct nble_gap_connect_evt *)		\
+	FN_SIG_S(on_nble_gap_disconnect_evt,			\
+		 const struct nble_gap_disconnect_evt *)	\
+	FN_SIG_S(on_nble_gap_conn_update_evt,			\
+		 const struct nble_gap_conn_update_evt *)	\
+	FN_SIG_S(on_nble_gap_sm_status_evt,			\
+		 const struct nble_gap_sm_status_evt *)		\
+	FN_SIG_S(on_nble_gap_sm_passkey_display_evt,		\
+		 const struct nble_gap_sm_passkey_disp_evt *)	\
+	FN_SIG_S(on_nble_gap_sm_passkey_req_evt,		\
+		 const struct nble_gap_sm_passkey_req_evt *)	\
+	FN_SIG_S(on_nble_gap_rssi_evt,				\
+		 const struct nble_gap_rssi_evt *)		\
+	FN_SIG_S(on_nble_common_rsp,				\
+		 const struct nble_response *)			\
+	FN_SIG_S(on_nble_gap_connect_rsp,			\
+		 const struct nble_response *)			\
+	FN_SIG_S(on_nble_gap_cancel_connect_rsp,		\
+		 const struct nble_response *)			\
+	FN_SIG_S(on_nble_gap_read_bda_rsp,			\
+		 const struct nble_service_read_bda_response *)	\
+	FN_SIG_S(on_nble_gap_sm_config_rsp,			\
+		 struct nble_gap_sm_config_rsp *)		\
+	FN_SIG_S(on_nble_gap_sm_common_rsp,			\
+		 const struct nble_gap_sm_response *)		\
+	FN_SIG_S(on_nble_set_bda_rsp,				\
+		 const struct nble_set_bda_rsp *)		\
+	FN_SIG_S(on_nble_gap_set_rssi_report_rsp,		\
+		 const struct nble_response *)			\
+	FN_SIG_S(on_nble_gap_dbg_rsp,				\
+		 const struct nble_debug_resp *)		\
+	FN_SIG_S(on_nble_gap_dir_adv_timeout_evt,		\
+		 const struct nble_gap_dir_adv_timeout_evt *)	\
+	FN_SIG_S(on_nble_gatts_send_notif_rsp,			\
+		 const struct nble_gatt_notif_rsp *)		\
+	FN_SIG_S(on_nble_gatts_send_ind_rsp,			\
+		 const struct nble_gatt_ind_rsp *)		\
+	FN_SIG_S(on_nble_gap_start_advertise_rsp,		\
+		 const struct nble_response *)			\
+	FN_SIG_S(on_nble_gap_scan_start_stop_rsp,		\
+		 const struct nble_response *)			\
+	FN_SIG_S(on_nble_gatts_read_evt,			\
+		 const struct nble_gatt_rd_evt *)		\
+	FN_SIG_S(on_nble_gatts_write_exec_evt,			\
+		 const struct nble_gatt_wr_exec_evt *)		\
+	FN_SIG_S(on_nble_uas_bucket_change,			\
+		 const struct nble_uas_bucket_change *)	\
+	FN_SIG_S(on_nble_gattc_write_rsp,			\
+		 const struct ble_gattc_write_rsp *)		\
+	FN_SIG_S(on_nble_gap_tx_power_rsp,			\
+		 const struct nble_response *)
+
+#define LIST_FN_SIG_P						\
+	FN_SIG_P(on_nble_gap_dtm_init_rsp, void *)
+
+#define LIST_FN_SIG_S_B						\
+	FN_SIG_S_B(nble_log, const struct nble_log_s *, char *,	\
+		   uint8_t)					\
+	FN_SIG_S_B(on_nble_gattc_value_evt,			\
+		   const struct ble_gattc_value_evt *,		\
+		   uint8_t *, uint8_t)				\
+	FN_SIG_S_B(on_nble_gatts_write_evt,			\
+		   const struct nble_gatt_wr_evt *,		\
+		   const uint8_t *, uint8_t)			\
+	FN_SIG_S_B(on_nble_gatt_register_rsp,			\
+		   const struct nble_gatt_register_rsp *,	\
+		   const struct nble_gatt_attr_handles *,	\
+		   uint8_t)					\
+	FN_SIG_S_B(on_nble_gattc_discover_rsp,			\
+		   const struct nble_gattc_discover_rsp *,	\
+		   const uint8_t *, uint8_t)			\
+	FN_SIG_S_B(on_nble_gap_adv_report_evt,			\
+		   const struct nble_gap_adv_report_evt *,	\
+		   const uint8_t *, uint8_t)			\
+	FN_SIG_S_B(on_nble_gap_sm_bond_info_rsp,		\
+		   const struct nble_gap_sm_bond_info_rsp*,	\
+		   const bt_addr_le_t *, uint16_t)		\
+	FN_SIG_S_B(on_nble_gattc_read_rsp,			\
+		     const struct ble_gattc_read_rsp *,		\
+		     uint8_t *, uint8_t)			\
+	FN_SIG_S_B(on_nble_gattc_read_multiple_rsp,		\
+		     const struct ble_gattc_read_rsp *,		\
+		     uint8_t *, uint8_t)
+
+#define LIST_FN_SIG_B_B_P
+
+#define LIST_FN_SIG_S_P
+
+#define LIST_FN_SIG_S_B_P
+
+#define LIST_FN_SIG_S_B_B_P
+
+#endif /* RPC_FUNCTIONS_TO_QUARK_H_ */
diff --git a/system/libarc32_arduino101/drivers/rpc/rpc_serialize.c b/system/libarc32_arduino101/drivers/rpc/rpc_serialize.c
new file mode 100644
index 00000000..e2144ee3
--- /dev/null
+++ b/system/libarc32_arduino101/drivers/rpc/rpc_serialize.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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 <string.h>
+
+#include "rpc.h"
+
+/* Include the functions called */
+#if defined(CONFIG_QUARK_SE_BLE_CORE)
+#include "rpc_functions_to_quark.h"
+#elif defined(CONFIG_SOC_QUARK_SE)
+#include "rpc_functions_to_ble_core.h"
+#elif defined(LINUX_HOST_RUNTIME)
+// for the host compilation (to simulate connection to BLE controller)
+#include "rpc_functions_to_ble_core.h"
+#else
+#error "File is compiled but should not"
+#endif
+
+/* Build the functions exposed */
+/* Define the functions identifiers per signature */
+#define FN_SIG_NONE(__fn)                                                          fn_index_##__fn,
+#define FN_SIG_S(__fn, __s)                                                        FN_SIG_NONE(__fn)
+#define FN_SIG_P(__fn, __type)                                                     FN_SIG_NONE(__fn)
+#define FN_SIG_S_B(__fn, __s, __type, __length)                                    FN_SIG_NONE(__fn)
+#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2,__type3)         FN_SIG_NONE(__fn)
+#define FN_SIG_S_P(__fn, __s, __type)                                              FN_SIG_NONE(__fn)
+#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr)                      FN_SIG_NONE(__fn)
+#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2, __length2,__type3)  FN_SIG_NONE(__fn)
+
+/* Build the list of function indexes -> this should match the array at deserialization */
+enum { LIST_FN_SIG_NONE fn_none_index_max };
+enum { LIST_FN_SIG_S fn_s_index_max };
+enum { LIST_FN_SIG_P fn_p_index_max };
+enum { LIST_FN_SIG_S_B fn_s_b_index_max };
+enum { LIST_FN_SIG_B_B_P fn_b_b_p_index_max };
+enum { LIST_FN_SIG_S_P fn_s_p_index_max };
+enum { LIST_FN_SIG_S_B_P fn_s_b_p_index_max };
+enum { LIST_FN_SIG_S_B_B_P fn_s_b_b_p_index_max };
+
+/* Implement the functions using serialization API */
+#undef FN_SIG_NONE
+#undef FN_SIG_S
+#undef FN_SIG_P
+#undef FN_SIG_S_B
+#undef FN_SIG_B_B_P
+#undef FN_SIG_S_P
+#undef FN_SIG_S_B_P
+#undef FN_SIG_S_B_B_P
+
+#define FN_SIG_NONE(__fn)                     \
+    void __fn(void) {                         \
+        rpc_serialize_none(fn_index_##__fn);  \
+    }                                         \
+
+#define FN_SIG_S(__fn, __s)                                   \
+    void __fn(__s p_s) {                                      \
+        rpc_serialize_s(fn_index_##__fn, p_s, sizeof(*p_s));  \
+    }                                                         \
+
+#define FN_SIG_P(__fn, __type)                                  \
+    void __fn(__type p_priv) {                                  \
+        rpc_serialize_p(fn_index_##__fn, p_priv);               \
+    }                                                           \
+
+#define FN_SIG_S_B(__fn, __s, __type, __length)                               \
+    void __fn(__s p_s, __type p_buf, __length length) {                       \
+        rpc_serialize_s_b(fn_index_##__fn, p_s, sizeof(*p_s), p_buf, length); \
+    }                                                                         \
+
+#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2, __type3)                             \
+    void __fn(__type1 p_buf1, __length1 length1, __type2 p_buf2, __length2 length2, __type3 p_priv) {   \
+        rpc_serialize_b_b_p(fn_index_##__fn, p_buf1, length1, p_buf2, length2, p_priv);            \
+    }                                                                                              \
+
+#define FN_SIG_S_P(__fn, __s, __type)                                         \
+    void __fn(__s p_s, __type p_priv) {                                       \
+        rpc_serialize_s_p(fn_index_##__fn, p_s, sizeof(*p_s), p_priv);        \
+    }                                                                         \
+
+#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr)                                   \
+    void __fn(__s p_s, __type p_buf, __length length, __type_ptr p_priv) {                      \
+        rpc_serialize_s_b_p(fn_index_##__fn, p_s, sizeof(*p_s), p_buf, length, p_priv);     \
+    }
+
+#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2, __length2, __type3)                                \
+    void __fn(__s p_s, __type1 p_buf1, __length1 length1, __type2 p_buf2, __length2 length2, __type3 p_priv) {    \
+        rpc_serialize_s_b_b_p(fn_index_##__fn, p_s, sizeof(*p_s), p_buf1, length1, p_buf2, length2, p_priv);      \
+    }                                                                                                             \
+
+
+/* Build the functions */
+LIST_FN_SIG_NONE
+LIST_FN_SIG_S
+LIST_FN_SIG_P
+LIST_FN_SIG_S_B
+LIST_FN_SIG_B_B_P
+LIST_FN_SIG_S_P
+LIST_FN_SIG_S_B_P
+LIST_FN_SIG_S_B_B_P
+
+#define SIG_TYPE_SIZE 1
+#define FN_INDEX_SIZE 1
+#define POINTER_SIZE 4
+
+static void _send(uint8_t *buf, uint16_t length) {
+	rpc_transmit_cb(buf, length);
+}
+
+static uint16_t encoded_structlen(uint8_t structlen) {
+	return 1 + structlen;
+}
+
+static uint8_t *serialize_struct(uint8_t *p, const uint8_t *struct_data, uint8_t struct_length) {
+	*p++ = struct_length;
+	memcpy(p, struct_data, struct_length);
+	p += struct_length;
+	return p;
+}
+
+static uint16_t encoded_buflen(const uint8_t *buf, uint16_t buflen) {
+	if (NULL == buf)
+		return 1;
+	else {
+		if (buflen < (1 << 7)) {
+			return 1 + buflen;
+		}
+		else
+			return 2 + buflen;
+	}
+}
+
+static uint8_t *serialize_buf(uint8_t *p, const uint8_t *buf, uint16_t buflen) {
+	uint16_t varint;
+
+	if (NULL == buf)
+		buflen = 0;
+
+	varint = buflen;
+
+	*p = varint & 0x7F;
+	if (varint >= (1 << 7)) {
+		*p |= 0x80;
+		p++;
+		*p = varint >> 7;
+	}
+	p++;
+	memcpy(p, buf, buflen);
+	p += buflen;
+	return p;
+}
+
+static uint8_t *serialize_p(uint8_t *p, uintptr_t priv) {
+	*p++ = priv;
+	*p++ = (priv >> 8);
+	*p++ = (priv >> 16);
+	*p++ = (priv >> 24);
+	return p;
+}
+
+void rpc_serialize_none(uint8_t fn_index) {
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE;
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_NONE;
+	*p   = fn_index;
+
+	_send(buf, length);
+}
+
+void rpc_serialize_s(uint8_t fn_index, const void * struct_data, uint8_t struct_length) {
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE + encoded_structlen(struct_length);
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_S;
+	*p++ = fn_index;
+	p = serialize_struct(p, struct_data, struct_length);
+
+	_send(buf, length);
+}
+
+
+void rpc_serialize_p(uint8_t fn_index, void * p_priv) {
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+	uintptr_t priv = (uintptr_t) p_priv;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE + POINTER_SIZE;
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_P;
+	*p++ = fn_index;
+	p = serialize_p(p, priv);
+
+	_send(buf, length);
+}
+
+void rpc_serialize_s_b(uint8_t fn_index, const void * struct_data, uint8_t struct_length, const void * vbuf, uint16_t vbuf_length) {
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE + encoded_structlen(struct_length) +
+			encoded_buflen(vbuf, vbuf_length);
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_S_B;
+	*p++ = fn_index;
+	p = serialize_struct(p, struct_data, struct_length);
+	p = serialize_buf(p, vbuf, vbuf_length);
+
+	_send(buf, length);
+}
+
+void rpc_serialize_b_b_p(uint8_t fn_index, const void * vbuf1, uint16_t vbuf1_length, const void * vbuf2, uint16_t vbuf2_length, void * p_priv) {
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+	uintptr_t priv = (uintptr_t) p_priv;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE + encoded_buflen(vbuf1, vbuf1_length) +
+			encoded_buflen(vbuf2, vbuf2_length) + POINTER_SIZE;
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_B_B_P;
+	*p++ = fn_index;
+	p = serialize_buf(p, vbuf1, vbuf1_length);
+	p = serialize_buf(p, vbuf2, vbuf2_length);
+	p = serialize_p(p, priv);
+
+	_send(buf, length);
+}
+
+void rpc_serialize_s_p(uint8_t fn_index, const void * struct_data, uint8_t struct_length, void * p_priv) {
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+	uintptr_t priv = (uintptr_t) p_priv;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE + encoded_structlen(struct_length)
+			+ POINTER_SIZE;
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_S_P;
+	*p++ = fn_index;
+	p = serialize_struct(p, struct_data, struct_length);
+	p = serialize_p(p, priv);
+
+	_send(buf, length);
+}
+
+void rpc_serialize_s_b_p(uint8_t fn_index, const void * struct_data, uint8_t struct_length,
+		const void * vbuf, uint16_t vbuf_length, void * p_priv) {
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+	uintptr_t priv = (uintptr_t) p_priv;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE + encoded_structlen(struct_length) +
+			encoded_buflen(vbuf, vbuf_length) + POINTER_SIZE;
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_S_B_P;
+	*p++ = fn_index;
+	p = serialize_struct(p, struct_data, struct_length);
+	p = serialize_buf(p, vbuf, vbuf_length);
+	p = serialize_p(p, priv);
+
+	_send(buf, length);
+}
+
+void rpc_serialize_s_b_b_p(uint8_t fn_index, const void * struct_data, uint8_t struct_length,
+		const void * vbuf1, uint16_t vbuf1_length, const void * vbuf2, uint16_t vbuf2_length, void * p_priv) {
+
+	uint16_t length;
+	uint8_t * buf;
+	uint8_t * p;
+	uintptr_t priv = (uintptr_t) p_priv;
+
+	length = SIG_TYPE_SIZE + FN_INDEX_SIZE + encoded_structlen(struct_length) +
+			encoded_buflen(vbuf1, vbuf1_length) +
+			encoded_buflen(vbuf2, vbuf2_length) + POINTER_SIZE;
+
+	p = buf = rpc_alloc_cb(length);
+
+	*p++ = SIG_TYPE_S_B_B_P;
+	*p++ = fn_index;
+	p = serialize_struct(p, struct_data, struct_length);
+	p = serialize_buf(p, vbuf1, vbuf1_length);
+	p = serialize_buf(p, vbuf2, vbuf2_length);
+	p = serialize_p(p, priv);
+
+	_send(buf, length);
+}
diff --git a/system/libarc32_arduino101/drivers/soc_gpio.c b/system/libarc32_arduino101/drivers/soc_gpio.c
index c06f14e4..382f8c61 100644
--- a/system/libarc32_arduino101/drivers/soc_gpio.c
+++ b/system/libarc32_arduino101/drivers/soc_gpio.c
@@ -34,6 +34,7 @@
 
 #include "soc_register.h"
 #include "portable.h"
+#include "platform.h"
 
 #define GPIO_CLKENA_POS         (31)
 #define GPIO_LS_SYNC_POS        (0)
@@ -429,6 +430,8 @@ static void soc_gpio_ISR_proc( uint32_t dev_id )
     uint32_t status = MMIO_REG_VAL_FROM_BASE(dev->reg_base, SOC_GPIO_INTSTATUS);
     // Mask the pending interrupts
     MMIO_REG_VAL_FROM_BASE(dev->reg_base, SOC_GPIO_INTMASK) |= status;
+    // Save a copy of the INTSTATUS register
+    shared_data->pm_int_status = status;
     // Clear interrupt flag (write 1 to clear)
     MMIO_REG_VAL_FROM_BASE(dev->reg_base, SOC_GPIO_PORTA_EOI) = status;
 
diff --git a/system/libarc32_arduino101/drivers/soc_gpio.h b/system/libarc32_arduino101/drivers/soc_gpio.h
index 401011cb..a2623f2d 100644
--- a/system/libarc32_arduino101/drivers/soc_gpio.h
+++ b/system/libarc32_arduino101/drivers/soc_gpio.h
@@ -43,6 +43,7 @@
 
 #include "data_type.h"
 #include "gpio.h"
+#include "platform.h"
 
 // soc gpio 32 bit count
 #if defined(CONFIG_SOC_GPIO_32)
diff --git a/system/libarc32_arduino101/drivers/ss_gpio_iface.c b/system/libarc32_arduino101/drivers/ss_gpio_iface.c
index 9d8543b6..fad7743e 100644
--- a/system/libarc32_arduino101/drivers/ss_gpio_iface.c
+++ b/system/libarc32_arduino101/drivers/ss_gpio_iface.c
@@ -37,6 +37,7 @@
 #include "io_config.h"
 #include "eiaextensions.h"
 #include "portable.h"
+#include "platform.h"
 
 /* EIA GPIO device registers  */
 #define     SWPORTA_DR      (0x00)  /* GPIO Port A Data Register*/
@@ -355,6 +356,8 @@ static void ss_gpio_ISR_proc( uint32_t dev_id )
     uint32_t status = REG_READ( INTSTATUS );
     /* Mask the pending IRQ in order to avoid a storm of interrupts */
     REG_WRITE(INTMASK, REG_READ(INTMASK) | status);
+    // Save a copy of the INTSTATUS register
+    shared_data->pm_int_status = status;
     // Clear interrupt flag (write 1 to clear)
     REG_WRITE( PORTA_EOI, status );
 
diff --git a/system/libarc32_arduino101/drivers/ss_gpio_iface.h b/system/libarc32_arduino101/drivers/ss_gpio_iface.h
index 86630387..58acfc7b 100644
--- a/system/libarc32_arduino101/drivers/ss_gpio_iface.h
+++ b/system/libarc32_arduino101/drivers/ss_gpio_iface.h
@@ -41,6 +41,7 @@
 
 #include "data_type.h"
 #include "gpio.h"
+#include "platform.h"
 
 #define SS_GPIO_8B0_BITS    (8)
 #define SS_GPIO_8B1_BITS    (8)
diff --git a/system/libarc32_arduino101/drivers/uart.h b/system/libarc32_arduino101/drivers/uart.h
index ed737780..456c244b 100644
--- a/system/libarc32_arduino101/drivers/uart.h
+++ b/system/libarc32_arduino101/drivers/uart.h
@@ -54,6 +54,9 @@ extern "C" {
 /* options for uart init */
 #define UART_OPTION_AFCE 0x01
 
+/* Size of the FIFO in bytes */
+#define UART_FIFO_SIZE 16
+
 /* generic UART info structure */
 struct uart_init_info {
 	int baud_rate;
@@ -92,6 +95,7 @@ int uart_break_check(int port);
 void uart_break_send(int port, int delay);
 void uart_disable(int port);
 uint8_t uart_tx_complete(int which);
+uint8_t uart_tx_ready(int which);
 void uart_loop_enable(int which);
 void uart_loop_disable(int which);
 
diff --git a/system/libarc32_arduino101/framework/include/cfw/cfw.h b/system/libarc32_arduino101/framework/include/cfw/cfw.h
index 9c9f040f..311dcf7b 100644
--- a/system/libarc32_arduino101/framework/include/cfw/cfw.h
+++ b/system/libarc32_arduino101/framework/include/cfw/cfw.h
@@ -113,7 +113,7 @@ typedef struct svc_client_handle_ {
 	 * Passed in the conn field of struct cfw_message for request messages
 	 */
 	void * server_handle;
-} svc_client_handle_t;
+} svc_client_handle_t, cfw_service_conn_t;
 
 struct cfw_message * cfw_alloc_message(int size, OS_ERR_TYPE * err);
 
diff --git a/system/libarc32_arduino101/framework/include/cfw/cfw_client.h b/system/libarc32_arduino101/framework/include/cfw/cfw_client.h
index 2a1ae2ba..2154208f 100644
--- a/system/libarc32_arduino101/framework/include/cfw/cfw_client.h
+++ b/system/libarc32_arduino101/framework/include/cfw/cfw_client.h
@@ -40,22 +40,6 @@
  * @{
  */
 
-/**
- * Create a handle to the component framework.
- * This handle is to be used for all other requests
- * to the component framework
- *
- * Implementation is different in the master and the slave contexts.
- * The master context will be pseudo-synchronous, while the slave
- * implementation will actually pass a message to the master context
- * in order to register a new client.
- *
- * \param queue pointer to service queue
- * \param cb the callback that will be called for each message reception
- * \param param the param passed along with the message to the callback
- */
-cfw_handle_t cfw_init(void * queue, handle_msg_cb_t cb, void * param);
-
 
 /**
  * Allocate a request message for a service.
diff --git a/system/libarc32_arduino101/framework/include/cfw_platform.h b/system/libarc32_arduino101/framework/include/cfw_platform.h
index 4e361961..aa5cb29e 100644
--- a/system/libarc32_arduino101/framework/include/cfw_platform.h
+++ b/system/libarc32_arduino101/framework/include/cfw_platform.h
@@ -39,7 +39,6 @@ extern "C" {
 #endif
 
 void    cfw_platform_init(void);
-void    cfw_platform_nordic_init(void);
 T_QUEUE cfw_get_service_queue(void);
 
 #ifdef __cplusplus
diff --git a/system/libarc32_arduino101/framework/include/infra/ipc_uart.h b/system/libarc32_arduino101/framework/include/infra/ipc_uart.h
index 5f5cbe76..6c8eab4f 100644
--- a/system/libarc32_arduino101/framework/include/infra/ipc_uart.h
+++ b/system/libarc32_arduino101/framework/include/infra/ipc_uart.h
@@ -78,25 +78,6 @@
 /* optional sync frame payload */
 #define SYNC_FRAME_DATA(_frame_) ((unsigned char *)&(_frame_)[20])
 
-#define IPC_CHANNEL_STATE_CLOSED 0
-#define IPC_CHANNEL_STATE_OPEN 1
-
-#define IPC_UART_MAX_CHANNEL 4
-
-struct ipc_uart_channels {
-	uint16_t index;
-	uint16_t state;
-	void (*cb) (uint8_t cpu_id, int chan, int len, void * data);
-};
-
-void * uart_ipc_channel_open(int channel, void(*cb)(uint8_t cpu_id, int chan, int len, void * data));
-int uart_ipc_send_message(void * handle, int len, void *p_data);
-void uart_ipc_set_channel(void * ipc_channel);
-void * uart_ipc_get_channel(void);
-int uart_ipc_send_sync_resp(int channel, int request_id, int param1, int param2, void * ptr);
-void uart_ipc_init(int num);
-void uart_ipc_disable(int num);
-
 /** @} */
 
 #endif /* _IPC_UART_H_ */
diff --git a/system/libarc32_arduino101/framework/include/infra/log.h b/system/libarc32_arduino101/framework/include/infra/log.h
index 528b0d99..07848310 100644
--- a/system/libarc32_arduino101/framework/include/infra/log.h
+++ b/system/libarc32_arduino101/framework/include/infra/log.h
@@ -36,6 +36,10 @@
 #include <infra/log_backend.h>
 #include <stdarg.h>
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /**
  * @defgroup infra_log Log
  * @ingroup infra
@@ -260,7 +264,11 @@ void log_resume();
  * @param module the ID of the log module related to this message
  * @param format the printf-like string format
  */
-#define pr_debug(module, format,...) pr_debug_ ## module(format, ##__VA_ARGS__)
+#define pr_debug(module, format,...) log_printk(LOG_LEVEL_DEBUG, module, format, ##__VA_ARGS__)
+
+#ifdef __cplusplus
+}
+#endif
 
 /** @} */
 
diff --git a/system/libarc32_arduino101/framework/include/infra/port.h b/system/libarc32_arduino101/framework/include/infra/port.h
index 8c7e52ff..e1c9ef5d 100644
--- a/system/libarc32_arduino101/framework/include/infra/port.h
+++ b/system/libarc32_arduino101/framework/include/infra/port.h
@@ -41,7 +41,7 @@
  * @{
  */
 
-#define MAX_PORTS       50
+#define MAX_PORTS       6
 
 
 #ifndef CONFIG_INFRA_IS_MASTER
diff --git a/system/libarc32_arduino101/framework/include/log_modules b/system/libarc32_arduino101/framework/include/log_modules
index c7246635..401bbae9 100644
--- a/system/libarc32_arduino101/framework/include/log_modules
+++ b/system/libarc32_arduino101/framework/include/log_modules
@@ -25,3 +25,4 @@ DEFINE_LOGGER_MODULE(LOG_MODULE_DRV, "DRV", 0)
 DEFINE_LOGGER_MODULE(LOG_MODULE_CUNIT, "CUNIT", 0)
 DEFINE_LOGGER_MODULE(LOG_MODULE_CFW, "CFW", 0)
 DEFINE_LOGGER_MODULE(LOG_MODULE_GPIO_SVC, "GPIO_SVC", 0)
+DEFINE_LOGGER_MODULE(LOG_MODULE_APP, "APP", 0)
diff --git a/system/libarc32_arduino101/framework/include/os/os.h b/system/libarc32_arduino101/framework/include/os/os.h
index 12b9a11a..7f5103d9 100644
--- a/system/libarc32_arduino101/framework/include/os/os.h
+++ b/system/libarc32_arduino101/framework/include/os/os.h
@@ -132,6 +132,15 @@ extern void queue_get_message (T_QUEUE queue, T_QUEUE_MESSAGE* message, int time
  */
 extern void queue_send_message(T_QUEUE queue, T_QUEUE_MESSAGE message, OS_ERR_TYPE* err );
 
+/**
+ * \brief Initialize the resources used by the framework's memory allocation services
+ *
+ * IMPORTANT : This function must be called during the initialization
+ *             of the OS abstraction layer.
+ *             This function shall only be called once after reset.
+ */
+extern void os_abstraction_init_malloc(void);
+
 /**
  * \brief Reserves a block of memory
  *
diff --git a/system/libarc32_arduino101/framework/include/panic_api.h b/system/libarc32_arduino101/framework/include/panic_api.h
index 70aea89c..5a579280 100644
--- a/system/libarc32_arduino101/framework/include/panic_api.h
+++ b/system/libarc32_arduino101/framework/include/panic_api.h
@@ -29,5 +29,5 @@
  */
 
 // TODO - replace with a proper implementation of panic()
-#define panic(x) _do_fault();
+//#define panic(x) _do_fault();
 #define force_panic() panic(-1)
diff --git a/system/libarc32_arduino101/framework/include/platform.h b/system/libarc32_arduino101/framework/include/platform.h
index 0bd3830b..801f87f5 100644
--- a/system/libarc32_arduino101/framework/include/platform.h
+++ b/system/libarc32_arduino101/framework/include/platform.h
@@ -39,12 +39,13 @@
 #define CPU_ID_HOST 3
 #define NUM_CPU     4
 
-#define SERIAL_BUFFER_SIZE 256
+#define CDCACM_BUFFER_SIZE 256
+#define SHARED_BUFFER_SIZE 64
 
 struct cdc_ring_buffer
 {
     /** Ring buffer data */
-    uint8_t data[SERIAL_BUFFER_SIZE];
+    uint8_t data[CDCACM_BUFFER_SIZE];
     /** Ring buffer head pointer, modified by producer */
     int head;
     /** Ring buffer head pointer, modified by consumer */
@@ -62,6 +63,30 @@ struct cdc_acm_shared_data {
     int device_open;
 };
 
+struct shared_ring_buffer
+{
+    /** Ring buffer data */
+    volatile uint8_t data[SHARED_BUFFER_SIZE];
+    /** Ring buffer head index, modified by producer */
+    volatile int head;
+    /** Ring buffer tail index, modified by consumer */
+    volatile int tail;
+
+    /** Buffer status
+     * 0 - locked by X86 core
+     * 1 - locked by arc core
+     * 2 - available to be taken by any core
+    **/
+    volatile int flag;
+};
+
+struct ipm_shared_data
+{
+    struct shared_ring_buffer *quark_buffer;
+    struct shared_ring_buffer *arc_buffer;
+};
+
+
 /**
  * LMT / ARC global shared structure. This structure lies in the beginning of
  * the RAM.
@@ -109,6 +134,30 @@ struct platform_shared_block_ {
      * The ARC core counts on QRK to find valid pointers in place.
      */
     struct cdc_acm_shared_data	* cdc_acm_buffers;
+    
+    struct cdc_acm_shared_data cdc_acm_buffers_obj;
+
+    struct cdc_ring_buffer cdc_acm_shared_rx_buffer;
+    struct cdc_ring_buffer cdc_acm_shared_tx_buffer;
+    
+    struct ipm_shared_data *ipm_shared_data_ptr;
+
+    struct ipm_shared_data ipm_shared_data_obj;
+
+    struct shared_ring_buffer quark_to_ARC;
+    struct shared_ring_buffer ARC_to_quark;
+    
+    uint32_t arc_cpu_context[33];
+    
+    uint32_t pm_status;
+    
+    void* arc_restore_addr;
+    
+    void* quark_restore_addr;
+    
+    uint32_t pm_int_status;
+    
+    uint8_t error_code;
 };
 
 #define RAM_START           0xA8000000
diff --git a/system/libarc32_arduino101/framework/include/services/ble/ble_service.h b/system/libarc32_arduino101/framework/include/services/ble/ble_service.h
index fa14941b..8917e61c 100644
--- a/system/libarc32_arduino101/framework/include/services/ble/ble_service.h
+++ b/system/libarc32_arduino101/framework/include/services/ble/ble_service.h
@@ -33,29 +33,205 @@
 
 #include <stdint.h>
 
+/* For MSG_ID_BLE_SERVICE_BASE */
+#include "services/services_ids.h"
+
+/* For bt_uuid  */
+#include "bluetooth/gatt.h"
+
+#include "bluetooth/bluetooth.h"
+
+#include "cfw/cfw.h"
+
+/* Forward declarations */
+struct _ble_service_cb;
+struct bt_conn;
+
 /**
- * @addtogroup ble_service
+ * @cond
+ * @defgroup ble_service BLE Service
+ *
+ * Bluetooth Low Energy (BLE) application level service.
+ *
+ * This service provides BLE service, abstracting most of the complexity of the underlying BLE services/profiles.
+ *
+ * @ingroup services
+ *
  * @{
+ */
+
+/*
+ * CFW Message ID base definitions for BLE services.
+ */
+
+/* BLE Service Message ID definitions. */
+#define MSG_ID_BLE_SERVICE_RSP                      (MSG_ID_BLE_SERVICE_BASE + 0x40)
+#define MSG_ID_BLE_SERVICE_EVT                      (MSG_ID_BLE_SERVICE_BASE + 0x80)
+
+/** BLE High level Message IDs used for request, response, events. */
+enum BLE_MSG_ID {
+	MSG_ID_BLE_ENABLE_RSP = MSG_ID_BLE_SERVICE_RSP, /**< Message ID for <i>enable</i> response, of type @ref ble_enable_rsp */
+	MSG_ID_BLE_INIT_SVC_RSP,                /**< Message ID for <i>init service</i> response, of type @ref ble_init_svc_rsp */
+
+	/* BLE direct test mode command */
+	MSG_ID_BLE_DBG_RSP,                     /**< Message ID for <i>DTM command</i> response, of type @ref ble_dbg_req_rsp */
+
+	MSG_ID_BLE_SERVICE_RSP_LAST,
+
+	/* events */
+	MSG_ID_BLE_ADV_TO_EVT = MSG_ID_BLE_SERVICE_EVT,           /**< Message ID for struct @ref ble_adv_to_evt */
+	MSG_ID_BLE_SERVICE_EVT_LAST
+};
+
+/** Macro to convert milliseconds to a specific unit */
+#define MSEC_TO_1_25_MS_UNITS(TIME) (((TIME) * 1000) / 1250)
+#define MSEC_TO_10_MS_UNITS(TIME) ((TIME) / 10)
+
+#define BLE_GAP_SEC_RAND_LEN    8		/**< Random Security number length (64 bits) */
+#define BLE_GAP_SEC_MAX_KEY_LEN 16		/**< Maximum security key len (LTK, CSRK) */
+
+/**
+ * Advertisement options.
+ */
+enum BLE_GAP_ADV_OPTIONS {
+	BLE_GAP_OPT_ADV_DEFAULT = 0,		/**< no specific option */
+	BLE_GAP_OPT_ADV_WHITE_LISTED = 0x02	/**< use white list and only report whitelisted devices */
+};
+
+/**
+ * LE security modes.
  *
+ * see BT spec  PART C, 10.2
+ *
+ * - Security mode 1
+ *   - Level 1: No security at all (service may use data signing)
+ *   - Level 2: Unauthenticated (no MITM protection pairing with encryption
+ *   - Level 3: Authenticated (MITM protection) pairing with encryption
+ *   - Level 4: Authenticated (MITM protection) LE Secure Connection wi
+ *
+ * - Security mode 2 (data signing)
+ *   - Level 1: Unauthenticated pairing with data signing
+ *   - Level 2: Authenticated (MITM protection) with data signing
  */
+enum BLE_GAP_SEC_MODES {
+	GAP_SEC_NO_PERMISSION = 0,	/**< No access permitted. */
+	GAP_SEC_LEVEL_1,
+	GAP_SEC_LEVEL_2,
+	GAP_SEC_LEVEL_3,
+	GAP_SEC_LEVEL_4,
+	GAP_SEC_MODE_1 = 0x10,
+	GAP_SEC_MODE_2 = 0x20		/**< only used for data signing, level 1 or 2 */
+};
 
-/** BLE response/event status codes. */
-enum BLE_STATUS {
-	BLE_STATUS_SUCCESS = 0, /**< General BLE Success code */
-	BLE_STATUS_PENDING, /**< Request received and execution started, response pending */
-	BLE_STATUS_TIMEOUT, /**< Request timed out */
-	BLE_STATUS_NOT_SUPPORTED, /**< Request/feature/parameter not supported */
-	BLE_STATUS_NOT_ALLOWED, /**< Request not allowed */
-	BLE_STATUS_LINK_TIMEOUT, /**< Link timeout (link loss) */
-	BLE_STATUS_NOT_ENABLED, /**< BLE not enabled, @ref ble_enable */
-	BLE_STATUS_ERROR,	/**< Generic Error */
-	BLE_STATUS_ALREADY_REGISTERED, /**< BLE service already registered */
-	BLE_STATUS_WRONG_STATE, /**< Wrong state for request */
-	BLE_STATUS_ERROR_PARAMETER, /**< Parameter in request is wrong */
-	BLE_STATUS_GAP_BASE = 0x100, /**< GAP specific error base */
-	BLE_STATUS_GATT_BASE = 0x200, /**< GATT specific Error base */
+/**
+ * Security manager passkey type.
+ */
+enum BLE_GAP_SM_PASSKEY_TYPE {
+	BLE_GAP_SM_PK_NONE = 0, /**< No key (may be used to reject). */
+	BLE_GAP_SM_PK_PASSKEY,  /**< Security data is a 6-digit passkey. */
+	BLE_GAP_SM_PK_OOB,         /**< Security data is 16 bytes of OOB data */
 };
 
-typedef uint16_t ble_status_t; /**< Response and event BLE service status type @ref BLE_STATUS */
+/**
+ * Connection Parameter update request event.
+ */
+struct ble_gap_conn_param_update_req_evt {
+	struct bt_le_conn_param param;
+};
+
+/* -  BLE_SERVICE_GAP_API.H */
+
+
+/** Generic BLE status response message. */
+struct ble_rsp {
+	struct cfw_message header;	/**< Component framework message header (@ref cfw), MUST be first element of structure */
+	int status;			/**< Response status */
+};
+
+/** Generic BLE response with connection reference and status. */
+struct ble_conn_rsp {
+	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
+	struct bt_conn *conn;      /**< Connection reference */
+	int status;                /**< Status */
+};
+
+/** BLE Enable configuration options. */
+struct ble_enable_config {
+	bt_addr_le_t * p_bda;  /**< Optional BT device address. If NULL, internal unique static random will be used */
+	struct bt_le_conn_param central_conn_params;    /**< Central supported range */
+};
+
+/** Parameters of MSG_ID_BLE_ENABLE_RSP. */
+struct ble_enable_rsp {
+	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
+	int status;                /**< Response status */
+	uint8_t enable;            /**< Enable state: 0:Disabled, 1:Enabled */
+	bt_addr_le_t bd_addr;
+};
+
+/**
+ * Attribute handle range definition.
+ */
+struct ble_gatt_handle_range {
+	uint16_t start_handle;
+	uint16_t end_handle;
+};
+
+/** Parameters of the current connection. */
+struct ble_connection_values {
+	uint16_t interval;		/**< Connection interval (unit 1.25 ms) */
+	uint16_t latency;		/**< Connection latency (unit interval) */
+	uint16_t supervision_to;	/**< Connection supervision timeout (unit 10ms)*/
+};
+
+/** Parameters for @ref MSG_ID_BLE_INIT_SVC_RSP. */
+struct ble_init_svc_rsp {
+	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
+	int status;
+};
+
+/** Authentication data. */
+struct ble_auth_data {
+	union {
+		uint8_t passkey[6];   /**< 6 digit key (000000 - 999999) */
+		uint8_t obb_data[16]; /**< 16 byte of OBB data */
+	};
+	uint8_t type;                 /**< @ref BLE_GAP_SM_PASSKEY_TYPE */
+};
+
+/** Parameters for @ref MSG_ID_BLE_ADV_TO_EVT. */
+struct ble_adv_to_evt {
+	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
+};
+
+/**
+ * BLE debug rsp message.
+ */
+
+/*
+ * BLE debug req message.
+ */
+struct ble_dbg_req_rsp {
+	struct cfw_message header;
+	uint32_t u0;
+	uint32_t u1;
+};
+
+/** Enable/Disable BLE stack. To be called before any BLE service related call.
+ *
+ * @param p_service_conn client service connection (cfw service connection)
+ * @param enable 1: enable BLE stack 0: disable BLE stack
+ * @param p_config configuration parameters when enabling BLE. shall be null in case of BLE disable. @ref ble_enable_config
+ * @param p_priv pointer to private structure returned in a response
+ *
+ * @return @ref OS_ERR_TYPE
+ * @note Expected notification:
+ *       - Message with @ref MSG_ID_BLE_ENABLE_RSP and type @ref ble_enable_rsp.
+ */
+int ble_service_enable(cfw_service_conn_t * p_service_conn, uint8_t enable,
+			const struct ble_enable_config * p_config,
+			void *p_priv);
+
+/** @endcond */
 /** @}*/
 #endif
diff --git a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gap_api.h b/system/libarc32_arduino101/framework/include/services/ble/ble_service_gap_api.h
deleted file mode 100644
index 6fd62cec..00000000
--- a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gap_api.h
+++ /dev/null
@@ -1,1036 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __BLE_SERVICE_GAP_H__
-#define __BLE_SERVICE_GAP_H__
-
-#include "cfw/cfw.h"
-#include "cfw/cfw_client.h"
-#include "infra/version.h"
-#include "ble_service_msg.h"
-#include "ble_service.h"
-
-/**
- * @defgroup ble_core_service BLE Core Service GAP/GATT APIs
- *
- * BLE Core service GAP/GATT APIs used by BLE service.
- *
- * @ingroup ble_service
- * @{
- */
-
-/** Macro to convert milliseconds to a specific unit */
-#define MSEC_TO_0_625_MS_UNITS(TIME) (((TIME) * 1000) / 625)
-#define MSEC_TO_1_25_MS_UNITS(TIME) (((TIME) * 1000) / 1250)
-#define MSEC_TO_10_MS_UNITS(TIME) ((TIME) / 10)
-
-/**
- * BLE GAP Status return codes.
- */
-enum BLE_SVC_GAP_STATUS_CODES {
-	BLE_SVC_GAP_STATUS_SUCCESS = BLE_STATUS_SUCCESS, /**< GAP success */
-	BLE_SVC_GAP_STATUS_ERROR = BLE_STATUS_GATT_BASE, /**< Generic GAP error */
-	BLE_SVC_GAP_STATUS_INVALID_UUID_LIST, /**< Invalid UUID list provided (e.g. advertisement) */
-	/* TODO: add more status codes */
-};
-
-/**
- * BLE GAP addr types.
- *
- * BLE GAP supported address types
- */
-enum BLE_ADDR_TYPES {
-	BLE_ADDR_PUBLIC = 0, /**< BD address assigned by IEEE */
-	BLE_ADDR_PRIVATE_RANDOM_STATIC, /**< Random address */
-	BLE_ADDR_RRIVATE_RANDOM_PRIVATE_RESOLVABLE, /**< Resolvable Private Random address */
-	BLE_ADDR_PRIVATE_RANDOM_PRIVATE_NONRESOLVABLE /**< Non-resolvable Private Random address */
-};
-
-/**
- * BT/BLE address Length.
- */
-#define BLE_ADDR_LEN 6
-
-#define BLE_GAP_SEC_RAND_LEN    8	/**< Random Security number length (64 bits) */
-#define BLE_GAP_SEC_MAX_KEY_LEN 16	/**< Maximum security key len (LTK, CSRK) */
-
-#define BLE_SVC_GAP_HANDLE_INVALID 0xffff /**< Invalid GAP connection handle */
-
-/**
- * Device GAP name characteristic write permission.
- *
- * If the characteristic shall be writable, use a combination of the values
- * defined in @ref BLE_GAP_SEC_MODES
- */
-#define BLE_DEVICE_NAME_WRITE_PERM GAP_SEC_NO_PERMISSION
-
-typedef struct {
-	uint8_t type;		/**< BLE Address type @ref BLE_ADDR_TYPES */
-	uint8_t addr[BLE_ADDR_LEN];
-				/**< BD address, little endian format */
-} ble_addr_t;
-
-/**
- * GAP device roles.
- */
-enum BLE_ROLES {
-	BLE_ROLE_INVALID = 0,
-	BLE_ROLE_PERIPHERAL = 0x01,
-	BLE_ROLE_CENTRAL = 0x02
-};
-
-typedef uint8_t ble_role_t;
-
-/**
- * BLE core (GAP, GATT) Message IDs used for request, response, events and indications.
- */
-enum BLE_GAP_MSG_ID {
-	MSG_ID_BLE_GAP_WR_CONF_REQ = MSG_ID_BLE_GAP_BASE,
-	MSG_ID_BLE_GAP_RD_BDA_REQ,
-	MSG_ID_BLE_GAP_WR_ADV_DATA_REQ,
-	MSG_ID_BLE_GAP_WR_WHITE_LIST_REQ,
-	MSG_ID_BLE_GAP_CLR_WHITE_LIST_REQ,
-	MSG_ID_BLE_GAP_ENABLE_ADV_REQ,
-	MSG_ID_BLE_GAP_DISABLE_ADV_REQ,
-	MSG_ID_BLE_GAP_CONN_UPDATE_REQ,
-	MSG_ID_BLE_GAP_DISCONNECT_REQ,
-	MSG_ID_BLE_GAP_SERVICE_WRITE_REQ,
-	MSG_ID_BLE_GAP_SERVICE_READ_REQ,
-	MSG_ID_BLE_GAP_SM_CONFIG_REQ,
-	MSG_ID_BLE_GAP_SM_PAIRING_REQ,
-	MSG_ID_BLE_GAP_SM_PASSKEY_REQ,
-	MSG_ID_BLE_GAP_SET_RSSI_REPORT_REQ,
-	MSG_ID_BLE_GAP_SCAN_START_REQ,
-	MSG_ID_BLE_GAP_SCAN_STOP_REQ,
-	MSG_ID_BLE_GAP_CONNECT_REQ,
-	MSG_ID_BLE_GAP_CONNECT_CANCEL_REQ,
-	MSG_ID_BLE_GAP_SET_OPTIONS_REQ,
-	MSG_ID_BLE_GAP_GENERIC_CMD_REQ,
-	MSG_ID_BLE_GAP_GET_VERSION_REQ,
-	MSG_ID_BLE_GAP_DTM_INIT_REQ,
-	MSG_ID_BLE_CTRL_LOG_REQ,
-	MSG_ID_BLE_GAP_REQ_LAST,
-
-	/** BLE GAP Response Messages IDs. */
-	MSG_ID_BLE_GAP_WR_CONF_RSP = MSG_ID_BLE_GAP_RSP,
-						    /**< Write controller config: own Bluetooth Device Address, tx power */
-	MSG_ID_BLE_GAP_RD_BDA_RSP,		    /**< Read own Bluetooth Device Address */
-	MSG_ID_BLE_GAP_WR_ADV_DATA_RSP,		    /**< Write Advertising Data and Scan response data */
-	MSG_ID_BLE_GAP_WR_WHITE_LIST_RSP,	    /**< Write white list to controller */
-	MSG_ID_BLE_GAP_CLR_WHITE_LIST_RSP,	    /**< Clear current white list */
-	MSG_ID_BLE_GAP_ENABLE_ADV_RSP,		    /**< Enable Advertising */
-	MSG_ID_BLE_GAP_DISABLE_ADV_RSP,		    /**< Disable Advertising */
-	MSG_ID_BLE_GAP_CONN_UPDATE_RSP,		    /**< Update Connection */
-	MSG_ID_BLE_GAP_DISCONNECT_RSP,		    /**< Disconnect */
-	MSG_ID_BLE_GAP_SERVICE_WRITE_RSP,	    /**< Write GAP Service specific like device name, appearance and PPCPparameters */
-	MSG_ID_BLE_GAP_SERVICE_READ_RSP,	    /**< Read GAP Service specific like device name, appearance and PPCPparameters */
-	MSG_ID_BLE_GAP_SM_CONFIG_RSP, /**< Response to @ref ble_gap_sm_config */
-	MSG_ID_BLE_GAP_SM_PAIRING_RSP, /**< Response to @ref ble_gap_sm_pairing_req */
-	MSG_ID_BLE_GAP_SM_PASSKEY_RSP, /**< Response to @ref ble_gap_sm_passkey_reply */
-	MSG_ID_BLE_GAP_SET_RSSI_REPORT_RSP,	    /**< Enable/Disable reporting of changes in RSSI */
-	MSG_ID_BLE_GAP_SCAN_START_RSP,		    /**< Start Scanning */
-	MSG_ID_BLE_GAP_SCAN_STOP_RSP,		    /**< Stop Scanning */
-	MSG_ID_BLE_GAP_CONNECT_RSP,		    /**< Start Connection procedure */
-	MSG_ID_BLE_GAP_CONNECT_CANCEL_RSP,	    /**< Cancel ongoing connection procedure */
-	MSG_ID_BLE_GAP_SET_OPTIONS_RSP,		    /**< Set gap options (e.g. co-ex, master/central role) */
-	MSG_ID_BLE_GAP_GENERIC_CMD_RSP,		    /**< Generic non connection related requests */
-	MSG_ID_BLE_GAP_GET_VERSION_RSP,
-	MSG_ID_BLE_GAP_DTM_INIT_RSP,
-	MSG_ID_BLE_CTRL_LOG_RSP, /**< BLE controller logging message */
-	MSG_ID_BLE_GAP_RSP_LAST,
-
-	/** GAP related events. */
-	MSG_ID_BLE_GAP_CONNECT_EVT = MSG_ID_BLE_GAP_EVT, /**< Connection established */
-	MSG_ID_BLE_GAP_DISCONNECT_EVT,		    /**< Disconnect from peer */
-	MSG_ID_BLE_GAP_CONN_UPDATE_EVT,	    /**< Connection Parameters update event (in central, they have been updated, in peripheral, also includes the status of the request) */
-	MSG_ID_BLE_GAP_SM_PAIRING_STATUS_EVT, /**< Pairing request status event */
-	MSG_ID_BLE_GAP_SM_PASSKEY_REQ_EVT, /**< Pairing passkey request (6 digits or 16 byte OOB data) */
-	MSG_ID_BLE_GAP_TO_EVT,			    /**< GAP Timeout event */
-	MSG_ID_BLE_GAP_ADV_DATA_EVT,		    /**< Advertising raw data event (central role) */
-	MSG_ID_BLE_GAP_RSSI_EVT,		    /**< Signal strength change event */
-	MSG_ID_BLE_GAP_GENERIC_CMD_EVT,		    /**< Generic command request event */
-	MSG_ID_BLE_CTRL_LOG_EVT,		    /**< BLE Controller Logging Events */
-	MSG_ID_BLE_GAP_EVT_LAST,
-};
-
-/**
- * Generic BLE Status Response.
- * Short status response for commands not returning any additional data
- */
-struct ble_rsp {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< Response status @ref BLE_STATUS */
-};
-
-/**
- * Connection requested parameters.
- */
-struct ble_gap_connection_params {
-	uint16_t interval_min;	    /**< minimal connection interval: range 0x0006 to 0x0c80 (unit 1.25ms) */
-	uint16_t interval_max;	    /**< maximum connection interval: range 0x0006 to 0x0c80 must be bigger then min! */
-	uint16_t slave_latency;	    /**< maximum connection slave latency: 0x0000 to 0x01f3 */
-	uint16_t link_sup_to;	    /**< link supervision timeout: 0x000a to 0x0c80 (unit 10ms) */
-};
-
-/**
- * Connection values.
- */
-struct ble_gap_connection_values {
-	uint16_t interval; /**< Connection interval (unit 1.25 ms) */
-	uint16_t latency; /**< Connection latency (unit interval) */
-	uint16_t supervision_to; /**< Connection supervision timeout (unit 10ms)*/
-};
-
-/**
- * Initial GAP configuration
- */
-struct ble_wr_config {
-	ble_addr_t *p_bda;
-	uint8_t *p_name; /**< GAP Device name, NULL terminated! */
-	uint16_t appearance; /**< see BLE spec */
-	int8_t tx_power;
-	struct ble_gap_connection_params peripheral_conn_params; /**< Peripheral preferred */
-	struct ble_gap_connection_params central_conn_params; /**< Central supported range */
-};
-
-/** Read BD address response. */
-typedef struct {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< Response status @ref BLE_STATUS */
-	ble_addr_t bd; /**< if status ok @ref ble_addr_t */
-} ble_bda_rd_rsp_t;
-
-struct ble_gap_adv_rsp_data {
-	uint8_t *p_data;		    /**< max 31 bytes! */
-	uint8_t len;
-};
-
-/**
- * Advertising types, see BT spec vol 6, Part B, chapter 2.3.
- */
-enum GAP_ADV_TYPES {
-	ADV_IND = 0x00,		    /**< Connectable undirected advertising */
-	ADV_DIRECT_IND = 0x01,	    /**< Connectable high duty cycle advertising */
-	ADV_NONCONN_IND = 0x02,	    /**< Non connectable undirected advertising */
-	ADV_SCAN_IND = 0x06,	    /**< Scannable undirected advertising */
-	ADV_SCAN_RSP = 0x81,	    /**< Scan response, only a return value in @ref ble_gap_adv_data_evt_t */
-	ADV_RESERVED		/* keep last */
-};
-
-typedef struct {
-	uint8_t irk[BLE_GAP_SEC_MAX_KEY_LEN];
-					    /**< Identity Resolving Key (IRK) */
-} ble_gap_irk_info_t;
-
-struct ble_gap_whitelist_info {
-	ble_addr_t **pp_bd;		/**< list of bd addresses */
-	ble_gap_irk_info_t **pp_key;	/**< list of irk keys (for address resolution offload) */
-	uint8_t bd_count;		/**< number of bd addresses */
-	uint8_t key_count;		/**< number of keys */
-};
-
-/**
- * Advertisement options.
- */
-enum BLE_GAP_ADV_OPTIONS {
-	BLE_GAP_OPT_ADV_DEFAULT = 0,	    /**< no specific option */
-	BLE_GAP_OPT_ADV_WHITE_LISTED = 0x02 /**< use white list and only report whitelisted devices */
-};
-
-/**
- * Advertisement parameters.
- */
-typedef struct {
-	uint16_t timeout;
-	uint16_t interval_min;	    /**< min interval 0xffff: use default 0x0800 */
-	uint16_t interval_max;	    /**< max interval 0xffff: use default 0x0800 */
-	uint8_t type;		    /**< advertisement types @ref GAP_ADV_TYPES */
-	uint8_t filter_policy;	    /**< filter policy to apply with white list */
-	ble_addr_t *p_peer_bda;	    /**< bd address of peer device in case of directed advertisement */
-	uint8_t options;	    /**< options see @ref BLE_GAP_ADV_OPTIONS (to be ORed) */
-} ble_gap_adv_param_t;
-
-/**
- * Generic BLE Status. Response
- * Short status response for commands not returning any additional data
- */
-typedef struct {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< Response status @ref BLE_STATUS */
-	uint32_t wl_handle; /**< reference handle. to be used for clearing it later */
-} ble_gap_wr_white_list_rsp_t;
-
-
-/**
- * Appearance read response message.
- */
-typedef struct {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< Response status @ref BLE_STATUS */
-	uint16_t uuid; /**< value of GAP appearance characteristic */
-} ble_rd_appearance_rsp_t;
-
-/**
- * LE security modes.
- *
- * see BT spec  PART C, 10.2
- *
- * - Security mode 1
- *   - Level 1: No security at all (service may use data signing)
- *   - Level 2: Unauthenticated (no MITM protection pairing with encryption
- *   - Level 3: Authenticated (MITM protection) pairing with encryption
- *   - Level 4: Authenticated (MITM protection) LE Secure Connection wi
- *
- * - Security mode 2 (data signing)
- *   - Level 1: Unauthenticated pairing with data signing
- *   - Level 2: Authenticated (MITM protection) with data signing
- */
-enum BLE_GAP_SEC_MODES {
-	GAP_SEC_NO_PERMISSION = 0, /**< No access permitted. */
-	GAP_SEC_LEVEL_1,
-	GAP_SEC_LEVEL_2,
-	GAP_SEC_LEVEL_3,
-	GAP_SEC_LEVEL_4,
-	GAP_SEC_MODE_1 = 0x10,
-	GAP_SEC_MODE_2 = 0x20	/**< only used for data signing, level 1 or 2 */
-};
-
-struct ble_gap_svc_local_name {
-	uint8_t sec_mode;	/**< security mode for writing device name, @ref BLE_GAP_SEC_MODES */
-	uint8_t authorization;	/**< 0: no authorization, 1: authorization required */
-	uint8_t len;		/**< device name length (0-248) */
-	const uint8_t *p_name;	/**< name to to write */
-};
-
-enum BLE_GAP_SVC_ATTR_TYPE {
-	GAP_SVC_ATTR_NAME = 0,		    /**< Device Name, UUID 0x2a00 */
-	GAP_SVC_ATTR_APPEARANCE,	    /**< Appearance, UUID 0x2a01 */
-	GAP_SVC_ATTR_PPCP = 4,		    /**< Peripheral Preferred Connection Parameters (PPCP), UUID 0x2a04 */
-	GAP_SVC_ATTR_CAR = 0xa6,	    /**< Central Address Resolution (CAR), UUID 0x2aa6, BT 4.2 */
-};
-
-struct ble_gap_service_write_params {
-	uint16_t attr_type;		      /**< GAP Characteristics attribute type  @ref BLE_GAP_SVC_ATTR_TYPE */
-	union {
-		struct ble_gap_svc_local_name name;
-		uint16_t appearance;	      /**< Appearance UUID */
-		struct ble_gap_connection_params conn_params;
-						 /**< Preferred Peripheral Connection Parameters */
-		uint8_t car;		      /**< Central Address Resolution support 0: no, 1: yes */
-	};
-};
-
-struct ble_gap_service_read_rsp {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< status of read operation @ref BLE_STATUS, in case failure union shall be empty */
-	uint16_t attr_type; /**< type of attribute returned (valid even in failure case! */
-	union {
-		struct ble_gap_svc_local_name name;
-		uint16_t appearance; /**< Appearance UUID */
-		struct ble_gap_connection_params conn_params; /**< Preferred Peripheral Connection Parameters */
-		uint8_t car; /** Central Address Resolution support 0: no, 1: yes */
-	};
-};
-
-/**
- * GAP security manager options for bonding/authentication procedures, see Vol 3: Part H, 3.5.
- */
-enum BLE_GAP_SM_OPTIONS {
-	BLE_GAP_BONDING = 0x01,		/**< SMP supports bonding */
-	BLE_GAP_MITM = 0x04,		/**< SMP requires Man In The Middle protection */
-	BLE_GAP_OOB = 0x08		/**< SMP supports Out Of Band data */
-};
-
-/**
- * IO capabilities, see Vol 3: PART H, 3.5.
- */
-enum BLE_GAP_IO_CAPABILITIES {
-	BLE_GAP_IO_DISPLAY_ONLY = 0,
-	BLE_GAP_IO_DISPLAY_YESNO = 1,
-	BLE_GAP_IO_KEYBOARD_ONLY = 2,
-	BLE_GAP_IO_NO_INPUT_NO_OUTPUT = 3,
-	BLE_GAP_IO_KEYBOARD_DISPLAY = 4
-};
-
-/**
- * Security manager configuration parameters.
- *
- * options and io_caps will define there will be a passkey request or not.
- * It is assumed that io_caps and options are compatible.
- */
-struct ble_gap_sm_config_params {
-	uint8_t options; /**< Security options (@ref BLE_GAP_SM_OPTIONS) */
-	uint8_t io_caps; /**< I/O Capabilities to allow passkey exchange (@ref BLE_GAP_IO_CAPABILITIES) */
-	uint8_t key_size; /**< Maximum encryption key size (7-16) */
-};
-
-/**
- * Security manager pairing parameters.
- */
-struct ble_gap_sm_pairing_params {
-	uint8_t auth_level; /**< authentication level see @ref BLE_GAP_SM_OPTIONS */
-};
-
-/**
- * Security manager passkey type.
- */
-enum BLE_GAP_SM_PASSKEY_TYPE {
-	BLE_GAP_SM_PASSKEY = 0, /**< Security data is a passkey. */
-	BLE_GAP_SM_OBB, /**< Security data is 16 bytes of OOB data */
-};
-/**
- * Security reply to incoming security request.
- */
-struct ble_gap_sm_passkey {
-	uint8_t type; /**< Security data type in this reply @ref BLE_GAP_SM_PASSKEY_TYPE */
-	union {
-		uint8_t passkey[6]; /**< 6 digits (string) */
-		uint8_t oob[16]; /**< 16 bytes of OOB security data */
-	};
-};
-
-/**
- * RSSI operation definition.
- */
-enum BLE_GAP_RSSI_OPS {
-	BLE_GAP_RSSI_DISABLE_REPORT = 0,
-	BLE_GAP_RSSI_ENABLE_REPORT
-};
-
-enum BLE_GAP_SCAN_OPTIONS {
-	BLE_GAP_SCAN_DEFAULT = 0,	    /**< no specific option */
-	BLE_GAP_SCAN_ACTIVE = 0x01,	    /**< do an active scan (request scan response */
-	BLE_GAP_SCAN_WHITE_LISTED = 0x02    /**< Use white list and only report whitelisted devices */
-};
-
-enum BLE_GAP_SET_OPTIONS {
-	BLE_GAP_SET_CH_MAP = 0,		    /**< Set channel map */
-};
-
-typedef struct {
-	uint16_t conn_handle;		    /**< connection on which to change channel map */
-	uint8_t map[5];			    /**< 37 bits are used of the 40 bits (LSB) */
-} ble_gap_channel_map_t;
-
-/**
- * GAP option data structure.
- */
-typedef union {
-	ble_gap_channel_map_t ch_map;	    /**< BLE channel map to set see BT spec */
-} ble_gap_option_t;
-
-/**
- * Scan parameters.
- *
- * @note Check BT core spec for high low duty cycle interval & window size!
- */
-typedef struct {
-	uint16_t timeout;		/**< scan timeout in s, 0 never */
-	uint16_t interval;		/**< interval: 0x4 - 0x4000. (unit: 0.625ms), use default: 0xffff (0x0010) */
-	uint16_t window;		/**< Window: 0x4 - 0x4000. (unit: 0.625ms), use default 0xffff (= 0x0010) */
-	uint8_t options;		/**< scan options, ORed options from @ref BLE_GAP_SCAN_OPTIONS */
-} ble_gap_scan_param_t;
-
-/**
- * Connect event @ref MSG_ID_BLE_GAP_CONNECT_EVT.
- */
-struct ble_gap_connect_evt {
-	struct ble_gap_connection_values conn_values; /**< Connection values */
-	uint8_t role; /**< role in this connection @ref */
-	ble_addr_t peer_bda; /**< address of peer device */
-};
-
-/**
- * Disconnect event @ref MSG_ID_BLE_GAP_DISCONNECT_EVT.
- */
-struct ble_gap_disconnected_evt {
-	uint8_t hci_reason; /**< HCI disconnect reason */
-};
-
-/**
- * Updated connection event @ref MSG_ID_BLE_GAP_CONN_UPDATE_EVT.
- */
-struct ble_gap_conn_update_evt {
-	struct ble_gap_connection_values conn_values;
-};
-
-/**
- * Security manager pairing status event @ref MSG_ID_BLE_GAP_SM_PAIRING_STATUS_EVT.
- */
-struct ble_gap_sm_pairing_status_evt {
-	uint16_t conn_handle;
-	uint16_t status;
-};
-
-/**
- * Security manager passkey request event @ref MSG_ID_BLE_GAP_SM_PASSKEY_REQ_EVT.
- */
-struct ble_gap_sm_passkey_req_evt {
-	uint8_t dummy;
-};
-
-/**
- * GAP/SMP security result status code.
- * see Vol 3: Part H, chapter 3.5.5.
- */
-enum BLE_GAP_SEC_RESULT_STATUS {
-	BLE_GAP_SEC_STATUS_SUCCESS = 0,
-				    /**< bonding/pairing completed successfully */
-	BLE_GAP_SEC_STATUS_PASSKEY_ENTRY_FAILED,/**< passkey entry failed */
-	BLE_GAP_SEC_STATUS_OOB_NOT_AVAILABLE,	/**< Out of Band data is not available */
-	BLE_GAP_SEC_STATUS_AUTH_REQUIREMENTS,	/**< Authentication requirements not met due to IO cap */
-	BLE_GAP_SEC_STATUS_CONFIRM_VALUE,	/**< Confirm value does not match calculated value */
-	BLE_GAP_SEC_STATUS_PAIRING_NOT_SUPPORTED,
-						/**< Pairing not supported by the device  */
-	BLE_GAP_SEC_STATUS_ENC_KEY_SIZE,	/**< Encryption key size insufficient */
-	BLE_GAP_SEC_STATUS_SMP_CMD_UNSUPPORTED,	/**< Unsupported SMP command on this device */
-	BLE_GAP_SEC_STATUS_UNSPECIFIED,		/**< Failure due to unspecified reason */
-	BLE_GAP_SEC_STATUS_REPEATED_ATTEMPTS,	/**< Pairing/authent disallowed due to too little time elapsed since last attempt */
-	BLE_GAP_SEC_STATUS_INVALID_PARAMS,	/**< Invalid parameters due to length or parameters */
-	/* 4.2 spec only ? */
-	BLE_GAP_SEC_STATUS_DHKEY_CHECK_FAILED,	/**< Remote device indicates that DHKey does not match local calculated key */
-	BLE_GAP_SEC_STATUS_NUMERIC_COMP_FAILED,	/**< values in numeric key comparison protocol do not match */
-	BLE_GAP_SEC_STATUS_BREDR_PAIRING_INPROGRESS,/**< Failure due to BR/EDR pairing request */
-	BLE_GAP_SEC_STATUS_CROSS_TSPRT_KEY_GEN_DIS,
-						/**< BR/EDR link key generation can not be use for LE keys handling */
-};
-
-enum BLE_SVC_GAP_TIMEOUT_REASON {
-	BLE_SVC_GAP_TO_ADV, /**< Advertisement Stopped. */
-	BLE_SVC_GAP_TO_SEC_REQ, /**< Security Request took too long. */
-	BLE_SVC_GAP_TO_SCAN, /**< Scanning stopped. */
-	BLE_SVC_GAP_TO_CONN, /**< Connection Link timeout. */
-};
-
-/**
- * GAP timeout event (e.g. protocol error) MSG_ID_BLE_GAP_TO_EVT.
- */
-struct ble_gap_timout_evt {
-	int reason;	    /**< reason for timeout @ref BLE_SVC_GAP_TIMEOUT_REASON */
-};
-
-/**
- * Advertisement data structure (central role) @ref MSG_ID_BLE_GAP_ADV_DATA_EVT.
- */
-struct ble_gap_adv_data_evt {
-	ble_addr_t remote_bda;	    /**< address of remote device */
-	int8_t rssi;		    /**< signal strength compared to 0 dBm */
-	uint8_t type;		    /**< type of advertisement data or scan response @ref GAP_ADV_TYPES */
-	uint8_t len;		    /**< length of advertisement data or scap response data */
-	uint8_t data[];		    /**< Advertisement or scan response data */
-};
-
-/**
- * Connection Parameter update request event @ref MSG_ID_BLE_GAP_CONN_PARAM_UPDATE_REQ_EVT.
- *
- * @note reply with @ref ble_gap_conn_update_params
- */
-struct ble_gap_conn_param_update_req_evt {
-	struct ble_gap_connection_params param;
-};
-
-/**
- * RSSI signal strength event @ref MSG_ID_BLE_GAP_RSSI_EVT.
- */
-struct ble_gap_rssi_evt {
-	int8_t rssi_lvl;	/**< RSSI level (compared to 0 dBm) */
-};
-
-/**
- * RSSI report parameters @ref MSG_ID_BLE_GAP_SET_RSSI_REPORT_REQ.
- */
-struct rssi_report_params {
-	uint16_t conn_hdl;	/**< Connection handle */
-	uint8_t op;	/**< RSSI operation @ref BLE_GAP_RSSI_OPS */
-	uint8_t delta_dBm;	/**< minimum RSSI dBm change to report a new RSSI value */
-	uint8_t min_count;	/**< number of delta_dBm changes before sending a new RSSI report */
-};
-
-/** Test Mode opcodes. */
-enum TEST_OPCODE {
-	BLE_TEST_INIT_DTM = 0x01,	/**< Put BLE controller in HCI UART DTM test mode */
-	BLE_TEST_START_DTM_RX = 0x1d,	/**< LE receiver test HCI opcode */
-	BLE_TEST_START_DTM_TX = 0x1e,	/**< LE transmitter test HCI opcode */
-	BLE_TEST_END_DTM = 0x1f,	/**< End LE DTM TEST */
-	/* vendor specific commands start at 0x80 */
-	BLE_TEST_SET_TXPOWER = 0x80,	/**< Set Tx power. To be called before start of tx test */
-	BLE_TEST_START_TX_CARRIER,	/**< Start Tx Carrier Test */
-};
-
-/**
- * Rx direct test mode data structure.
- */
-struct ble_dtm_rx_test {
-	uint8_t freq;		    /**< rf channel 0x00 - 0x27, resulting F = 2402 MHz + [freq * 2 MHz] */
-};
-
-/**
- * Tx direct test mode data structure
- */
-struct ble_dtm_tx_test {
-	uint8_t freq;		    /**< rf channel 0x00 - 0x27 where resulting F = 2402 + [freq * 2 MHz] */
-	uint8_t len;		    /**< length of test data payload for each packet */
-	uint8_t pattern;	    /**< packet payload pattern type, 0x00 - 0x02 mandatory */
-};
-
-/**
- * Tx power settings data structure.
- */
-struct ble_set_txpower {
-	int8_t dbm;		    /**< Tx power level to set (e.g. -30: -30 dBm). Depends on BLE Controller */
-};
-
-/**
- * RX test result data.
- */
-struct ble_dtm_test_result {
-	uint16_t mode;
-	uint16_t nb;
-};
-
-/**
- * Direct Test mode command params
- */
-struct ble_test_cmd {
-	uint8_t mode;		/**< test mode to execute @ref TEST_OPCODE */
-	union {
-		struct ble_dtm_rx_test rx;
-		struct ble_dtm_tx_test tx;
-		struct ble_set_txpower tx_pwr;	/**< Tx power to use for Tx tests. */
-	};
-};
-
-/**
- * BLE GAP event structure.
- */
-struct  ble_gap_event {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t conn_handle; /**< connection handle */
-	union {
-		struct ble_gap_connect_evt connected;       /**< connected event parameters */
-		struct ble_gap_disconnected_evt disconnected;    /**< disconnected reason */
-		struct ble_gap_conn_update_evt conn_updated;     /**< connection updated */
-		struct ble_gap_sm_pairing_status_evt sm_pairing_status; /**< Security Manager pairing status */
-		struct ble_gap_sm_passkey_req_evt sm_passkey_req;  /**< Security Manager passkey request */
-							    /**< connection related security update */
-		struct ble_gap_timout_evt timeout;		    /**< gap timeout occurred */
-		struct ble_gap_adv_data_evt adv_data;	    /**< advertisement data */
-		struct ble_gap_conn_param_update_req_evt conn_param_req;
-							    /**< update request from remote for connection parameters */
-		struct ble_gap_rssi_evt rssi;		    /**< new rssi level if rssi reporting is enabled */
-	};
-};
-
-/** Generic request op codes.
- * This allows to access some non connection related commands like DTM.
- */
-enum BLE_GAP_GEN_OPS {
-	DUMMY_VALUE = 0,		    /**< Not used now. */
-};
-
-/**
- * Generic command parameters.
- *
- * @note Independent of connection!
- */
-struct ble_gap_gen_cmd_params {
-	uint8_t op_code; /**< @ref BLE_GAP_GEN_OPS */
-};
-
-struct ble_version_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	struct version_header version; /**< Nordic version header */
-};
-
-struct ble_dtm_init_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-};
-
-struct ble_dtm_result_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	struct ble_dtm_test_result result; /**< Result data of DTM RX test */
-};
-
-/**
- * Generic request message response or event.
- */
-struct ble_generic_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	uint8_t op_code; /**< Opcode to which this message is applicable @ref BLE_GAP_GEN_OPS */
-};
-
-/**
- * Set Enable configuration parameters (BD address, etc).
- *
- * This shall put the controller stack into a usable (enabled) state.
- * Hence this should be called first!
- *
- * @param p_svc_handle service handle
- * @param p_config     BLE write configuration
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG_ID_BLE_GAP_WR_CONF_RSP @ref ble_rsp, TODO: return maybe more info?
- */
-int ble_gap_set_enable_config(svc_client_handle_t * p_svc_handle,
-			 const struct ble_wr_config * p_config, void *p_priv);
-
-/**
- * Read BD address from Controller.
- *
- *
- * @param p_svc_handle service handle
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: @ref MSG_ID_BLE_GAP_RD_BDA_RSP @ref ble_bda_rd_rsp_t
- */
-int ble_gap_read_bda(svc_client_handle_t * p_svc_handle, void *p_priv);
-
-/**
- * Write Advertisement data to BLE controller.
- *
- * Store advertisement data in BLE controller. It needs to be done BEFORE starting advertisement
- *
- * @param p_svc_handle service handle
- * @param p_adv_data   adv data to store in BLE controller
- * @param p_scan_data  scan response data to store in controller, can be NULL
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_WR_ADV_DATA_RSP @ref ble_rsp
- */
-int ble_gap_wr_adv_data(svc_client_handle_t * p_svc_handle,
-			  const struct ble_gap_adv_rsp_data * p_adv_data,
-			  const struct ble_gap_adv_rsp_data * p_scan_data,
-			  void *p_priv);
-
-/**
- * Write white list to the BLE controller.
- *
- * Store white in BLE controller. It needs to be done BEFORE starting advertisement or
- * start scanning
- *
- * @param p_svc_handle service handle
- * @param p_white_list white list to store in the controller
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_WR_WHITE_LIST @ref ble_gap_wr_white_list_rsp_t
- */
-int ble_gap_wr_white_list(svc_client_handle_t * p_svc_handle,
-			  const struct ble_gap_whitelist_info * p_white_list,
-			  void *p_priv);
-
-/**
- * Clear previously stored white list.
- *
- * @param p_svc_handle service handle
- * @param wl_handle handle to the white list previously stored
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GAP_CLR_WHITE_LIST @ref ble_rsp
- */
-int ble_gap_clr_white_list(svc_client_handle_t * p_svc_handle,
-			   uint32_t wl_handle, void *p_priv);
-
-/**
- * Start advertising.
- *
- * @param p_svc_handle service handle
- * @param p_adv_param advertisement
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GAP_ENABLE_ADV @ref ble_rsp
- */
-int ble_gap_start_advertise(svc_client_handle_t * p_svc_handle,
-			    const ble_gap_adv_param_t * p_adv_param,
-			    void *p_priv);
-
-/**
- * Stop advertising.
- *
- * @param p_svc_handle service handle
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GAP_DISABLE_ADV @ref ble_rsp
- */
-int ble_gap_stop_advertise(svc_client_handle_t * p_svc_handle, void *p_priv);
-
-/**
- * Update connection.
- *
- * This function's behavior depends on the role of the connection:
- * - in peripheral mode, it sends an L2CAP signaling connection parameter
- *  update request based the values in @ref p_conn_param
- *  and the action can be taken by the central at link layer
- *  - in central mode, it will send a link layer command to change the
- *  connection values based on the values in @ref p_conn_param where the
- *  connection interval is interval_min
- *
- * When the connection is updated, the event @ref MSG_ID_BLE_GAP_CONN_UPDATE_EVT will
- * be received.
- *
- * @param conn_handle Connection handle
- * @param p_conn_param Connection parameters
- * @param p_priv pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: @ref MSG_ID_BLE_GAP_CONN_UPDATE_RSP @ref ble_rsp
- */
-int ble_gap_conn_update(svc_client_handle_t * p_svc_handle,
-			uint16_t conn_handle,
-			const struct ble_gap_connection_params * p_conn_param,
-			void *p_priv);
-
-/**
- * Disconnect connection (peripheral or central role).
- *
- * @param p_svc_handle service handle
- * @param conn_hhdl    connection to terminate
- * @param reason       HCI reason for connection termination, most often 0x16 (connection terminated by local host)
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GAP_DISCONNECT @ref ble_rsp, MSG_ID_BLE_GAP_DISCONNECT_EVT @ref ble_gap_disconnected_evt_t
- */
-int ble_gap_disconnect(svc_client_handle_t * p_svc_handle,
-		       uint16_t conn_hhdl, uint8_t reason,
-		       void *p_priv);
-/**
- * Write GAP Service Attribute Characteristics.
- *
- * @param p_svc_handle service handle
- * @param p_params data of characteristic to write
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GAP_SERVICE_WRITE_RSP @ref ble_rsp
- */
-int ble_gap_service_write(svc_client_handle_t * p_svc_handle,
-			  const struct ble_gap_service_write_params * p_params,
-			  void *p_priv);
-
-/**
- * Read GAP Service Characteristics.
- *
- * @param p_svc_handle service handle
- * @param type type of GAP service data characteristic to read @ref BLE_GAP_SVC_ATTR_TYPE
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_SERVICE_READ_RSP @ref ble_gap_service_read_rsp
- */
-int ble_gap_service_read(svc_client_handle_t * p_svc_handle,
-			 uint16_t type, void * p_priv);
-
-/**
- * Function for configuring the security manager.
- *
- * @param h Service client
- * @param p_params local authentication/bonding parameters
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- *
- * @note Upon completion of the procedure, the client will receive
- * a message @ref MSG_ID_BLE_GAP_SM_CONFIG_RSP
- */
-int ble_gap_sm_config(const svc_client_handle_t * h,
-		    const struct ble_gap_sm_config_params * p_params,
-		    void *p_priv);
-
-/**
- * Initiate the bonding procedure (central).
- *
- * @param h            Service client
- * @param conn_handle  connection on which bonding procedure is executed
- * @param p_params     local authentication/bonding parameters
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- *
- * @note Upon completion of the procedure, the client receives
- * @ref MSG_ID_BLE_GAP_SM_PAIRING_RSP
- */
-int ble_gap_sm_pairing_req(const svc_client_handle_t * h,
-		uint16_t conn_handle,
-		const struct ble_gap_sm_pairing_params * p_params,
-		void *p_priv);
-
-/**
- * Reply to an incoming passkey request event (@ref MSG_ID_BLE_GAP_SM_PASSKEY_REQ_EVT).
- *
- * @param p_svc_handle service handle
- * @param conn_handle  connection on which bonding is going on
- * @param p_params     bonding security reply
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- *
- * @note Upon completion of the procedure, the client receives
- * @ref MSG_ID_BLE_GAP_SM_PASSKEY_RSP
- */
-int ble_gap_sm_passkey_reply(svc_client_handle_t * p_svc_handle,
-			     uint16_t conn_handle,
-			     const struct ble_gap_sm_passkey * p_params,
-			     void *p_priv);
-
-/**
- * Enable disable the reporting of the RSSI value.
- *
- * @param p_svc_handle service handle
- * @param conf RSSI report parameters @ref MSG_ID_BLE_GAP_SET_RSSI_REPORT_REQ
- * @param p_priv  pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_SET_RSSI_REPORT_RSP @ref ble_rsp
- */
-int ble_gap_set_rssi_report(svc_client_handle_t * p_svc_handle,
-			    const struct rssi_report_params *params,
-			    void *p_priv);
-
-/**
- * Start scanning for BLE devices doing advertisement.
- *
- * @param p_svc_handle service handle
- * @param p_scan_params scan parameters to use @ref ble_gap_scan_param_t
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_SCAN_START_RSP @ref ble_rsp
- */
-int ble_gap_start_scan(svc_client_handle_t * p_svc_handle,
-		       const ble_gap_scan_param_t * p_scan_params,
-		       void *p_priv);
-
-/**
- * Stop scanning.
- *
- * @param p_svc_handle service handle
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_STOP_START_RSP @ref ble_rsp
- */
-int ble_gap_stop_scan(svc_client_handle_t * p_svc_handle, void *p_priv);
-
-/**
- * Connect to a Remote Device.
- *
- * @param p_svc_handle service handle
- * @param p_bd bd to connect to. shall be null if BLE_GAP_SCAN_WHITE_LISTED option is set in @ref ble_gap_scan_param_t
- * @param p_scan_params scan parameters
- * @param p_conn_params connection parameters
- * @param p_priv        pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_CONNECT_RSP @ref ble_rsp,
- * @return MSG: MSG_ID_BLE_GAP_CONNECT_EVT @ref ble_gap_connect_evt_t
- */
-int ble_gap_connect(svc_client_handle_t * p_svc_handle, const ble_addr_t * p_bd,
-		    const ble_gap_scan_param_t * p_scan_params,
-		    const struct ble_gap_connection_params * p_conn_params,
-		    void *p_priv);
-
-/**
- * Cancel an ongoing connection attempt.
- *
- * @param p_svc_handle service handle
- * @param p_bd bd      address of device for which the connection shall be canceled
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_CONNECT @ref ble_rsp
- */
-int ble_gap_cancel_connect(svc_client_handle_t * p_svc_handle,
-			   const ble_addr_t * p_bd, void *p_priv);
-
-/**
- * Set a gap option (channel map etc) on a connection.
- *
- * @param p_svc_handle service handle
- * @param op option to set @ref BLE_GAP_SET_OPTIONS
- * @param p_opt bd address of device for which the connection shall be canceled ble_gap_option_t
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_SET_OPTIONS @ref ble_rsp
- */
-int ble_gap_set_option(svc_client_handle_t * p_svc_handle, uint8_t op,
-		       const ble_gap_option_t * p_opt, void *p_priv);
-
-/**
- * Set a gap option (channel map etc) on a connection.
- *
- * @param p_svc_handle service handle
- * @param p_params bd address of device for which the connection shall be canceled ble_gap_option_t
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_GENERIC_CMD_RSP @ref ble_rsp or @ref ble_generic_msg
- */
-int ble_gap_generic_cmd_req(svc_client_handle_t * p_svc_handle,
-			    const struct ble_gap_gen_cmd_params *p_params,
-			    void *p_priv);
-
-/**
- * Get nordic version.
- *
- * @param p_svc_handle service handle
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_GET_VERSION_RSP @ref ble_rsp or @ref ble_generic_msg
- */
-int ble_gap_get_version_req(svc_client_handle_t * p_svc_handle,
-			    void *p_priv);
-
-/**
- * Init dtm mode.
- *
- * @param p_svc_handle service handle
- * @param p_params bd address of device for which the connection shall be canceled ble_gap_option_t
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GAP_DTM_INIT_RSP @ref ble_rsp or @ref ble_generic_msg
- */
-int ble_gap_dtm_init_req(svc_client_handle_t * p_svc_handle,
-			    void *p_priv);
-/** @} */
-
-#endif /* __BLE_SVC_API_H__ */
diff --git a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gatt.h b/system/libarc32_arduino101/framework/include/services/ble/ble_service_gatt.h
deleted file mode 100644
index 23c65b73..00000000
--- a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gatt.h
+++ /dev/null
@@ -1,325 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __BLE_SERVICE_GATT_H__
-#define __BLE_SERVICE_GATT_H__
-
-#include "ble_service.h"
-#include "ble_service_gap_api.h"
-
-/** GATT common definitions.
- *
- * @ingroup ble_core_service
- *
- * @addtogroup ble_core_service_gatt BLE core service common GATT definitions
- * @{
- */
-
-/**
- * GATT Success code and error codes.
- */
-enum BLE_SVC_GATT_STATUS_CODES {
-	BLE_SVC_GATT_STATUS_SUCCESS = BLE_STATUS_SUCCESS, /**< GATT success @ref BLE_STATUS_SUCCESS */
-	BLE_SVC_GATT_STATUS_ENCRYPTED_MITM = BLE_SVC_GATT_STATUS_SUCCESS,
-	BLE_SVC_GATT_STATUS_INVALID_HANDLE = BLE_STATUS_GATT_BASE + 0x01,/**< 0x01 see BT Spec Vol 3: Part F (ATT), chapter 3.4.1.1 */
-	BLE_SVC_GATT_STATUS_READ_NOT_PERMIT,
-	BLE_SVC_GATT_STATUS_WRITE_NOT_PERMIT,
-	BLE_SVC_GATT_STATUS_INVALID_PDU,
-	BLE_SVC_GATT_STATUS_INSUF_AUTHENTICATION,
-	BLE_SVC_GATT_STATUS_REQ_NOT_SUPPORTED,
-	BLE_SVC_GATT_STATUS_INVALID_OFFSET,
-	BLE_SVC_GATT_STATUS_INSUF_AUTHORIZATION,
-	BLE_SVC_GATT_STATUS_PREPARE_Q_FULL,
-	BLE_SVC_GATT_STATUS_NOT_FOUND,
-	BLE_SVC_GATT_STATUS_NOT_LONG,
-	BLE_SVC_GATT_STATUS_INSUF_KEY_SIZE,
-	BLE_SVC_GATT_STATUS_INVALID_ATTR_LEN,
-	BLE_SVC_GATT_STATUS_ERR_UNLIKELY,
-	BLE_SVC_GATT_STATUS_INSUF_ENCRYPTION,
-	BLE_SVC_GATT_STATUS_UNSUPPORT_GRP_TYPE,
-	BLE_SVC_GATT_STATUS_INSUF_RESOURCE,
-
-	/**< TODO: maybe be not needed, to be covered by generic GAP status */
-	BLE_SVC_GATT_STATUS_NO_RESOURCES = BLE_STATUS_GATT_BASE | 0x80,
-	BLE_SVC_GATT_STATUS_INTERNAL_ERROR,
-	BLE_SVC_GATT_STATUS_WRONG_STATE,
-	BLE_SVC_GATT_STATUS_DB_FULL,
-	BLE_SVC_GATT_STATUS_BUSY,
-	BLE_SVC_GATT_STATUS_ERROR,
-	BLE_SVC_GATT_STATUS_CMD_STARTED,
-	BLE_SVC_GATT_STATUS_ILLEGAL_PARAMETER,
-	BLE_SVC_GATT_STATUS_PENDING,
-	BLE_SVC_GATT_STATUS_AUTH_FAIL,
-	BLE_SVC_GATT_STATUS_MORE,
-	BLE_SVC_GATT_STATUS_INVALID_CFG,
-	BLE_SVC_GATT_STATUS_SERVICE_STARTED,
-	BLE_SVC_GATT_STATUS_ENCRYPTED_NO_MITM,
-	BLE_SVC_GATT_STATUS_NOT_ENCRYPTED,
-	BLE_SVC_GATT_STATUS_CONGESTED,
-};
-
-/**
- * GATT Server Message ID definitions.
- */
-enum BLE_GATTS_MSG_ID {
-	/**< GATT Server Requests */
-	MSG_ID_BLE_GATTS_ADD_SERVICE_REQ = MSG_ID_BLE_GAP_REQ_LAST,
-	MSG_ID_BLE_GATTS_ADD_INCL_SVC_REQ,
-	MSG_ID_BLE_GATTS_ADD_CHARACTERISTIC_REQ,
-	MSG_ID_BLE_GATTS_ADD_DESCRIPTOR_REQ,
-	MSG_ID_BLE_GATTS_START_SERVICE_REQ,
-	MSG_ID_BLE_GATTS_REMOVE_SERVICE_REQ,
-	MSG_ID_BLE_GATTS_INDICATE_SERVICE_CHANGE_REQ,
-	MSG_ID_BLE_GATTS_SET_ATTRIBUTE_VALUE_REQ,
-	MSG_ID_BLE_GATTS_GET_ATTRIBUTE_VALUE_REQ,
-	MSG_ID_BLE_GATTS_SEND_NOTIF_REQ,
-	MSG_ID_BLE_GATTS_SEND_IND_REQ,
-	MSG_ID_BLE_GATTS_SEND_RW_AUTHORIZATION_REQ,
-	MSG_ID_BLE_GATTS_WR_CONN_ATTRIBUTES_REQ,
-	MSG_ID_BLE_GATTS_RD_CONN_ATTRIBUTES_REQ /* 37 */ ,
-	MSG_ID_BLE_GATTS_REQ_LAST,
-
-	/**< GATT Server Requests */
-	MSG_ID_BLE_GATTS_ADD_SERVICE_RSP = MSG_ID_BLE_GAP_RSP_LAST,
-							/**< create new service */
-	MSG_ID_BLE_GATTS_ADD_INCL_SVC_RSP,
-	MSG_ID_BLE_GATTS_ADD_CHARACTERISTIC_RSP,
-	MSG_ID_BLE_GATTS_ADD_DESCRIPTOR_RSP,
-	MSG_ID_BLE_GATTS_START_SERVICE_RSP,		    /**< enable created service */
-	MSG_ID_BLE_GATTS_REMOVE_SERVICE_RSP,	    /**< stop and remove service */
-	MSG_ID_BLE_GATTS_INDICATE_SERVICE_CHANGE_RSP,	/**< indicate a service change */
-	MSG_ID_BLE_GATTS_SET_ATTRIBUTE_VALUE_RSP,
-	MSG_ID_BLE_GATTS_GET_ATTRIBUTE_VALUE_RSP,
-	MSG_ID_BLE_GATTS_SEND_NOTIF_RSP,	    /**< send notification */
-	MSG_ID_BLE_GATTS_SEND_IND_RSP,	    /**< send indication */
-	MSG_ID_BLE_GATTS_SEND_RW_AUTHORIZATION_RSP,	    /**< authorize a R/W request from remote */
-	MSG_ID_BLE_GATTS_WR_CONN_ATTRIBUTES_RSP,	    /**< write connection related attributes (previously bonded!) */
-	MSG_ID_BLE_GATTS_RD_CONN_ATTRIBUTES_RSP /* 37 */ ,	    /**< read connection related attributes (only for bonded connections!*/
-	MSG_ID_BLE_GATTS_RSP_LAST,
-
-	/**< GATT Server Events */
-	MSG_ID_BLE_GATTS_WRITE_EVT = MSG_ID_BLE_GAP_EVT_LAST, /**< remote client write happened */
-	MSG_ID_BLE_GATTS_RW_AUTHORIZATION_REQ_EVT,  /**< remote client R/W authorization request */
-	MSG_ID_BLE_GATTS_CONN_ATTRIB_MISSING_EVT,   /**< connection related attributes have not been set, access pending */
-	MSG_ID_BLE_GATTS_INDICATION_CONF_EVT,	    /**< indication confirmation event */
-	MSG_ID_BLE_GATTS_SVC_CHG_CONF_EVT,	    /**< confirmation of service change indication (no params) */
-	MSG_ID_BLE_GATTS_TO_EVT,		    /**< GATTS timeout indication */
-	MSG_ID_BLE_GATTS_EVT_LAST
-};
-
-/**
- * GATT Client Message ID definitions.
- */
-enum BLE_GATTC_MSG_ID {
-	/**< GATT Client Requests, responses, events and indications */
-	MSG_ID_BLE_GATTC_DISCOVER_PRIMARY_SERVICE_REQ =
-	    MSG_ID_BLE_GATTS_REQ_LAST,
-	MSG_ID_BLE_GATTC_DISCOVER_INCLUDED_SERVICES_REQ,
-	MSG_ID_BLE_GATTC_DISCOVER_CHAR_REQ,
-	MSG_ID_BLE_GATTC_DISCOVER_DESCRIPTOR_REQ,
-	MSG_ID_BLE_GATTC_RD_CHARS_REQ,
-	MSG_ID_BLE_GATTC_WR_OP_REQ,
-	MSG_ID_BLE_GATTC_SEND_HANDLE_VALUE_REQ /* 44 */ ,
-
-	/** GATT Client Requests, responses, events and indications */
-	MSG_ID_BLE_GATTC_DISCOVER_PRIMARY_SERVICE_RSP = MSG_ID_BLE_GATTS_RSP_LAST,  /**< discover primary service */
-	MSG_ID_BLE_GATTC_DISCOVER_INCLUDED_SERVICES_RSP,/**< find included service procedure */
-	MSG_ID_BLE_GATTC_DISCOVER_CHAR_RSP,		    /**< discover characteristics of a service */
-	MSG_ID_BLE_GATTC_DISCOVER_DESCRIPTOR_RSP,	    /**< discover descriptor of a characteristic */
-	MSG_ID_BLE_GATTC_RD_CHARS_RSP,		    /**< read characteristic or long characteristics */
-	MSG_ID_BLE_GATTC_WR_OP_RSP,			    /**< different types of write operations */
-	MSG_ID_BLE_GATTC_SEND_HANDLE_VALUE_RSP /* 44 */ ,   /**< send attribute handle to server */
-
-	/** GATT Client Events */
-	MSG_ID_BLE_GATTC_DISC_PRIM_SVC_EVT = MSG_ID_BLE_GATTS_EVT_LAST,	    /**< primary service discovery response */
-	MSG_ID_BLE_GATTC_DISC_INCL_SVC_EVT,	    /**< include service discovery response */
-	MSG_ID_BLE_GATTC_DISC_CHAR_EVT,		    /**< characteristic discovery response */
-	MSG_ID_BLE_GATTC_DISC_DESCR_EVT,	    /**< descriptor discovery response */
-	MSG_ID_BLE_GATTC_RD_EVT,		    /**< data read response */
-	MSG_ID_BLE_GATTC_WR_EVT,		    /**< data write response */
-	MSG_ID_BLE_GATTC_HDL_NOTIF_EVT,		    /**< handle indication/notification event */
-	MSG_ID_BLE_GATTC_TO_EVT,		    /**< GATT Client timeout event */
-	MSG_ID_BLE_GATTC_LAST
-};
-
-/**
- * Maximum UUID size - 16 bytes, and structure to hold any type of UUID.
- */
-#define MAX_UUID_SIZE              16
-
-#define BLE_GATT_INVALID_HANDLE              0x0000  /**< reserved invalid attribute handle */
-#define BLE_GATT_MAX_HANDLE                  0xffff  /**< maximum handle in a BLE server */
-#define BLE_GATT_START_HANDLE_DISCOVER       0x0001  /**< Value of start handle during discovery. */
-
-/** BT uuid types defined as length. */
-enum BT_UUID_TYPES {
-	BT_UUID16 = 2, /**< 16 bit UUID type */
-	BT_UUID32 = 4, /**< 32 bit UUID type */
-	BT_UUID128 = 16 /**< 128 bit UUID type */
-};
-
-/**
- * Generic uuid structure specific to BT/BLE.
- */
-struct bt_uuid {
-	uint8_t type; /**< UUID type (encoded as length of the union element) @ref BT_UUID_TYPES */
-	union {
-		uint16_t uuid16;
-		uint32_t uuid32;
-		uint8_t uuid128[MAX_UUID_SIZE];
-	};
-};
-
-/**
- * UUID and Handle combination for services and characteristics
- *
- *  Make sure this is 32 bit aligned!
- */
-struct bt_uuid_handle_tuple {
-	void *p_priv;		/**< Service private reference handle. */
-	uint16_t handle;	/** Service or characteristic handle. */
-};
-
-/**
- * GATT service types, primary versus secondary/included one.
- */
-enum BLE_GATT_SVC_TYPES {
-	BLE_GATT_SVC_PRIMARY = 0,	    /**< primary service */
-	BLE_GATT_SVC_INCLUDED		    /**< include service (must be referenced by a primary) */
-};
-
-/**
- * Characteristic properties.
- */
-enum BLE_GATT_CHAR_PROPS {
-	BLE_GATT_CHAR_PROP_BIT_NONE = 0,
-	BLE_GATT_CHAR_PROP_BIT_BROADCAST = 0x01,
-	BLE_GATT_CHAR_PROP_BIT_READ = 0x02,
-	BLE_GATT_CHAR_PROP_BIT_WRITE_NR = 0x04,
-	BLE_GATT_CHAR_PROP_BIT_WRITE = 0x08,
-	BLE_GATT_CHAR_PROP_BIT_NOTIFY = 0x10,
-	BLE_GATT_CHAR_PROP_BIT_INDICATE = 0x20,
-	BLE_GATT_CHAR_PROP_BIT_AUTH = 0x40,
-	BLE_GATT_CHAR_PROP_BIT_EXTEND = 0x80/**< if set the extend property @ref BLE_GATT_CHAR_EXT_PROPS is present! */
-};
-
-/**
- * Extended characteristic properties.
- */
-enum BLE_GATT_CHAR_EXT_PROPS {
-	BLE_GATT_CHAR_EXT_PROP_BIT_NONE = 0,
-	BLE_GATT_CHAR_EXT_PROP_RELIABLE_WR = 0x0001,    /**< Reliable write procedure is supported */
-	BLE_GATT_CHAR_EXT_PROP_WR_AUX = 0x0002,         /**< User Descriptor Writes are permitted */
-};
-
-struct ble_gatt_char_properties {
-	uint8_t props;          /**< properties, @ref BLE_GATT_CHAR_PROPS */
-	uint16_t ext_props;     /**< extended properties, @ref BLE_GATT_CHAR_EXT_PROPS, valid if BLE_GATT_CHAR_PROP_BIT_EXTEND set */
-};
-
-/**
- * Format of the value of a characteristic, enumeration type.
- */
-enum BLE_GATT_FORMATS {
-	BLE_GATT_FORMAT_RES,	/* rfu */
-	BLE_GATT_FORMAT_BOOL,	/* 0x01 boolean */
-	BLE_GATT_FORMAT_2BITS,	/* 0x02 2 bit */
-	BLE_GATT_FORMAT_NIBBLE,	/* 0x03 nibble */
-	BLE_GATT_FORMAT_UINT8,	/* 0x04 uint8 */
-	BLE_GATT_FORMAT_UINT12,	/* 0x05 uint12 */
-	BLE_GATT_FORMAT_UINT16,	/* 0x06 uint16 */
-	BLE_GATT_FORMAT_UINT24,	/* 0x07 uint24 */
-	BLE_GATT_FORMAT_UINT32,	/* 0x08 uint32 */
-	BLE_GATT_FORMAT_UINT48,	/* 0x09 uint48 */
-	BLE_GATT_FORMAT_UINT64,	/* 0x0a uint64 */
-	BLE_GATT_FORMAT_UINT128,/* 0x0B uint128 */
-	BLE_GATT_FORMAT_SINT8,	/* 0x0C signed 8 bit integer */
-	BLE_GATT_FORMAT_SINT12,	/* 0x0D signed 12 bit integer */
-	BLE_GATT_FORMAT_SINT16,	/* 0x0E signed 16 bit integer */
-	BLE_GATT_FORMAT_SINT24,	/* 0x0F signed 24 bit integer */
-	BLE_GATT_FORMAT_SINT32,	/* 0x10 signed 32 bit integer */
-	BLE_GATT_FORMAT_SINT48,	/* 0x11 signed 48 bit integer */
-	BLE_GATT_FORMAT_SINT64,	/* 0x12 signed 64 bit integer */
-	BLE_GATT_FORMAT_SINT128,/* 0x13 signed 128 bit integer */
-	BLE_GATT_FORMAT_FLOAT32,/* 0x14 float 32 */
-	BLE_GATT_FORMAT_FLOAT64,/* 0x15 float 64 */
-	BLE_GATT_FORMAT_SFLOAT,	/* 0x16 IEEE-11073 16 bit SFLOAT */
-	BLE_GATT_FORMAT_FLOAT,	/* 0x17 IEEE-11073 32 bit SFLOAT */
-	BLE_GATT_FORMAT_DUINT16,/* 0x18 IEEE-20601 format */
-	BLE_GATT_FORMAT_UTF8S,	/* 0x19 UTF-8 string */
-	BLE_GATT_FORMAT_UTF16S,	/* 0x1a UTF-16 string */
-	BLE_GATT_FORMAT_STRUCT,	/* 0x1b Opaque structure */
-	BLE_GATT_FORMAT_MAX	/* 0x1c or above reserved */
-};
-
-/**
- * GATT characteristic user description.
- */
-struct ble_gatt_char_user_desc {
-	uint8_t *buffer;        /**< Pointer to a UTF-8 string. */
-	uint8_t len;            /**< The size in bytes of the user description. */
-};
-
-/**
- * GATT characteristic presentation format description.
- */
-struct ble_gatt_pf_desc {
-	uint16_t unit;      /**< as UUIUD defined by SIG */
-	uint16_t descr;     /**< as UUID as defined by SIG */
-	uint8_t format;     /**< @ref BLE_GATT_FORMATS */
-	int8_t exp;         /**< see Unit from Bluetooth Assigned Numbers, https://developer.bluetooth.org/gatt/units/Pages/default.aspx */
-	uint8_t name_spc;   /**< name space of the description */
-} ;
-
-/**
- * GATT indication types.
- */
-enum BLE_GATT_IND_TYPES {
-	BLE_GATT_IND_TYPE_NONE = 0,
-	BLE_GATT_IND_TYPE_NOTIFICATION,
-	BLE_GATT_IND_TYPES_INDICATION,
-};
-
-/**
- * GATT Write operation types
- *
- * (BT spec Vol 3, Part G, chapter. 4.9)
- * @note long char write, Prepare & Exe request are handled internally to the controller stack
- */
-enum BLE_GATT_WR_OP_TYPES {
-	BLE_GATT_WR_OP_NOP = 0,	    /**< normally not used except to cancel BLE_GATT_WR_OP_REQ long char write procedure */
-	BLE_GATT_WR_OP_CMD,	    /**< Write Command, (no response) */
-	BLE_GATT_WR_OP_REQ,	    /**< Write Request, Write response is received , if length is longer then ATT MTU, Prepare write procedure */
-	BLE_GATT_WR_OP_SIGNED_CMD,  /**< Signed Write Command */
-};
-
-/** @} */
-
-#endif
diff --git a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gattc_api.h b/system/libarc32_arduino101/framework/include/services/ble/ble_service_gattc_api.h
deleted file mode 100644
index d7b42df9..00000000
--- a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gattc_api.h
+++ /dev/null
@@ -1,372 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __BLE_SERVICE_GATTC_H__
-#define __BLE_SERVICE_GATTC_H__
-
-#include "ble_service.h"
-#include "ble_service_gap_api.h"
-#include "ble_service_gatt.h"
-
-/** 
- * @defgroup ble_core_service_gattc BLE Core Service GATTC
- * @ingroup ble_core_service
- *
- * BLE Core Service GATTC APIs used to implement GATT Clients.
- *
- * This is typically only used to add new client services to BLE service.
- *
- * It provides the following services:
- * - Discover remote \b primary services or a specific service
- * - Discover remote characteristics
- * - Discover remote descriptors
- * - read/write remote characteristics
- * - Getting notified on characteristic changes
- *
- * @{
- */
-
-/**
- * Generic GATTC response message.
- */
-struct ble_gattc_rsp {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	uint16_t conn_handle; /**< GAP connection handle */
-};
-
-/**
- * Generic GATTC error event.
- */
-struct ble_gattc_err_rsp_evt {
-	uint16_t err_handle;    /**< handle of char attribute causing the failure */
-};
-
-/**
- * Handle range for a service operation.
- */
-struct ble_gattc_handle_range {
-	uint16_t start_handle;
-	uint16_t end_handle;
-};
-
-typedef struct {
-	struct ble_gattc_handle_range handle_range; /**< range of characteristic handles within a service */
-	struct bt_uuid uuid; /**< service uuid */
-} ble_gattc_svc_t;
-
-/**
- * Primary Service discovery Indication message @ref MSG_ID_BLE_GATTC_DISC_PRIM_SVC_EVT.
- */
-typedef struct {
-	uint16_t svc_count;		/**< number of service included into this indication */
-	ble_gattc_svc_t service_found[];/**< array on fouTnd services */
-} ble_gattc_primary_svc_disc_evt_t;
-
-/**
- * Included service.
- */
-typedef struct {
-	uint16_t incl_handle;		/**< handle of included service */
-	ble_gattc_svc_t svc;		/**< included service */
-} ble_gattc_incl_svc_t;
-
-/**
- * Discovered included services @ref MSG_ID_BLE_GATTC_DISC_INCL_SVC_EVT.
- */
-typedef struct {
-	uint16_t incl_count; /**< Number of included services */
-	ble_gattc_incl_svc_t included[]; /**< Array on found services */
-} ble_gattc_incl_svc_disc_evt_t;
-
-typedef struct {
-	struct ble_gatt_char_properties char_properties;	/**< characteristic properties */
-	uint16_t decl_handle;			/**< Characteristic declaration handle */
-	uint16_t value_handle;			/**< Char's value handle */
-	struct bt_uuid uuid;			/**< Characteristic's UUID */
-} ble_gattc_characteristic_t;
-
-/**
- * Discovered characteristics indication @ref MSG_ID_BLE_GATTC_DISC_CHAR_EVT.
- */
-typedef struct {
-	uint16_t char_count;			/**< number of characteristics in this message */
-	ble_gattc_characteristic_t chars[];	/**< characteristics data as per char_count */
-} ble_gattc_char_disc_evt_t;
-
-/**
- * GATTC descriptor.
- */
-typedef struct {
-	uint16_t handle;	/**< descriptor handle */
-	struct bt_uuid uuid;	/**< uuid of the descriptor */
-} ble_gattc_descriptor_t;
-
-/**
- * Descriptor discover indication.
- */
-typedef struct {
-	uint16_t desc_count;			/**< number of descriptors in this message */
-	ble_gattc_descriptor_t descs[];		/**< found descriptors */
-} ble_gattc_desc_disc_evt_t;
-
-enum BLE_GATTC_RD_CHAR_TYPES {
-	BLE_GATTC_RD_CHAR_BY_UUID = 0,		/**< Read characteristic by UUID */
-	BLE_GATTC_RD_CHAR,			/**< Read (Long) characteristic or (Long) descriptor. Maybe called multiple times in case of long */
-	BLE_GATTC_RD_CHAR_MULTIPLE		/**< Read multiple characteristic attributes */
-};
-
-/**
- * Characteristic read by using UUID.
- */
-typedef struct {
-	struct ble_gattc_handle_range handle_range;	/**< characteristic or descriptor handle range */
-	struct bt_uuid *p_uuid; /**< uuid of characteristic to read */
-} ble_gattc_rd_char_by_uuid_t;
-
-/**
- * Characteristic or descriptor read.
- *
- * Maybe used for long too.
- */
-typedef struct {
-	uint16_t handle;			/**< attribute handle for reading */
-	uint16_t offset;			/**< offset into attribute data to read */
-} ble_gattc_rd_char_t;
-
-/**
- * Read multiple characteristics values.
- */
-typedef struct {
-	uint16_t handle_count;			/**< number of handles in this structure */
-	uint16_t handle[];			/**< handles of attributes to read from */
-} ble_gattc_rd_multi_char_t;
-
-typedef struct {
-	union {
-		ble_gattc_rd_char_by_uuid_t char_by_uuid;
-		ble_gattc_rd_char_t char_desc;	    /**< (Long) characteristic or descriptor to read */
-		ble_gattc_rd_multi_char_t multi_char;
-						    /**< read multiple characteristics */
-	};
-} ble_gattc_rd_characteristic_t;
-
-typedef struct {
-	uint16_t char_handle;		/**< handle of characteristic */
-	uint16_t len;			/**< if len is bigger then ATT MTU size, the controller fragment buffer itself */
-	uint8_t *p_value;		/**< characteristic value to write */
-	uint8_t wr_type;		/**< type of write operation @ref BLE_GATT_WR_OP_TYPES */
-} ble_gattc_wr_characteristic_t;
-
-/**
- * Read characteristic response indication (@ref MSG_ID_BLE_GATTC_RD_EVT).
- */
-typedef struct {
-	uint16_t handle;		/**< handle of characteristic attribute read */
-	uint16_t offset;		/**< offset of data returned */
-	uint16_t len;			/**< length of data returned */
-	uint8_t data[];			/**< characteristic attribute data */
-} ble_gattc_rd_char_evt_t;
-
-/**
- * Characteristic write response indication @ref MSG_ID_BLE_GATTC_WR_EVT.
- */
-typedef struct {
-	uint16_t char_handle;
-	uint16_t len;
-} ble_gattc_wr_char_evt_t;
-
-/**
- * Handle value indication or notification indication/event (@ref MSG_ID_BLE_GATTC_HDL_NOTIF_EVT).
- */
-typedef struct {
-	uint16_t handle;	/**< handle of characteristic being notified/indicated */
-	uint16_t len;		/**< length of value included into this indication */
-	uint8_t type;		/**< notification versus indication, @ref BLE_GATT_IND_TYPES */
-	uint8_t data[];         /**< value data received */
-} ble_gattc_value_evt_t;
-
-/**
- * GATT timeout reason.
- */
-typedef struct {
-	uint16_t reason;	/**< GATT timeout reason */
-} ble_gattc_to_evt_t;
-
-/**
- * GATTC indication or response message structure applicable to most indications/events/responses.
- */
-struct ble_gattc_evt_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	uint16_t conn_handle;
-	union {
-		struct ble_gattc_err_rsp_evt err_rsp; /**< returned only if status != BLE_GATT_STATUS_SUCCESS */
-		ble_gattc_primary_svc_disc_evt_t prim_svc_disc;
-							    /**< primary service discovery indication event */
-		ble_gattc_incl_svc_disc_evt_t incl_svc_disc; /**< included services discovered */
-		ble_gattc_char_disc_evt_t char_disc;	    /**< characteristic discover event/indication */
-		ble_gattc_desc_disc_evt_t desc_disc;	    /**< discovered descriptors indication/event */
-		ble_gattc_rd_char_evt_t char_rd;	    /**< read characteristic indication/event */
-		ble_gattc_wr_char_evt_t char_wr;	    /**< characteristic write indication event */
-		ble_gattc_value_evt_t val_ind;		    /**< value indication or notification */
-		ble_gattc_to_evt_t timeout_ind;		    /**< gattc timeout protocol error */
-	};						    /**< in case for responses, union is not used! */
-};
-
-/**
- * Discover primary service.
- *
- * @param p_svc_handle service handle
- * @param conn_handle  connection to use
- * @param p_svc_uuid   points to service UUID. if NULL, all services found are returned
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTC_DISCOVER_PRIMARY_SERVICE_RSP @ref ble_gattc_rsp
- * @return EVT: MSG_ID_BLE_GATTC_DISC_PRIM_SVC_EVT @ref ble_gattc_primary_svc_disc_evt_t
- */
-int ble_gattc_discover_primary_service(svc_client_handle_t * p_svc_handle,
-				       uint16_t conn_handle,
-				       const struct bt_uuid * p_svc_uuid,
-				       void *p_priv);
-
-/**
- * Discover included services on a previously discovered primary service.
- *
- * @param p_svc_handle   service handle
- * @param conn_handle    connection to use
- * @param p_handle_range handle range previously returned by @ref ble_gattc_primary_svc_disc_evt_t
- * @param p_priv         pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTC_DISCOVER_INCLUDED_SERVICES_RSP @ref ble_gattc_rsp
- */
-int ble_gattc_discover_included_service(svc_client_handle_t * p_svc_handle,
-					uint16_t conn_handle,
-					const struct ble_gattc_handle_range *
-					p_handle_range, void *p_priv);
-
-/**
- * Discover characteristics on a service.
- *
- * May be called several times if not all characteristics have been discovered.
- * In this case a new handle range needs to be provided.
- *
- * @param p_svc_handle   service handle
- * @param conn_handle    connection to use
- * @param p_handle_range handle range
- * @param p_priv         pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTC_DISCOVER_CHAR_RSP @ref ble_gattc_rsp
- *          @ref MSG_ID_BLE_GATTC_DISC_CHAR_EVT @ref ble_gattc_char_disc_evt_t
- */
-int ble_gattc_discover_characteristic(svc_client_handle_t * p_svc_handle,
-				      uint16_t conn_handle,
-				      const struct ble_gattc_handle_range * p_handle_range,
-					  void *p_priv);
-
-/**
- * Discover characteristics on a service.
- *
- * May be called several times if not all characteristics have been discovered.
- * In this case a new handle range needs to be provided.
- *
- * @param p_svc_handle   service handle
- * @param conn_handle    connection to use
- * @param p_handle_range handle range
- * @param p_priv         pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTC_DISCOVER_DESCRIPTOR_RSP @ref ble_gattc_rsp
- *          @ref MSG_ID_BLE_GATTC_DISC_DESCR_EVT @ref ble_gattc_desc_disc_evt_t
- */
-int ble_gattc_discover_descriptor(svc_client_handle_t * p_svc_handle,
-				  uint16_t conn_handle,
-				  const struct ble_gattc_handle_range *
-				  p_handle_range, void *p_priv);
-
-/**
- * Read characteristic on remote server.
- *
- * @param p_svc_handle    service handle
- * @param conn_handle     connection to use
- * @param type            type of read to execute @ref BLE_GATTC_RD_CHAR_TYPES
- * @param p_rd_char_param read type specific characteristic read parameter
- * @param p_priv          pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTC_RD_CHARS_RSP @ref ble_gattc_rsp
- * @return EVT: MSG_ID_BLE_GATTC_RD_EVT @ref ble_gattc_rd_char_evt_t
- */
-int ble_gattc_read_characteristic(svc_client_handle_t * p_svc_handle,
-				  uint16_t conn_handle,
-				  uint8_t type,
-				  const ble_gattc_rd_characteristic_t * p_rd_char_param,
-				  void *p_priv);
-
-/**
- * Write characteristic on server.
- *
- * @param p_svc_handle service handle
- * @param conn_handle connection to use
- * @param p_wr_char_param write characteristic on remote service
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GATTC_WR_OP_RSP @ref ble_gattc_rsp
- */
-int ble_gattc_write_char_op(svc_client_handle_t * p_svc_handle,
-			    uint16_t conn_handle,
-			    const ble_gattc_wr_characteristic_t *
-			    p_wr_char_param,
-			    void *p_priv);
-
-
-/**
- * Write characteristic on server.
- *
- * @param p_svc_handle service handle
- * @param conn_handle  connection to use
- * @param val_handle   handle to confirm and received by Handle Value Indication (@ref MSG_ID_BLE_GATTC_HDL_NOTIF_EVT)
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE,
- * @return MSG: MSG_ID_BLE_GATTC_SEND_HANDLE_VALUE_RSP @ref ble_gattc_rsp
- */
-int ble_gattc_send_confirm_handle_value(svc_client_handle_t * p_svc_handle,
-					uint16_t conn_handle,
-					uint16_t val_handle,
-					void *p_priv);
-
-/** @} */
-
-#endif
diff --git a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gatts_api.h b/system/libarc32_arduino101/framework/include/services/ble/ble_service_gatts_api.h
deleted file mode 100644
index 76bc3149..00000000
--- a/system/libarc32_arduino101/framework/include/services/ble/ble_service_gatts_api.h
+++ /dev/null
@@ -1,559 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __BLE_SERVICE_GATTS_H__
-#define __BLE_SERVICE_GATTS_H__
-
-#include "ble_service.h"
-#include "ble_service_gap_api.h"
-#include "ble_service_gatt.h"
-
-/**  @defgroup ble_core_service_gatts BLE Core Service GATTS
- * @ingroup ble_core_service
- *
- * BLE Core GATTS Service APIs to implement GATT Servers.
- *
- * This API should only be used by BLE service to implement additional BLE profiles/services.
- *
- * Those the GATT server APIs provide the following services:
- * - Create an new (server) BLE service
- * - Add characteristics to the service
- * - Write local server characteristics
- * - Receive data when updated by client
- *
- * @note If a service is based on a 128 bit UUID (vendor service), all the characteristic
- * need to use the same 128 bit UUID base and only vary octets 12-13 of base UUID.
- *
- * @{
- */
-
-/**
- * BLE GATTS max attribute length.
- * @note BLE controller dependent
- */
-#define BLE_SVC_GATTS_FIX_ATTR_LEN_MAX 510 /**< Maximum length for fixed length Attribute Values. */
-#define BLE_SVC_GATTS_VAR_ATTR_LEN_MAX 512 /**< Maximum length for variable length Attribute Values. */
-
-/* forward declaration for callback handlers */
-struct _ble_service_cb;
-struct ble_gatts_add_svc_rsp;
-struct ble_gatts_add_char_rsp;
-struct ble_gatts_add_desc_rsp;
-struct ble_gatts_notif_ind_rsp_msg;
-
-/**
- * Generic GATTS response message.
- */
-typedef struct {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	uint16_t conn_handle; /**< GAP connection handle */
-} ble_gatts_rsp_t;
-
-/**
- * Add Service callback handler.
- */
-typedef int (* ble_gatts_add_svc_cback_t)(struct ble_gatts_add_svc_rsp * rsp,
-		struct _ble_service_cb * p_cb);
-
-/**
- * Add service response message.
- */
-struct ble_gatts_add_svc_rsp {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< status of service creation */
-	ble_gatts_add_svc_cback_t cback; /**< Callback function to execute on reception of this message */
-	uint16_t svc_handle; /**< Handle of newly allocated service (only valid in case of success. */
-};
-
-/**
- * Include service response.
- */
-typedef struct {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< status of service creation */
-	uint16_t svc_handle; /**< updated handle of included service (only valid in case of success */
-} ble_gatts_incl_svc_rsp_t;
-
-/**
- * ATT attribute permission
- */
-struct ble_gatts_permissions {
-	uint8_t rd;    /**< Read permissions, @ref BLE_GAP_SEC_MODES */
-	uint8_t wr;    /**< Write permissions @ref BLE_GAP_SEC_MODES */
-};
-
-/**
- * GATT characteristic.
- */
-struct ble_gatts_characteristic {
-	struct bt_uuid * p_uuid;                        /**< Pointer to the characteristic UUID. */
-	struct ble_gatts_permissions perms;             /**< Characteristic value attribute permissions */
-	struct ble_gatt_char_properties props;          /**< Characteristic Properties. @ref ble_gatt_char_properties */
-	uint16_t max_len;                               /**< Maximum characteristic value length in bytes, see @ref BLE_SVC_GATTS_FIX_ATTR_LEN_MAX or @ref BLE_SVC_GATTS_VAR_ATTR_LEN_MAX. */
-	uint16_t init_len;                              /**< Initial characteristic value length in bytes. */
-	uint8_t * p_value;                              /**< Pointer to the characteristic initialization value */
-	// optional descriptors
-	struct ble_gatt_char_user_desc * p_user_desc;   /**< Optional user description of the characteristic, NULL if not required */
-	struct ble_gatt_pf_desc *p_char_pf_desc;        /**< Pointer to a presentation format structure or NULL if the descriptor is not required. */
-};
-
-/**
- * GATT generic descriptor.
- */
-struct ble_gatts_descriptor {
-	struct bt_uuid * p_uuid; /**< Pointer to the descriptor UUID. */
-	uint8_t * p_value; /**< Value of the descriptor */
-	uint16_t length; /**< Length of the descriptor value */
-	struct ble_gatts_permissions perms; /**< Descriptor attribute permissions */
-};
-
-struct ble_gatts_char_handles {
-	uint16_t value_handle;		    /**< Handle to the characteristic value. */
-	uint16_t cccd_handle;		    /**< Handle to the Client Characteristic Configuration Descriptor, or BLE_GATT_HANDLE_INVALID if not present. */
-	uint16_t sccd_handle;		    /**< Handle to the Server Characteristic Configuration Descriptor, or BLE_GATT_HANDLE_INVALID if not present. */
-};
-
-/**
- * Add Service callback handler.
- */
-typedef int (* ble_gatts_add_char_cback_t)(struct ble_gatts_add_char_rsp * rsp,
-		struct _ble_service_cb * p_cb);
-
-/**
- * Add characteristic response message.
- */
-struct ble_gatts_add_char_rsp {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< Status of the operation. */
-	ble_gatts_add_char_cback_t cback; /**< Callback function to call on reception of this message */
-	struct ble_gatts_char_handles char_h; /**< Handles of the created characteristic */
-};
-
-/**
- * Add Service callback handler.
- */
-typedef int (* ble_gatts_add_desc_cback_t)(struct ble_gatts_add_desc_rsp * rsp,
-		struct _ble_service_cb * p_cb);
-
-/**
- * Add descriptor response message.
- */
-struct ble_gatts_add_desc_rsp {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< Status of the operation. */
-	ble_gatts_add_desc_cback_t cback; /**< Callback function to call on reception of this message */
-	uint16_t handle; /**< Handle of the created descriptor */
-};
-
-/**
- * Set attribute response message.
- */
-struct  ble_gatts_set_attr_rsp_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	uint16_t value_handle;
-};
-
-/**
- * Notification/Indication callback.
- */
-typedef int (* ble_gatts_notif_ind_cback_t)(struct ble_gatts_notif_ind_rsp_msg * rsp,
-		struct _ble_service_cb * p_cb);
-
-/**
- * Notification/Indication response message.
- */
-struct ble_gatts_notif_ind_rsp_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	uint16_t conn_handle;
-	ble_gatts_notif_ind_cback_t cback; /**< Callback function to call on reception of this message */
-	uint16_t handle;                /**< Characteristic value handle */
-};
-
-/**
- * Shortened attribute type definitions.
- * See BT Spec r Vol 3, PART G, chapter 3
- */
-enum BLE_SVC_GATTS_ATTR_TYPES {
-	BLE_SVC_GATTS_ATTR_TYPE_NONE = 0,
-	BLE_SVC_GATTS_ATTR_TYPE_PRIMARY_SVC_DECL,
-					    /**< primary service attribute declaration (chpt 3.1) */
-	BLE_SVC_GATTS_ATTR_TYPE_SECONDARY_SVC_DECL,
-					    /**< secondary service attribute declaration (chpt 3.1) */
-	BLE_SVC_GATTS_ATTR_TYPE_INCLUDE_DECL,   /**< include attribute declaration (3.2) */
-	BLE_SVC_GATTS_ATTR_TYPE_CHAR_DECL,	    /**< characteristic declaration (3.3.1) */
-	BLE_SVC_GATTS_ATTR_TYPE_CHAR_VALUE_DECL,/**< Characteristic value declaration */
-	BLE_SVC_GATTS_ATTR_TYPE_DESC_DECL,	    /**< descriptor declaration */
-};
-
-/**
- * GATT server write ops.
- */
-enum BLE_GATTS_WR_OPS {
-	BLE_GATTS_OP_NONE = 0,
-	BLE_GATTS_OP_WR,		    /**< 3.4.5.1 Write Request (Attribute), expects write response */
-	BLE_GATTS_OP_WR_CMD,		    /**< 3.4.5.3 Write Command (Attribute) NO response sent */
-	BLE_GATTS_OP_WR_CMD_SIGNED,	    /**< 3.4.5.4 Write Command Signed (Attribute), NO response sent */
-	BLE_GATTS_OP_WR_PREP_REQ,	    /**< 3.4.6.1 Write Prepare Request, expects a prepare write request response */
-	BLE_GATTS_OP_WR_EXE_REQ_CANCEL,	    /**< 3.4.6.3 Cancel Executed Write Request, cancel and clear queue (flags = 0) */
-	BLE_GATTS_OP_WR_EXE_REQ_IMM	    /**< 3.4.6.3 Immediately Execute Write Request */
-};
-
-/**
- * Write authorization context data structure.
- */
-typedef struct {
-	uint16_t attr_handle;			    /**< handle of attribute to write */
-	uint16_t offset;
-	uint16_t len;
-	uint8_t op;				    /**< @ref BLE_GATTS_WR_OPS */
-	uint8_t data[];
-} ble_gatt_wr_evt_t;
-
-/**
- * Read authorization context data structure.
- */
-typedef struct {
-	uint16_t attr_handle;			    /**< handle of attribute to been read */
-	uint16_t offset;
-} ble_gatt_rd_evt_t;
-
-/**
- * Connection attribute data is missing @ref MSG_ID_BLE_GATTS_CONN_ATTRIB_MISSING_EVT.
- */
-typedef struct {
-	uint16_t miss_type;		/**< missing connection attribute type */
-} ble_gatts_conn_attrib_missing_evt_t;
-
-/**
- * Handle Value Confirmation, @ref MSG_ID_BLE_GATTS_INDICATION_CONF_EVT in response an handle value indication.
- */
-typedef struct {
-	uint16_t handle;		    /**< attribute handle of indication value sent */
-} ble_gatts_handle_value_conf_evt_t;
-
-/**
- * Read attribute value rsp message.
- */
-typedef struct {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;
-	uint16_t conn_handle;		    /**< GAP connection handle */
-	uint16_t val_handle;		    /**< handle of attribute value */
-	uint16_t len;			    /**< length of value returned */
-	uint16_t offset;		    /**< offset in the value. the same as in the rd request! */
-	uint8_t value[];		    /**< value data of length \ref len */
-} ble_gatts_rd_attrib_val_rsp_t;
-
-/**
- * Indication or notification.
- */
-typedef struct {
-	uint16_t val_handle;		    /**< handle of attribute value */
-	uint16_t len;			    /**< len of attribute data value to indicate */
-	uint8_t *p_data;		    /**< data to indicate (maybe NULL if currently stored shall be used) */
-	uint16_t offset;		    /**< optional offset into attribute value data */
-} ble_gatts_ind_params_t;
-
-/**
- * Connection related attribute data parameters (stack specific).
- */
-typedef struct {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status;		/**< result for request, if not success, the data afterwards may not be valid */
-	uint16_t conn_handle;		/**< connection handle of the retrieved connection attribute data */
-	uint16_t len;			/**< length of the following connection attribute data */
-	uint8_t conn_attr_data[];
-} ble_gatts_rd_conn_attr_rsp_t;
-
-/**
- * GATTS timeout @ref MSG_ID_BLE_GATTS_TO_EVT.
- */
-typedef struct {
-	uint16_t reason;	/**< reason for timeout */
-} ble_gatts_timeout_evt_t;
-
-/**
- * BLE GATTS Indication Data structure.
- */
-struct ble_gatts_evt_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_status_t status; /**< result for request, if not success, the data afterwards may not be valid */
-	uint16_t conn_handle;
-	union {
-		ble_gatt_wr_evt_t wr;			    /**< write indication */
-		ble_gatts_conn_attrib_missing_evt_t conn_attr;
-							    /**< connection related attribute missing */
-		ble_gatts_handle_value_conf_evt_t handle_val_conf;
-							    /**< value confirmation (confirmation of an indication) */
-		ble_gatts_timeout_evt_t timeout;	    /**< GATTS timeout occurred */
-	};
-};
-
-/**
- * Create an new service, primary or include.
- *
- * @param p_svc_handle service handle
- * @param p_uuid       UUID of new service
- * @param type         primary versus included @ref BLE_GATT_SVC_TYPES
- * @param cback        Callback function to be called on reception of add service response
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_ADD_SERVICE_RSP @ref ble_gatts_add_svc_rsp
- */
-int ble_gatts_add_service(svc_client_handle_t * p_svc_handle,
-			  const struct bt_uuid * p_uuid,
-			  uint8_t type,
-			  ble_gatts_add_svc_cback_t cback,
-			  void *p_priv);
-
-/**
- * Include a (secondary) service into a primary service.
- *
- * @param p_svc_handle          service handle
- * @param svc_handle            service to which to add the included service
- * @param svc_handle_to_include the previously created includable service
- * @param p_priv                pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_ADD_INCL_SVC @ref ble_gatts_incl_svc_rsp_t
- */
-int ble_gatts_add_included_svc(svc_client_handle_t * p_svc_handle,
-			       uint16_t svc_handle,
-			       uint16_t svc_handle_to_include,
-			       void *p_priv);
-
-/**
- * Add a characteristic to service.
- *
- * @note this may called with the same UUID. the returned handle will be different in this case to
- *       distinguish multiple instances of the same char
- *
- * @param p_svc_handle service handle
- * @param svc_handle   service to which to add the characteristic
- * @param p_char       meta data for characteristic
- * @param cback        Callback function called on reception of response message
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_ADD_CHARACTERISTIC @ref ble_gatts_add_char_rsp
- */
-int ble_gatts_add_characteristic(svc_client_handle_t * p_svc_handle,
-				 uint16_t svc_handle,
-				 const struct ble_gatts_characteristic * p_char,
-				 ble_gatts_add_char_cback_t cback,
-				 void *p_priv);
-
-/**
- * Add a descriptor to the last added characteristic.
- *
- * @note The descriptor is automatically added to the latest
- *       added characteristic
- *
- * @param p_svc_handle service handle
- * @param p_desc       description of the descriptor
- * @param cback        Callback function called on reception of response message
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_ADD_DESCRIPTOR @ref ble_gatts_add_desc_rsp
- */
-int ble_gatts_add_descriptor(svc_client_handle_t * p_svc_handle,
-			     const struct ble_gatts_descriptor * p_desc,
-			     ble_gatts_add_desc_cback_t cback,
-			     void * p_priv);
-
-/**
- * Start BLE Service setup before.
- *
- * @param p_svc_handle service handle
- * @param svc_handle service to start
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_START_SERVICE @ref ble_rsp
- */
-int ble_gatts_start_service(svc_client_handle_t * p_svc_handle,
-			    uint16_t svc_handle,
-			    void *p_priv);
-
-/**
- * Stop and remove service.
- *
- * @note Not yet supported
- *
- * @param p_svc_handle service handle
- * @param svc_handle   handle of characteristic to which to add the descriptor
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_REMOVE_SERVICE_RSP @ref ble_rsp
- */
-int ble_gatts_remove_service(svc_client_handle_t * p_svc_handle,
-			     uint16_t svc_handle,
-			     void *p_priv);
-
-/**
- * Send a service change indication.
- *
- * @note Not yet supported
- *
- * @param p_svc_handle service handle
- * @param conn_handle  handle of the connection affected by the service layout change
- * @param start_handle start handle of changed attribute handle range
- * @param end_handle   end handle of changed attribute handle range
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_INDICATE_SERVICE_CHANGE @ref ble_gatts_rsp_t
- */
-int ble_gatts_send_svc_changed(svc_client_handle_t * p_svc_handle,
-			       uint16_t conn_handle,
-			       uint16_t start_handle,
-			       uint16_t end_handle,
-			       void *p_priv);
-
-/**
- * Set an attribute value.
- *
- * @param p_svc_handle service handle
- * @param value_handle handle of value to change
- * @param len          length of attribute value to write
- * @param p_value      attribute value data to write
- * @param offset       optional offset from which on to write the attribute value data
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_WR_ATTRIBUTE_VALUE ble_gatts_wr_attr_rsp_msg
- */
-int ble_gatts_set_attribute_value(svc_client_handle_t * p_svc_handle,
-				 uint16_t value_handle,
-				 uint16_t len,
-				 const uint8_t * p_value,
-				 uint16_t offset,
-				 void *p_priv);
-
-/**
- * Get an attribute value.
- *
- * @param p_svc_handle service handle
- * @param value_handle handle of the attribute value to retrieve
- * @param len          length of the attribute value to get
- * @param offset       optional offset from which on to get the attribute value
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_RD_ATTRIBUTE_VALUE @ref ble_gatts_rd_attrib_val_rsp_t
- */
-int ble_gatts_get_attribute_value(svc_client_handle_t * p_svc_handle,
-				 uint16_t value_handle,
-				 uint16_t len,
-				 uint16_t offset,
-				 void *p_priv);
-
-/**
- * Send notification.
- *
- * @param p_svc_handle service handle
- * @param conn_handle  handle of the connection affected by the service layout change
- * @param p_ind_params length of attribute value to write
- * @param cback        callback function to be called on reception of reception of notif response
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_SEND_NOTIF_RSP @ref ble_gatts_notif_ind_rsp_msg
- */
-int ble_gatts_send_notif(svc_client_handle_t * p_svc_handle,
-			     uint16_t conn_handle,
-			     const ble_gatts_ind_params_t * p_params,
-			     ble_gatts_notif_ind_cback_t cback,
-			     void *p_priv);
-
-/**
- * Send indication.
- *
- * @param p_svc_handle service handle
- * @param conn_handle  handle of the connection affected by the service layout change
- * @param p_ind_params length of attribute value to write
- * @param cback        callback function to be called on reception of reception of ind response
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_SEND_NOTIF_RSP @ref ble_gatts_notif_ind_rsp_msg
- */
-int ble_gatts_send_ind(svc_client_handle_t * p_svc_handle,
-			     uint16_t conn_handle,
-			     const ble_gatts_ind_params_t * p_params,
-			     ble_gatts_notif_ind_cback_t cback,
-			     void *p_priv);
-
-/**
- * Write stack specific data of a previously bonded connection.
- *
- * @note this data is typically stored in NVRAM in relation ship to the bonded device!
- *       (e.g. CCD)
- *
- * @param p_svc_handle service handle
- * @param conn_handle  handle of the connection
- * @param p_data       data blob specific to stack to write
- * @param len          length of above byte stream (little endian)
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_WR_CONN_ATTRIBUTES @ref ble_gatts_rsp_t
- */
-int ble_gatts_write_conn_attributes(svc_client_handle_t * p_svc_handle,
-				    uint16_t conn_handle,
-				    const uint8_t * p_data,
-				    uint16_t len,
-				    void *p_priv);
-
-/**
- * Read stack specific data of the bonded connection.
- *
- * @note this data is typically stored in NVRAM in relation ship to the bonded device!
- *
- * @param p_svc_handle service handle
- * @param conn_handle  handle of the connection
- * @param p_priv       pointer to private data
- *
- * @return @ref OS_ERR_TYPE
- * @return MSG: MSG_ID_BLE_GATTS_RD_CONN_ATTRIBUTES @ref ble_gatts_rd_conn_attr_rsp_t
- */
-int ble_gatts_read_conn_attributes(svc_client_handle_t * p_svc_handle,
-				   uint16_t conn_handle,
-				   void *p_priv);
-
-/** @} */
-
-#endif
diff --git a/system/libarc32_arduino101/framework/include/services/services_ids.h b/system/libarc32_arduino101/framework/include/services/services_ids.h
index ccf2260c..c153a468 100644
--- a/system/libarc32_arduino101/framework/include/services/services_ids.h
+++ b/system/libarc32_arduino101/framework/include/services/services_ids.h
@@ -57,6 +57,7 @@ enum {
 	CFW_LAST_SERVICE_ID   = 17
 };
 
+#define MSG_ID_BLE_SERVICE_BASE         (BLE_SERVICE_ID << 10)
 #define BLE_SERVICE_MSG_BASE      (BLE_SERVICE_ID << 10)
 #define BLE_SERVICE_GAP_MSG_BASE  (BLE_CORE_SERVICE_ID << 10)
 #define MSG_ID_GPIO_BASE          (SOC_GPIO_SERVICE_ID << 10)
diff --git a/system/libarc32_arduino101/framework/include/util/misc.h b/system/libarc32_arduino101/framework/include/util/misc.h
new file mode 100644
index 00000000..ea2cdff4
--- /dev/null
+++ b/system/libarc32_arduino101/framework/include/util/misc.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2015, Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __MISC_H__
+#define __MISC_H__
+
+/* required for offsetof */
+#include <stddef.h>
+
+//#include "util/compiler.h"
+
+/** Generate a build time error if a condition is met
+ *
+ * @param condition The condition to be tested
+ *
+ * @note This function should be used inside a code block
+ */
+#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2 * !!(condition)]))
+
+/** Generate a build time error if a condition is met or return 0
+ *
+ * @param condition The condition to be tested
+ *
+ * @return 0
+ */
+#define BUILD_BUG_ON_ZERO(e) (sizeof(char[1 - 2 * !!(e)]) - 1)
+
+/**
+ * Similar to BUILD_BUG_ON_ZERO
+ */
+#define STATIC_ASSERT(EXPR) typedef char __unused static_assert_failed[(EXPR) ?	\
+								       1 : -1]
+
+/** Generate a build time error if the parameter is not an array otherwise return 0
+ *
+ * @param   A   The array to be checked.
+ *
+ * @return 0
+ */
+#define __must_be_array(A) \
+	BUILD_BUG_ON_ZERO (HAVE_SAME_TYPE(A, &A[0]))
+
+/**
+ * Check if the integer provided is a power of two.
+ *
+ * @param   A   Number to be tested.
+ *
+ * @return      true if value is power of two else false.
+ */
+#define IS_POWER_OF_TWO(A)  ((A) != 0 && (((A)-1) & (A)) == 0)
+
+/**
+ * Given a pointer on a member, return the pointer on the containing struct
+ *
+ * @param   ptr    Pointer to the member
+ * @param   type   Containing struct
+ * @param   member Member name
+ *
+ * @return         Casted pointer to the struct
+ *
+ * @note Use if possible CONTAINER_OF() defined in Zephyr include <misc/util.h>
+ */
+#define container_of(ptr, type, member) ({ \
+						 (type *)((char *)(ptr) - \
+							  offsetof(type, member)); })
+
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+
+/**
+ * Concatenate 4 ASCII characters into an integer
+ *
+ * @param key_1 first ASCII char
+ * @param key_2 second ASCII char
+ * @param key_3 third ASCII char
+ * @param key_4 fourth ASCII char
+ *
+ * @return the integer representation of the four-digit string
+ */
+#define GEN_KEY(key_1, key_2, key_3, key_4) \
+	(key_1 << 24) | (key_2 << 16) | (key_3 << 8) | key_4
+
+#endif  /* __MISC_H__ */
diff --git a/system/libarc32_arduino101/framework/src/cfw/service_api.c b/system/libarc32_arduino101/framework/src/cfw/service_api.c
index 936fa3f9..183167b6 100644
--- a/system/libarc32_arduino101/framework/src/cfw/service_api.c
+++ b/system/libarc32_arduino101/framework/src/cfw/service_api.c
@@ -111,6 +111,11 @@ struct cfw_message * cfw_alloc_evt_msg(service_t *svc, int msg_id, int size) {
 
 struct cfw_message * cfw_alloc_internal_msg(int msg_id, int size, void * priv) {
     struct cfw_message * evt = (struct cfw_message *) cfw_alloc_message(size, NULL);
+    if (NULL == evt)
+    {
+        return NULL;
+    }
+    
     CFW_MESSAGE_TYPE(evt) = TYPE_INT;
     CFW_MESSAGE_ID(evt) = msg_id;
     CFW_MESSAGE_LEN(evt) = size;
@@ -122,54 +127,6 @@ struct cfw_message * cfw_alloc_internal_msg(int msg_id, int size, void * priv) {
     return evt;
 }
 
-void default_msg_handler(struct cfw_message * msg, void *data) {
-    pr_error(LOG_MODULE_CFW, "Bug: %s should not be called data: %p", __func__, data);
-    cfw_dump_message(msg);
-}
-
-void client_handle_message(struct cfw_message * msg, void *param) {
-    _cfw_handle_t * h = (_cfw_handle_t*)param;
-    switch(CFW_MESSAGE_ID(msg)) {
-    case MSG_ID_CFW_OPEN_SERVICE:
-    {
-        cfw_open_conn_rsp_msg_t * cnf = (cfw_open_conn_rsp_msg_t *) msg;
-        /** Make client handle point to server handle */
-        ((svc_client_handle_t*)cnf->client_handle)->server_handle = cnf->svc_server_handle;
-        /** Initialize service port. */
-        ((svc_client_handle_t*)cnf->client_handle)->port = cnf->port;
-#ifndef CONFIG_INFRA_IS_MASTER
-        /* Set local port and cpu id */
-        if (get_cpu_id() != cnf->cpu_id) {
-            port_set_port_id(cnf->port);
-            port_set_cpu_id(cnf->port, cnf->cpu_id);
-        }
-#endif
-        break;
-    }
-    case MSG_ID_CFW_CLOSE_SERVICE:
-    {
-        /* Free client-side conn */
-        bfree(msg->conn);
-        break;
-    }
-    default:
-        //Nothing to do
-        break;
-    }
-    h->handle_msg(msg, h->data);
-}
-
-cfw_handle_t cfw_init(void * queue, handle_msg_cb_t cb, void *cb_data) {
-    _cfw_handle_t * handle = (_cfw_handle_t*)balloc(sizeof(*handle), NULL);
-    handle->handle_msg = cb;
-    handle->data = cb_data;
-
-    handle->client_port_id = port_alloc(queue);
-
-    cfw_port_set_handler(handle->client_port_id, client_handle_message, handle);
-
-    return (cfw_handle_t) handle;
-}
 
 int _cfw_send_message(struct cfw_message * message)
 {
diff --git a/system/libarc32_arduino101/framework/src/cfw_platform.c b/system/libarc32_arduino101/framework/src/cfw_platform.c
index b2da0288..5daa0951 100644
--- a/system/libarc32_arduino101/framework/src/cfw_platform.c
+++ b/system/libarc32_arduino101/framework/src/cfw_platform.c
@@ -42,10 +42,11 @@
 
 #include "cfw/cfw_messages.h"
 
-#include "nordic_interface.h"
+#include "nble_driver.h"
 
 /* FIXME: Service manager API */
 extern void _cfw_init(void *);
+extern void ble_cfw_service_init(int service_id, T_QUEUE queue);
 
 extern void *services;
 
@@ -81,21 +82,6 @@ static void free_message_ipc(struct message *msg) {
 extern "C" {
 #endif
 
-
-void cfw_platform_nordic_init(void)
-{
-    /* Setup UART0 for BLE communication, HW flow control required  */
-    SET_PIN_MODE(18, QRK_PMUX_SEL_MODEA); /* UART0_RXD        */
-    SET_PIN_MODE(19, QRK_PMUX_SEL_MODEA); /* UART0_TXD        */
-    SET_PIN_MODE(40, QRK_PMUX_SEL_MODEB); /* UART0_CTS_B      */
-    SET_PIN_MODE(41, QRK_PMUX_SEL_MODEB); /* UART0_RTS_B      */
-
-    /* Reset the nordic to force sync - Warning: not working everytime */
-    nordic_interface_init(service_mgr_queue);
-    uart_ipc_init(0);
-    uart_ipc_set_channel(uart_ipc_channel_open(SYNC_CHANNEL, uart_ipc_message_cback));
-}
-
 static void cfw_platform_mbx_int_enable(void)
 {
     /* Set up mailbox interrupt handler */
@@ -116,8 +102,6 @@ void cfw_platform_init(void)
     set_cpu_id(CPU_ID_ARC);
     set_cpu_message_sender(ipc_remote_cpu, send_message_ipc);
     set_cpu_free_handler(ipc_remote_cpu, free_message_ipc);
-    set_cpu_message_sender(CPU_ID_BLE, send_message_ipc_uart);
-    set_cpu_free_handler(CPU_ID_BLE, free_message_ipc_uart);
 
     service_mgr_queue = queue_create(IPC_QUEUE_DEPTH, NULL);
 
@@ -131,6 +115,7 @@ void cfw_platform_init(void)
                     shared_data->services, shared_data->service_mgr_port_id);
 #else
     _cfw_init(service_mgr_queue);
+    ble_cfw_service_init(BLE_SERVICE_ID, service_mgr_queue);
 
     /* Initialized shared structure. */
     shared_data->ports = port_get_port_table();
diff --git a/system/libarc32_arduino101/framework/src/infra/log.c b/system/libarc32_arduino101/framework/src/infra/log.c
index c0d0ab7a..7e21decf 100644
--- a/system/libarc32_arduino101/framework/src/infra/log.c
+++ b/system/libarc32_arduino101/framework/src/infra/log.c
@@ -62,10 +62,12 @@ void log_init()
 	uint8_t i;
 
 	for (i = 0; i < LOG_MODULE_NUM; i++) {
-		global_infos.modules_filter[i].status = 1;
-		global_infos.modules_filter[i].log_level = LOG_LEVEL_INFO;
+		global_infos.modules_filter[i].status = true;
+		global_infos.modules_filter[i].log_level = LOG_LEVEL_DEBUG;
 	}
-	global_infos.log_level_limit = LOG_LEVEL_INFO;
+	global_infos.log_level_limit = LOG_LEVEL_DEBUG;
+	
+    global_infos.modules_filter[LOG_MODULE_MAIN].log_level = LOG_LEVEL_INFO;
 
 	log_impl_init();
 }
diff --git a/system/libarc32_arduino101/framework/src/infra/log_impl_printk.c b/system/libarc32_arduino101/framework/src/infra/log_impl_printk.c
index ed8f1e99..958b7521 100644
--- a/system/libarc32_arduino101/framework/src/infra/log_impl_printk.c
+++ b/system/libarc32_arduino101/framework/src/infra/log_impl_printk.c
@@ -32,13 +32,14 @@
 #include "log_impl.h"
 #include "infra/log_backend.h"
 
-extern int printk(const char * format, ...);
+//extern int printk(const char * format, ...);
+extern void printk(const char *fmt, va_list args);
 
 uint32_t log_write_msg(uint8_t level, uint8_t module, const char *format,
 				va_list args)
 {
 // TODO - implement printk
-//	printk(format, args);
+	printk(format, args);
 	return 0;
 }
 
diff --git a/system/libarc32_arduino101/framework/src/infra/port.c b/system/libarc32_arduino101/framework/src/infra/port.c
index e9f7eef6..d150ccd3 100644
--- a/system/libarc32_arduino101/framework/src/infra/port.c
+++ b/system/libarc32_arduino101/framework/src/infra/port.c
@@ -121,6 +121,7 @@ static struct port * get_port(uint16_t port_id)
 	if (port_id == 0 || port_id > MAX_PORTS) {
 		pr_error(LOG_MODULE_MAIN, "Invalid port: %d", port_id);
 		panic(-1); /*TODO: replace with an assert */
+        return NULL;
 	}
 	return &ports[port_id - 1];
 }
@@ -128,7 +129,10 @@ static struct port * get_port(uint16_t port_id)
 void port_set_queue(uint16_t port_id, void * queue)
 {
 	struct port * p = get_port(port_id);
-	p->queue = queue;
+    if (p)
+    {
+        p->queue = queue;
+    }
 }
 
 #ifdef CONFIG_INFRA_IS_MASTER
@@ -175,8 +179,11 @@ uint16_t port_alloc(void *queue)
 void port_set_handler(uint16_t port_id, void (*handler)(struct message*, void*), void *param)
 {
 	struct port * port = get_port(port_id);
-	port->handle_message = handler;
-	port->handle_param = param;
+    if (port)
+    {
+        port->handle_message = handler;
+        port->handle_param = param;
+    }
 }
 
 struct message * message_alloc(int size, OS_ERR_TYPE * err)
@@ -192,7 +199,7 @@ struct message * message_alloc(int size, OS_ERR_TYPE * err)
 void port_process_message(struct message * msg)
 {
 	struct port * p = get_port(msg->dst_port_id);
-	if (p->handle_message != NULL) {
+	if (p && p->handle_message != NULL) {
 		p->handle_message(msg, p->handle_param);
 	}
 }
@@ -200,19 +207,32 @@ void port_process_message(struct message * msg)
 void port_set_cpu_id(uint16_t port_id, uint8_t cpu_id)
 {
 	struct port * p = get_port(port_id);
-	p->cpu_id = cpu_id;
+    if (p)
+    {
+        p->cpu_id = cpu_id;
+    }
 }
 
 void port_set_port_id(uint16_t port_id)
 {
 	struct port * p = get_port(port_id);
-	p->id = port_id;
+    if (p)
+    {
+        p->id = port_id;
+    }
 }
 
 uint8_t port_get_cpu_id(uint16_t port_id)
 {
 	struct port * p = get_port(port_id);
-	return p->cpu_id;
+    if (p)
+    {
+	    return p->cpu_id;
+    }
+    else
+    {
+        return 0;
+    }
 }
 
 #ifdef INFRA_MULTI_CPU_SUPPORT
@@ -257,7 +277,7 @@ int port_send_message(struct message * message)
             pr_info(LOG_MODULE_MAIN, "Sending message %p to port %p(q:%p) ret: %d", message, port, port->queue, err);
 #endif
             struct port *src_port = get_port(MESSAGE_SRC(message));
-            if (src_port->cpu_id == get_cpu_id()) {
+            if (src_port && src_port->cpu_id == get_cpu_id()) {
                 /* We bypass the software queue here and process directly
                  * due to lack of background thread on this implementation
                  */
@@ -277,6 +297,10 @@ int port_send_message(struct message * message)
 void message_free(struct message * msg)
 {
     struct port * port = get_port(MESSAGE_SRC(msg));
+    if (!port)
+    {
+        return;
+    }
     pr_debug(LOG_MODULE_MAIN, "free message %p: port %p[%d] this %d id %d",
             msg, port, port->cpu_id, get_cpu_id(), MESSAGE_SRC(msg));
     if (port->cpu_id == get_cpu_id()) {
@@ -290,8 +314,12 @@ void message_free(struct message * msg)
 
 int port_send_message(struct message * msg)
 {
-	struct port * port = get_port(MESSAGE_DST(msg));
 	OS_ERR_TYPE err;
+	struct port * port = get_port(MESSAGE_DST(msg));
+    if (!port)
+    {
+        return E_OS_ERR_NO_MEMORY;
+    }
         if (src_port->cpu_id == get_cpu_id()) {
             /* We bypass the software queue here and process directly
              * due to lack of background thread on this implementation
@@ -317,7 +345,7 @@ uint16_t queue_process_message(T_QUEUE queue)
        uint16_t id = 0;
        queue_get_message(queue, &m, OS_NO_WAIT, &err);
        message = (struct message *) m;
-       if ( message != NULL && err == E_OS_OK) {
+       if ( message != NULL) { // && err == E_OS_OK dismiss Klock scan issue
                id = MESSAGE_ID(message);
                port_process_message(message);
        }
diff --git a/system/libarc32_arduino101/framework/src/nordic_interface.c b/system/libarc32_arduino101/framework/src/nordic_interface.c
deleted file mode 100644
index daee0f12..00000000
--- a/system/libarc32_arduino101/framework/src/nordic_interface.c
+++ /dev/null
@@ -1,192 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <string.h>
-#include "os/os.h"
-#include "cfw/cfw.h"
-#include "cfw/cfw_debug.h"
-#include "cfw/cfw_internal.h"
-#include "cfw/cfw_messages.h"
-#include "infra/ipc_uart.h"
-#include "infra/ipc_requests.h"
-#include "infra/log.h"
-#include "infra/message.h"
-#include "infra/time.h"
-#include "drivers/soc_gpio.h"
-#include "platform.h"
-
-#include "portable.h"
-
-/* Macro definition for reset pin */
-#define BLE_SW_CLK_PIN  27
-#define BLE_SWDIO_PIN   6
-#define RESET_PIN       BLE_SWDIO_PIN
-#define ATP_BLE_INT     5
-
-static T_QUEUE service_mgr_queue;
-
-/* When we set val to 1 it will wake up the Nordic, setting it to 0
- * will set it back to sleep. */
-static int nordic_wake_assert(bool val)
-{
-	uint8_t ret = soc_gpio_write(SOC_GPIO_32_ID, ATP_BLE_INT, val);
-	if (ret != DRV_RC_OK)
-		pr_error(LOG_MODULE_IPC, "Error setting ATP_BLE_INT %d", val);
-	return ret;
-}
-
-void nordic_wake_deassert(void* ignored)
-{
-	nordic_wake_assert(0);
-}
-
-int send_message_ipc_uart(struct cfw_message * message) {
-	int ret = uart_ipc_send_message(uart_ipc_get_channel(),
-			CFW_MESSAGE_LEN(message), message);
-	message_free((struct message *)message);
-	return ret;
-}
-
-void free_message_ipc_uart(void * ptr) {
-	bfree(ptr);
-}
-
-/**
- * IPC CFW message format is the following (little endian):
- * -------------------------------------
- * |  len: 2 bytes  | chan 1 byte: sender cpu id: 1 byte |
- * -------------------------------------
- * | REQUEST_ID   | payload             |
- * -------------------------------------
- *
- * For TYPE_MESSAGE request, the payload is the message copy.
- * For TYPE_FREE is not valid (this ipc is not shared mem based)
- */
-void uart_ipc_message_cback(uint8_t cpu_id, int channel, int len, void * p_data)
-{
-	struct cfw_message * msg;
-	unsigned int request = *(uint32_t*)p_data;
-
-	switch (request) {
-		case IPC_MSG_TYPE_MESSAGE:
-		{
-			OS_ERR_TYPE error = E_OS_OK;
-			int size = len - sizeof(uint32_t);
-			msg = (struct cfw_message *)message_alloc(size, &error);
-			if (error != E_OS_OK)
-				pr_error(LOG_MODULE_IPC, "NO MEM: error: %d size: %d", error, size);
-			else {
-				memcpy(msg, (uint8_t *)p_data + sizeof(uint32_t), size);
-				handle_ipc_sync_request(cpu_id, request, 0, 0, msg);
-			}
-			break;
-		}
-		case IPC_REQUEST_ALLOC_PORT:
-		{
-			unsigned int ret;
-			unsigned int error;
-			uint16_t port_id = port_alloc(NULL);
-			port_set_cpu_id(port_id, cpu_id);
-			ret = port_id;
-			pr_info(LOG_MODULE_IPC, "%s return port_id %d", __func__, ret);
-			error = uart_ipc_send_sync_resp(channel, request, ret, 0, NULL);
-			if (error)
-				pr_error(LOG_MODULE_IPC, "%s returned error from ipc uart sync resp %d", __func__, error);
-			break;
-		}
-		default:
-		{
-			unsigned int error;
-			int32_t * p = ((int32_t *) p_data) + 1;
-			int32_t param0 = *p++;
-			int32_t param1 = *p++;
-			void * ptr = (void *) *p;
-			pr_info(LOG_MODULE_IPC, "%s request %xh, param1 %d, param2 %d", __func__, request, param0, param1);
-			handle_ipc_sync_request(cpu_id, request, param0, param1, ptr);
-			error = uart_ipc_send_sync_resp(channel, request, 0, 0, NULL);
-			if (error)
-				pr_error(LOG_MODULE_IPC, "%s returned error from ipc uart sync resp %d", __func__, error);
-			break;
-		}
-	}
-	bfree(p_data);
-
-	/* Dequeue and process any new messages received */
-	while(queue_process_message(service_mgr_queue) != 0);
-}
-
-/* Nordic reset is achieved by asserting low the SWDIO pin.
- * However, the Nordic chip can be in SWD debug mode, and NRF_POWER->RESET = 0 due to,
- * other constraints: therefore, this reset might not work everytime, especially after
- * flashing or debugging.
- */
-static int nordic_reset(void)
-{
-	/* RESET_PIN depends on the board and the local configuration: check top of file */
-	uint32_t delay_until;
-	gpio_cfg_data_t pin_cfg = { .gpio_type = GPIO_OUTPUT };
-
-	soc_gpio_set_config(SOC_GPIO_32_ID, RESET_PIN, &pin_cfg);
-
-	/* Reset hold time is 0.2us (normal) or 100us (SWD debug) */
-	soc_gpio_write(SOC_GPIO_32_ID, RESET_PIN, 0);
-	/* Wait for ~1ms */
-	delay_until = get_uptime_32k() + 32768;
-	while (get_uptime_32k() < delay_until);
-	/* De-assert the reset */
-	soc_gpio_write(SOC_GPIO_32_ID, RESET_PIN, 1);
-
-	/* Set back GPIO to input to avoid interfering with external debugger */
-	pin_cfg.gpio_type = GPIO_INPUT;
-	soc_gpio_set_config(SOC_GPIO_32_ID, RESET_PIN, &pin_cfg);
-
-	return 0;
-}
-
-int nordic_interface_init(T_QUEUE queue)
-{
-	uint8_t ret;
-	gpio_cfg_data_t config;
-
-	service_mgr_queue = queue;
-
-	config.gpio_type = GPIO_OUTPUT;
-	ret = soc_gpio_set_config(SOC_GPIO_32_ID, ATP_BLE_INT, &config);
-	if (ret != DRV_RC_OK)
-		return -1;
-	ret = nordic_wake_assert(1);
-	if (ret != DRV_RC_OK)
-		return -1;
-	ret = nordic_reset();
-	if (ret != DRV_RC_OK)
-		return -1;
-
-	return 0;
-}
diff --git a/system/libarc32_arduino101/framework/src/os/balloc.c b/system/libarc32_arduino101/framework/src/os/balloc.c
new file mode 100644
index 00000000..aa2906b3
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/os/balloc.c
@@ -0,0 +1,517 @@
+
+/**
+ * @ingroup os_mem_alloc Memory Allocation
+ * Defines balloc and bfree functions for dynamic memory allocation.
+ * @{
+ */
+
+#include <stdio.h>
+
+#include "os/os.h"
+#include "infra/log.h"
+#include "dccm_alloc.h"
+
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_TRACK_OWNER
+#include "misc/printk.h"
+#include <string.h>
+#endif
+extern void panic(int x);
+#define BITS_PER_U32 (sizeof(uint32_t) * 8)
+
+/** If defined, allow to use a block larger than required when all smaller blocks are already reserved */
+#define MALLOC_ALLOW_OUTCLASS
+
+/** Descriptor for a memory pool */
+typedef struct {
+    uint32_t *track;        /** block allocation tracker */
+    uint32_t start;         /** start address of the pool */
+    uint32_t end;           /** end address of the pool */
+    uint16_t count;         /** total number of blocks within the pool */
+    uint16_t size;          /** size of each memory block within the pool */
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_STATISTICS
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_TRACK_OWNER
+    uint32_t **owners;
+#endif
+    uint32_t max;           /** maximum number of allocated blocks at the same time */
+    uint32_t cur;           /** current number of allocated blocks */
+    uint32_t sum;           /** Cumulative size in bytes */
+    uint32_t nbrs;          /** Cumulative block allocated */
+#endif
+}T_POOL_DESC;
+
+/**********************************************************
+************** Private variables  ************************
+**********************************************************/
+
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_STATISTICS
+
+/** Allocate the memory blocks and tracking variables for each pool */
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_TRACK_OWNER
+#define DECLARE_MEMORY_POOL(index, size, count) \
+    uint8_t mblock_ ## index[count][size] __aligned(4); \
+    uint32_t mblock_alloc_track_ ## index[count / BITS_PER_U32 + 1] = { 0 }; \
+    uint32_t *mblock_owners_ ## index[count] = { 0 };
+#else
+#define DECLARE_MEMORY_POOL(index, size, count) \
+    uint8_t mblock_ ## index[count][size] __aligned(4); \
+    uint32_t mblock_alloc_track_ ## index[count / BITS_PER_U32 + \
+                          1] = { 0 };
+#endif
+
+#include "memory_pool_list.def"
+
+/** Pool descriptor definition */
+T_POOL_DESC mpool[] =
+{
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_TRACK_OWNER
+#define DECLARE_MEMORY_POOL(index, size, count) \
+    { \
+/* T_POOL_DESC.track */ mblock_alloc_track_ ## index, \
+/* T_POOL_DESC.start */ (uint32_t)mblock_ ## index, \
+/* T_POOL_DESC.end */ (uint32_t)mblock_ ## index + count * size, \
+/* T_POOL_DESC.count */ count, \
+/* T_POOL_DESC.size */ size, \
+/* T_POOL_DESC.owners */ mblock_owners_ ## index, \
+/* T_POOL_DESC.max */ 0, \
+/* T_POOL_DESC.cur */ 0, \
+/* T_POOL_DESC.sum */ 0, \
+/* T_POOL_DESC.nbrs */ 0 \
+    },
+#else
+#define DECLARE_MEMORY_POOL(index, size, count) \
+    { \
+/* T_POOL_DESC.track */ mblock_alloc_track_ ## index, \
+/* T_POOL_DESC.start */ (uint32_t)mblock_ ## index, \
+/* T_POOL_DESC.end */ (uint32_t)mblock_ ## index + count * size, \
+/* T_POOL_DESC.count */ count, \
+/* T_POOL_DESC.size */ size, \
+/* T_POOL_DESC.max */ 0, \
+/* T_POOL_DESC.cur */ 0, \
+/* T_POOL_DESC.sum */ 0, \
+/* T_POOL_DESC.nbrs */ 0 \
+    },
+#endif
+
+#include "memory_pool_list.def"
+};
+
+
+#else
+
+/** Allocate the memory blocks and tracking variables for each pool */
+#define DECLARE_MEMORY_POOL(index, size, count) \
+    uint32_t mblock_alloc_track_ ## index[count / BITS_PER_U32 + 1] = { 0 };
+
+#include "memory_pool_list.def"
+
+
+/** Pool descriptor definition */
+T_POOL_DESC mpool [] =
+{
+#define DECLARE_MEMORY_POOL(index, size, count) \
+    { \
+/* T_POOL_DESC.track */ mblock_alloc_track_ ## index, \
+/* T_POOL_DESC.start */ 0, \
+/* T_POOL_DESC.end */ 0, \
+/* T_POOL_DESC.count */ count, \
+/* T_POOL_DESC.size */ size \
+    },
+
+#include "memory_pool_list.def"
+};
+
+
+
+#endif
+
+
+/** Number of memory pools */
+#define NB_MEMORY_POOLS   (sizeof(mpool) / sizeof(T_POOL_DESC))
+
+/**********************************************************
+************** Private functions  ************************
+**********************************************************/
+
+/**
+ * Return the next free block of a pool and
+ *   mark it as reserved/allocated.
+ *
+ * @param pool index of the pool in mpool
+ *
+ * @return allocated buffer or NULL if none is
+ *   available
+ */
+static void *memblock_alloc(uint32_t pool)
+{
+    uint16_t block;
+    uint32_t flags = interrupt_lock();//irq_lock();
+
+    for (block = 0; block < mpool[pool].count; block++) {
+        if (((mpool[pool].track)[block / BITS_PER_U32] & 1 <<
+             (BITS_PER_U32 - 1 - (block % BITS_PER_U32))) == 0) {
+            (mpool[pool].track)[block / BITS_PER_U32] =
+                (mpool[pool].track)[block / BITS_PER_U32] |
+                (1 << (BITS_PER_U32 - 1 - (block % BITS_PER_U32)));
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_STATISTICS
+            mpool[pool].cur = mpool[pool].cur + 1;
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_TRACK_OWNER
+            /* get return address */
+            uint32_t ret_a = (uint32_t)__builtin_return_address(0);
+            mpool[pool].owners[block] =
+                (uint32_t *)(((ret_a & 0xFFFF0U) >> 4) |
+                         ((get_uptime_ms() & 0xFFFF0) << 12));
+#endif
+            if (mpool[pool].cur > mpool[pool].max)
+                mpool[pool].max = mpool[pool].cur;
+#endif
+            interrupt_unlock(flags);//irq_unlock(flags);
+            return (void *)(mpool[pool].start +
+                    mpool[pool].size * block);
+        }
+    }
+    //irq_unlock(flags);
+    interrupt_unlock(flags);
+    return NULL;
+}
+
+
+
+/**
+ * Free an allocated block from a pool.
+ *
+ * @param pool index of the pool in mpool
+ *
+ * @param ptr points to the start of the block
+ *     to free
+ *
+ */
+static void memblock_free(uint32_t pool, void *ptr)
+{
+    uint16_t block;
+    uint32_t flags;
+
+    block = ((uint32_t)ptr - mpool[pool].start) / mpool[pool].size;
+    if (block < mpool[pool].count) {
+        flags = interrupt_lock();//irq_lock();
+        (mpool[pool].track)[block / BITS_PER_U32] &=
+            ~(1 << (BITS_PER_U32 - 1 - (block % BITS_PER_U32)));
+        interrupt_unlock(flags);//irq_unlock(flags);
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_STATISTICS
+        mpool[pool].cur = mpool[pool].cur - 1;
+#endif
+    } else {
+        pr_debug(
+            LOG_MODULE_UTIL,
+            "ERR: memblock_free: ptr 0x%X is not within pool %d [0x%X , 0x%X]",
+            ptr, pool, mpool[pool].start, mpool[pool].end);
+    }
+}
+
+
+
+
+/**
+ * Test if a block is allocated.
+ *
+ * @param pool index of the pool in mpool
+ *
+ * @param ptr points to the start of the block
+ *
+ * @return true if the block is allocated/reserved,
+ *   false if the block is free
+ *
+ */
+static bool memblock_used(uint32_t pool, void *ptr)
+{
+    uint16_t block;
+
+    block = ((uint32_t)ptr - mpool[pool].start) / mpool[pool].size;
+    if (block < mpool[pool].count) {
+        if (((mpool[pool].track)[block / BITS_PER_U32] &
+             (1 << (BITS_PER_U32 - 1 - (block % BITS_PER_U32)))) != 0)
+            return true;
+    }
+    return false;
+}
+
+
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_STATISTICS
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_TRACK_OWNER
+
+#define PRINT_METHOD_PRINTK   0
+#define PRINT_METHOD_TCMD_RSP 1
+#define PRINT_METHOD_PR_INFO  2
+#define PRINT_POOL(method, str, ctx) \
+    do { \
+        if (method == PRINT_METHOD_PRINTK) { \
+            printk("%s\n", str); } \
+        else if (method == PRINT_METHOD_TCMD_RSP) { \
+            TCMD_RSP_PROVISIONAL(((struct tcmd_handler_ctx *)ctx), \
+                         str); } \
+        else if (method == PRINT_METHOD_PR_INFO) { \
+            pr_info(LOG_MODULE_UTIL, str); \
+            local_task_sleep_ms(100); \
+        } \
+    } while (0);
+static void print_pool(int method, void *ctx)
+{
+    char tmp[128];
+    uint32_t pool;
+    uint32_t average;
+    uint16_t block;
+    uint8_t str_count;
+    char *cur = tmp;
+
+    for (pool = 0; pool < NB_MEMORY_POOLS; pool++) {
+        str_count = 0;
+        average = 0;
+        if (mpool[pool].nbrs)
+            average = mpool[pool].sum / mpool[pool].nbrs;
+        snprintf(
+            tmp, sizeof(tmp),
+            "\npool %-4d bytes count:%-2d cur:%-2d max:%-2d avg: %-3d \n",
+            mpool[pool].size,
+            mpool[pool].count,
+            mpool[pool].cur,
+            mpool[pool].max,
+            average);
+        PRINT_POOL(method, tmp, ctx);
+
+        memset(tmp, 0, sizeof(tmp));
+        str_count = 0;
+
+        for (block = 0; block < mpool[pool].count; block++) {
+            if (((mpool[pool].track)[block / BITS_PER_U32] & 1 <<
+                 (BITS_PER_U32 - 1 -
+                  (block % BITS_PER_U32)))) {
+                if (str_count == 0) {
+                    cur = tmp;
+                    PRINT_POOL(method, " owners:", ctx);
+                }
+                str_count++;
+                snprintf(cur, 7, " T%04u",
+                     (((uint32_t)mpool[pool].owners[
+                           block]) &
+                      (uint32_t)0xFFFF0000) >> 16);
+                cur += 6;
+                snprintf(cur, 6, "C%04x",
+                     (((uint32_t)mpool[pool].owners[
+                           block]) &
+                      (uint32_t)0xFFFF));
+                cur += 5; /* hack to print the owner */
+                if (str_count % 4 == 0) {
+                    PRINT_POOL(method, tmp, ctx);
+                    memset(tmp, 0, sizeof(tmp));
+                    cur = tmp;
+                }
+            }
+        }
+        if (str_count % 4)
+            PRINT_POOL(method, tmp, ctx);
+    }
+    PRINT_POOL(method, "*** END", ctx);
+}
+
+#endif
+#endif
+
+
+/**********************************************************
+************** Exported functions ************************
+**********************************************************/
+
+/*----- Initialization  */
+
+/**
+ * Initialize the resources used by the framework's memory allocation services
+ *
+ * IMPORTANT : This function must be called during the initialization
+ *             of the OS abstraction layer.
+ *             This function shall only be called once after reset.
+ */
+void os_abstraction_init_malloc(void)
+{
+  int indx;
+  uint32_t bufSize;
+
+  for (indx=0; indx < NB_MEMORY_POOLS; indx++) {
+    bufSize = mpool[indx].count * mpool[indx].size;
+    mpool[indx].start = (uint32_t)dccm_memalign((uint16_t)bufSize);
+    mpool[indx].end = mpool[indx].start + bufSize;
+  }
+}
+
+/**
+ * Reserves a block of memory.
+ *
+ * Authorized execution levels:  task, fiber, ISR
+ *
+ * This function returns a pointer on the start of
+ * a reserved memory block whose size is equal or
+ * larger than the requested size.
+ *
+ * The returned pointer shall be null if the function
+ * fails.
+ *
+ * This function may panic if err is null and
+ *  - size is null or bigger than allowed, or
+ *  - there is not enough available memory
+ *
+ * @param size number of bytes to reserve
+ *
+ *
+ * @param err execution status:
+ *    E_OS_OK : block was successfully reserved
+ *    E_OS_ERR : size is null
+ *    E_OS_ERR_NO_MEMORY: there is not enough available
+ *              memory
+ *    E_OS_ERR_NOT_ALLOWED : size is bigger than the
+ *         biggest block size defined in os_config.h
+ *
+ * @return pointer to the reserved memory block
+ *    or null if no block is available
+ */
+void *balloc(uint32_t size, OS_ERR_TYPE *err)
+{
+    OS_ERR_TYPE localErr = E_OS_OK;
+    void *buffer = NULL;
+    uint8_t poolIdx;
+
+    if (size > 0) {
+        /* find the first block size greater or equal to requested size */
+        poolIdx = 0;
+        while (poolIdx < NB_MEMORY_POOLS &&
+               (size > mpool[poolIdx].size))
+            poolIdx++;
+
+        /* reserve the block */
+        if (poolIdx < NB_MEMORY_POOLS) {
+#ifdef MALLOC_ALLOW_OUTCLASS
+            /* loop until an available (maybe larger) block is found */
+            do {
+                if (size <= mpool[poolIdx].size) { /* this condition may be false if pools are not sorted according to block size */
+#endif
+            buffer = memblock_alloc(poolIdx);
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_STATISTICS
+            if ((buffer != NULL) &&
+                ((poolIdx == 0) ||
+                 (size > mpool[poolIdx - 1].size))) {
+                mpool[poolIdx].nbrs += 1;
+                mpool[poolIdx].sum += size;
+            }
+#endif
+#ifdef MALLOC_ALLOW_OUTCLASS
+        }
+
+        if (NULL == buffer)
+            poolIdx++;
+    }
+    while ((poolIdx < NB_MEMORY_POOLS) && (NULL == buffer)) ;
+#endif
+            if (NULL == buffer) { /* All blocks of relevant size are already reserved */
+                pr_debug(LOG_MODULE_UTIL,
+                     "Attempt to allocate %d bytes failed",
+                     size);
+                localErr = E_OS_ERR_NO_MEMORY;
+            }
+        } else { /* Configuration does not define blocks large enough for the requested size */
+            localErr = E_OS_ERR_NOT_ALLOWED;
+        }
+    } else { /* invalid function parameter */
+        localErr = E_OS_ERR;
+    }
+
+    /* set err or panic if err == NULL and localErr != E_OS_OK */
+    if (err != NULL) {
+        *err = localErr;
+    } else {
+        if (localErr != E_OS_OK) {
+#ifdef CONFIG_MEMORY_POOLS_BALLOC_TRACK_OWNER
+#ifdef CONFIG_NANOKERNEL
+            /* disable stack checking */
+            uint32_t status32 = _arc_v2_aux_reg_read(
+                _ARC_V2_STATUS32);
+            status32 &= ~(_ARC_V2_STATUS32_SC);
+            __asm__ volatile ("kflag %0" : : "ir" (status32));
+#endif
+            if (localErr == E_OS_ERR_NO_MEMORY)
+                print_pool(PRINT_METHOD_PRINTK, NULL);
+#endif
+            panic(localErr);
+        }
+    }
+
+    return buffer;
+}
+
+/**
+ * Frees a block of memory.
+ *
+ * Authorized execution levels:  task, fiber, ISR
+ *
+ * This function frees a memory block that was
+ * reserved by malloc.
+ *
+ * The "buffer" parameter must point to the
+ * start of the reserved block (i.e. it shall
+ * be a pointer returned by malloc).
+ *
+ * @param buffer pointer returned by malloc
+ *
+ * @return execution status:
+ *    E_OS_OK : block was successfully freed
+ *    E_OS_ERR : "buffer" param did not match
+ *        any reserved block
+ */
+OS_ERR_TYPE bfree(void *buffer)
+{
+    OS_ERR_TYPE err = E_OS_ERR;
+    uint8_t poolIdx;
+
+    /* find which pool the buffer was allocated from */
+    poolIdx = 0;
+    while ((NULL != buffer) && (poolIdx < NB_MEMORY_POOLS)) {
+        /* check if buffer is within mpool[poolIdx] */
+        if (((uint32_t)buffer >= mpool[poolIdx].start) &&
+            ((uint32_t)buffer < mpool[poolIdx].end)) {
+            if (false != memblock_used(poolIdx, buffer)) {
+                memblock_free(poolIdx, buffer);
+                err = E_OS_OK;
+            }
+            /* else: buffer is not marked as used, keep err = E_OS_ERR */
+            else {
+                pr_debug(
+                    LOG_MODULE_UTIL,
+                    "ERR: memory_free: buffer %p is already free\n",
+                    buffer);
+            }
+            buffer = NULL;  /* buffer was found in the pools, end the loop */
+        } else {                /* buffer does not belong to mpool[poolIdx], go to the next one */
+            poolIdx++;
+        }
+    }
+    return err;
+}
+
+
+#ifdef CONFIG_DBG_POOL_TCMD
+
+void tcmd_pool(int argc, char *argv[], struct tcmd_handler_ctx *ctx)
+{
+#ifdef CONFIG_QUARK
+    /* Display with TCMD response on Quark */
+    print_pool(PRINT_METHOD_TCMD_RSP, ctx);
+#endif
+#ifdef CONFIG_ARC
+    /* Display with pr_info on ARC to avoid message overflow and panic */
+    print_pool(PRINT_METHOD_PR_INFO, ctx);
+#endif
+    TCMD_RSP_FINAL(ctx, "");
+}
+
+
+DECLARE_TEST_COMMAND_ENG(dbg, pool, tcmd_pool);
+
+#endif
+
+/** @} */
diff --git a/system/libarc32_arduino101/framework/src/os/memory_pool_list.def b/system/libarc32_arduino101/framework/src/os/memory_pool_list.def
new file mode 100644
index 00000000..b331f9b7
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/os/memory_pool_list.def
@@ -0,0 +1,21 @@
+/*
+ * Definition of the memory pools used by balloc/bfree:
+ *  DECLARE_MEMORY_POOL( <index>, <size>, <count>, <align> )
+ *  <index> : must start at 0 and be of consecutive values *
+ *  <size>  : size in bytes of each block from the pool
+ *  <count> : number of blocks in the pool
+ *
+ *  * Pool definitions must be sorted according the block size
+ *  value: pool with <index> 0 must have the smallest <size>.
+ */
+
+DECLARE_MEMORY_POOL(0,8,32)
+DECLARE_MEMORY_POOL(1,16,32)
+DECLARE_MEMORY_POOL(2,32,32)
+DECLARE_MEMORY_POOL(3,64,8)
+DECLARE_MEMORY_POOL(4,128,4)
+DECLARE_MEMORY_POOL(5,256,2)
+DECLARE_MEMORY_POOL(6,512,2)
+DECLARE_MEMORY_POOL(7,2096,1)
+
+#undef DECLARE_MEMORY_POOL
diff --git a/system/libarc32_arduino101/framework/src/os/os.c b/system/libarc32_arduino101/framework/src/os/os.c
index 63aa31f6..f9e6faa3 100644
--- a/system/libarc32_arduino101/framework/src/os/os.c
+++ b/system/libarc32_arduino101/framework/src/os/os.c
@@ -33,7 +33,11 @@
 
 /*************************    MEMORY   *************************/
 
+
+#if 0
+
 #ifdef TRACK_ALLOCS
+#include "infra/log.h"
 int alloc_count = 0;
 #endif
 
@@ -45,6 +49,7 @@ void * cfw_alloc(int size, OS_ERR_TYPE * err) {
         (*(int*) ptr) = size;
 #ifdef TRACK_ALLOCS
         alloc_count++;
+        pr_info(0, "alloc_count - %d", alloc_count);
 #endif
         interrupt_unlock(flags);
         return ptr;
@@ -56,6 +61,7 @@ void cfw_free(void * ptr, OS_ERR_TYPE * err) {
     int flags = interrupt_lock();
 #ifdef TRACK_ALLOCS
     alloc_count--;
+    pr_info(0, "alloc_countf - %d", alloc_count);
 #endif
     free(ptr);
     interrupt_unlock(flags);
@@ -69,6 +75,7 @@ OS_ERR_TYPE bfree(void *ptr) {
 	cfw_free(ptr, NULL);
 	return E_OS_OK;
 }
+#endif
 
 /*************************    QUEUES   *************************/
 
@@ -128,7 +135,8 @@ void queue_delete(T_QUEUE queue, OS_ERR_TYPE* err) {
     q_t * q = (q_t*) queue;
     while((element = list_get(&q->lh)) != NULL)
         list_remove(&q->lh, element);
-    cfw_free(q, NULL);
+    //cfw_free(q, NULL);
+    q->used = 0;
 }
 
 /*************************    MUTEXES   *************************/
diff --git a/system/libarc32_arduino101/framework/src/os/panic.c b/system/libarc32_arduino101/framework/src/os/panic.c
new file mode 100644
index 00000000..c77fc501
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/os/panic.c
@@ -0,0 +1,32 @@
+
+#include "os/os.h"
+
+#include "infra/log.h"
+#include "aux_regs.h"
+
+extern void _do_fault();
+void panic(int x)
+{
+    _do_fault();
+}
+
+
+void __assert_fail()
+{
+	panic(-10);
+}
+
+
+void __attribute__((weak)) _Fault(void)
+{
+    uint32_t exc_addr = aux_reg_read(ARC_V2_EFA);
+    uint32_t ecr = aux_reg_read(ARC_V2_ECR);
+
+    pr_error(0, "Exception vector: 0x%x, cause code: 0x%x, parameter 0x%x\n",
+           ARC_V2_ECR_VECTOR(ecr),
+           ARC_V2_ECR_CODE(ecr),
+           ARC_V2_ECR_PARAMETER(ecr));
+    pr_error(0, "Address 0x%x\n", exc_addr);
+    while (1);  // Sid.  Acknowledge KW warning.
+}
+
diff --git a/system/libarc32_arduino101/framework/src/services/ble/ble_protocol.h b/system/libarc32_arduino101/framework/src/services/ble/ble_protocol.h
deleted file mode 100644
index 460e7458..00000000
--- a/system/libarc32_arduino101/framework/src/services/ble/ble_protocol.h
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __BLE_PROTOCOL_H__
-#define __BLE_PROTOCOL_H__
-
-/**
- *  @defgroup ble_protocol BLE protocol definitions
- *
- * BT Spec definitions.
- * @ingroup ble_service
- * @{
- *
- * Bluetooth SIG defined macros and enum extracted from Bluetooth Spec 4.2
- */
-#define BLE_MAX_DEVICE_NAME  20 /**< Max BLE device name length, spec size: 248 */
-#define BLE_MAX_ADV_SIZE     31
-
-/** Advertising Data Type. */
-#define BLE_ADV_TYPE_FLAGS                         0x01	/* Flags */
-#define BLE_ADV_TYPE_INC_16_UUID                   0x02	/* Incomplete List of 16-bit Service Class UUIDs */
-#define BLE_ADV_TYPE_COMP_16_UUID                  0x03	/* Complete List of 16-bit Service Class UUIDs */
-#define BLE_ADV_TYPE_INC_32_UUID                   0x04	/* Incomplete List of 32-bit Service Class UUIDs */
-#define BLE_ADV_TYPE_COMP_32_UUID                  0x05	/* Complete List of 32-bit Service Class UUIDs */
-#define BLE_ADV_TYPE_INC_128_UUID                  0x06	/* Incomplete List of 128-bit Service Class UUIDs */
-#define BLE_ADV_TYPE_COMP_128_UUID                 0x07	/* Complete List of 128-bit Service Class UUIDs */
-#define BLE_ADV_TYPE_SHORT_LOCAL_NAME              0x08	/* Shortened Local Name */
-#define BLE_ADV_TYPE_COMP_LOCAL_NAME               0x09	/* Complete Local Name */
-#define BLE_ADV_TYPE_TX_POWER                      0x0A	/* Tx Power Level */
-#define BLE_ADV_TYPE_CLASS                         0x0D	/* Class of Device */
-#define BLE_ADV_TYPE_PAIRING_C                     0x0E	/* Simple Pairing Hash C */
-#define BLE_ADV_TYPE_PAIRING_C_192                 0x0E	/* Simple Pairing Hash C-192 */
-#define BLE_ADV_TYPE_PAIRING_R_192                 0x0F	/* Simple Pairing Randomizer R-192 */
-#define BLE_ADV_TYPE_DEVICE_ID                     0x10	/* Device ID */
-#define BLE_ADV_TYPE_TK_VALUE                      0x10	/* Security Manager TK Value */
-#define BLE_ADV_TYPE_OOB_FLAGS                     0x11	/* Security Manager Out of Band Flags */
-#define BLE_ADV_TYPE_CONN_INTERVAL_RANGE           0x12	/* Slave Connection Interval Range */
-#define BLE_ADV_TYPE_SERVICE_SOLICITATION_16_UUID  0x14	/* List of 16-bit Service Solicitation UUIDs */
-#define BLE_ADV_TYPE_SERVICE_SOLICITATION_32_UUID  0x1F	/* List of 32-bit Service Solicitation UUIDs */
-#define BLE_ADV_TYPE_SERVICE_SOLICITATION_128_UUID 0x15	/* List of 128-bit Service Solicitation UUIDs */
-#define BLE_ADV_TYPE_SERVICE_DATA                  0x16	/* Service Data */
-#define BLE_ADV_TYPE_SERVICE_DATA_16_UUID          0x16	/* Service Data - 16-bit UUID */
-#define BLE_ADV_TYPE_SERVICE_DATA_32_UUID          0x20	/* Service Data - 32-bit UUID */
-#define BLE_ADV_TYPE_SERVICE_DATA_128_UUID         0x21	/* Service Data - 128-bit UUID */
-#define BLE_ADV_TYPE_SEC_CONF_VALUE                0x22	/* LE Secure Connections Confirmation Value */
-#define BLE_ADV_TYPE_SEC_RANDOM_VALUE              0x23 /* LE Secure Connections Random Value */
-#define BLE_ADV_TYPE_PUB_TARGET_ADDR               0x17	/* Public Target Address */
-#define BLE_ADV_TYPE_APPEARANCE                    0x19	/* Appearance */
-#define BLE_ADV_TYPE_ADV_INTERVAL                  0x1A	/* Advertising Interval */
-#define BLE_ADV_TYPE_DEVICE_ADDR                   0x1B	/* LE Bluetooth Device Address */
-#define BLE_ADV_TYPE_ROLE                          0x1C	/* LE Role */
-#define BLE_ADV_TYPE_PAIRING_C_256                 0x1D	/* Simple Pairing Hash C-256 */
-#define BLE_ADV_TYPE_PAIRING_R_256                 0x1E	/* Simple Pairing Randomizer R-256 */
-#define BLE_ADV_TYPE_3D                            0x3D	/* 3D Information Data */
-#define BLE_ADV_TYPE_MANUFACTURER                  0xFF	/* Manufacturer Specific Data */
-
-/** BLE Service UUID Definitions. */
-#define BLE_SVC_UUID_IMMEDIATE_ALERT_SERVICE            0x1802	   /**< Immediate Alert service UUID. */
-#define BLE_SVC_UUID_LINK_LOSS_SERVICE                  0x1803	   /**< Link Loss service UUID. */
-#define BLE_SVC_UUID_TX_POWER_SERVICE                   0x1804	   /**< TX Power service UUID. */
-#define BLE_SVC_UUID_CURRENT_TIME_SERVICE               0x1805	   /**< Current Time service UUID. */
-#define BLE_SVC_UUID_REFERENCE_TIME_UPDATE_SERVICE      0x1806	   /**< Reference Time Update service UUID. */
-#define BLE_SVC_UUID_NEXT_DST_CHANGE_SERVICE            0x1807	   /**< Next Dst Change service UUID. */
-#define BLE_SVC_UUID_GLUCOSE_SERVICE                    0x1808	   /**< Glucose service UUID. */
-#define BLE_SVC_UUID_HEALTH_THERMOMETER_SERVICE         0x1809	   /**< Health Thermometer service UUID. */
-#define BLE_SVC_UUID_DEVICE_INFORMATION_SERVICE         0x180A	   /**< Device Information service UUID. */
-#define BLE_SVC_UUID_HEART_RATE_SERVICE                 0x180D	   /**< Heart Rate service UUID. */
-#define BLE_SVC_UUID_PHONE_ALERT_STATUS_SERVICE         0x180E	   /**< Phone Alert Status service UUID. */
-#define BLE_SVC_UUID_BATTERY_SERVICE                    0x180F	   /**< Battery service UUID. */
-#define BLE_SVC_UUID_BLOOD_PRESSURE_SERVICE             0x1810	   /**< Blood Pressure service UUID. */
-#define BLE_SVC_UUID_ALERT_NOTIFICATION_SERVICE         0x1811	   /**< Alert Notification service UUID. */
-#define BLE_SVC_UUID_HUMAN_INTERFACE_DEVICE_SERVICE     0x1812	   /**< Human Interface Device service UUID. */
-#define BLE_SVC_UUID_SCAN_PARAMETERS_SERVICE            0x1813	   /**< Scan Parameters service UUID. */
-#define BLE_SVC_UUID_RUNNING_SPEED_AND_CADENCE          0x1814	   /**< Running Speed and Cadence service UUID. */
-#define BLE_SVC_UUID_CYCLING_SPEED_AND_CADENCE          0x1816	   /**< Cycling Speed and Cadence service UUID. */
-
-#define BLE_SVC_GAP_ADV_FLAG_LE_LIMITED_DISC_MODE	(0x01)   /**< LE Limited Discoverable Mode. */
-#define BLE_SVC_GAP_ADV_FLAG_LE_GENERAL_DISC_MODE	(0x02)   /**< LE General Discoverable Mode. */
-#define BLE_SVC_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED	(0x04)   /**< BR/EDR not supported. */
-#define BLE_SVC_GAP_ADV_FLAG_LE_BR_EDR_CONTROLLER	(0x08)   /**< Simultaneous LE and BR/EDR, Controller. */
-#define BLE_SVC_GAP_ADV_FLAG_LE_BR_EDR_HOST		(0x10)   /**< Simultaneous LE and BR/EDR, Host. */
-#define BLE_SVC_GAP_ADV_FLAGS_LE_ONLY_LIMITED_DISC_MODE   (BLE_SVC_GAP_ADV_FLAG_LE_LIMITED_DISC_MODE | BLE_SVC_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED)   /**< LE Limited Discoverable Mode, BR/EDR not supported. */
-#define BLE_SVC_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE   (BLE_SVC_GAP_ADV_FLAG_LE_GENERAL_DISC_MODE | BLE_SVC_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED)   /**< LE General Discoverable Mode, BR/EDR not supported. */
-
-
-/**
- * Characteristics UUID definitions.
- */
-
-/* GAP */
-#define BLE_GAP_DEVICE_NAME                     0x2A00
-#define BLE_GAP_APPEARANCE                      0x2A01
-#define BLE_GAP_PER_PRIVACY_FLAG                0x2A02
-#define BLE_GAP_RECONN_ADDR                     0x2A03
-#define BLE_GAP_PREF_CONN_PARAM                 0x2A04
-
-/* DIS */
-#define BLE_DIS_MANUFACTURER_NAME               0x2A29
-#define BLE_DIS_MODEL_NB                        0x2A24
-#define BLE_DIS_SERIAL_NB                       0x2A25
-#define BLE_DIS_FW_REV                          0x2A26
-#define BLE_DIS_HW_REV                          0x2A27
-#define BLE_DIS_SW_REV                          0x2A28
-#define BLE_DIS_SYS_ID                          0x2A23
-#define BLE_DIS_CERTIF_DATA_LIST                0x2A2A
-#define BLE_DIS_PNP_ID                          0x2A50
-
-/* BATTERY */
-#define BLE_BAT_BAT_LEVEL                       0x2A19
-
-/* HR */
-#define BLE_HEART_RATE_MEASUREMENT              0x2A37
-#define BLE_HEART_RATE_SENSOR_LOCATION          0x2A38
-#define BLE_HEART_RATE_CONTROL_POINT            0x2A39
-
-/* RSC */
-#define BLE_RSC_MEASUREMENT                     0x2A53
-#define BLE_RSC_SENSOR_LOCATION                 0x2A5D
-#define BLE_RSC_SUPPORTED_FEATURE               0x2A54
-#define BLE_RSC_CONTROL_POINT                   0x2A55
-
-/*CSC */
-#define BLE_CSC_MEASUREMENT                     0x2A5B
-#define BLE_CSC_SENSOR_LOCATION                 BLE_RSC_SENSOR_LOCATION
-#define BLE_CSC_SUPPORTED_FEATURE               0x2A5C
-#define BLE_CSC_CONTROL_POINT                   0x2A55
-
-/* CP */
-#define BLE_CP_MEASUREMENT                      0x2A63
-#define BLE_CP_SENSOR_LOCATION                  BLE_RSC_SENSOR_LOCATION
-#define BLE_CP_SUPPORTED_FEATURE                0x2A65
-#define BLE_CP_POWER_VECTOR                     0x2A64
-#define BLE_CP_CONTROL_POINT                    0x2A66
-
-/* HCI status (error) codes as per BT spec */
-#define HCI_REMOTE_USER_TERMINATED_CONNECTION           0x13
-#define HCI_REMOTE_DEV_TERMINATION_DUE_TO_LOW_RESOURCES 0x14
-#define HCI_REMOTE_DEV_TERMINATION_DUE_TO_POWER_OFF     0x15
-#define HCI_LOCAL_HOST_TERMINATED_CONNECTION            0x16
-
-/** BLE GAP Appearance Characteristic definitions.
- *
- * See http://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.gap.appearance.xml
- */
-#define BLE_GAP_APPEARANCE_TYPE_UNKNOWN                                0
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_PHONE                         64
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_COMPUTER                     128
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_WATCH                        192
-#define BLE_GAP_APPEARANCE_TYPE_WATCH_SPORTS_WATCH                   193
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_CLOCK                        256
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_DISPLAY                      320
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_REMOTE_CONTROL               384
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_EYE_GLASSES                  448
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_TAG                          512
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_KEYRING                      576
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_MEDIA_PLAYER                 640
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_BARCODE_SCANNER              704
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_THERMOMETER                  768
-#define BLE_GAP_APPEARANCE_TYPE_THERMOMETER_EAR                      769
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_HEART_RATE_SENSOR            832
-#define BLE_GAP_APPEARANCE_TYPE_HEART_RATE_SENSOR_HEART_RATE_BELT    833
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_BLOOD_PRESSURE               896
-#define BLE_GAP_APPEARANCE_TYPE_BLOOD_PRESSURE_ARM                   897
-#define BLE_GAP_APPEARANCE_TYPE_BLOOD_PRESSURE_WRIST                 898
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_HID                          960
-#define BLE_GAP_APPEARANCE_TYPE_HID_KEYBOARD                         961
-#define BLE_GAP_APPEARANCE_TYPE_HID_MOUSE                            962
-#define BLE_GAP_APPEARANCE_TYPE_HID_JOYSTICK                         963
-#define BLE_GAP_APPEARANCE_TYPE_HID_GAMEPAD                          964
-#define BLE_GAP_APPEARANCE_TYPE_HID_DIGITIZERSUBTYPE                 965
-#define BLE_GAP_APPEARANCE_TYPE_HID_CARD_READER                      966
-#define BLE_GAP_APPEARANCE_TYPE_HID_DIGITAL_PEN                      967
-#define BLE_GAP_APPEARANCE_TYPE_HID_BARCODE                          968
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_GLUCOSE_METER               1024
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_RUNNING_WALKING_SENSOR      1088
-#define BLE_GAP_APPEARANCE_TYPE_RUNNING_WALKING_SENSOR_IN_SHOE      1089
-#define BLE_GAP_APPEARANCE_TYPE_RUNNING_WALKING_SENSOR_ON_SHOE      1090
-#define BLE_GAP_APPEARANCE_TYPE_RUNNING_WALKING_SENSOR_ON_HIP       1091
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_CYCLING                     1152
-#define BLE_GAP_APPEARANCE_TYPE_CYCLING_CYCLING_COMPUTER            1153
-#define BLE_GAP_APPEARANCE_TYPE_CYCLING_SPEED_SENSOR                1154
-#define BLE_GAP_APPEARANCE_TYPE_CYCLING_CADENCE_SENSOR              1155
-#define BLE_GAP_APPEARANCE_TYPE_CYCLING_POWER_SENSOR                1156
-#define BLE_GAP_APPEARANCE_TYPE_CYCLING_SPEED_CADENCE_SENSOR        1157
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_PULSE_OXIMETER              3136
-#define BLE_GAP_APPEARANCE_TYPE_PULSE_OXIMETER_FINGERTIP            3137
-#define BLE_GAP_APPEARANCE_TYPE_PULSE_OXIMETER_WRIST_WORN           3138
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_WEIGHT_SCALE                3200
-#define BLE_GAP_APPEARANCE_TYPE_GENERIC_OUTDOOR_SPORTS_ACT          5184
-#define BLE_GAP_APPEARANCE_TYPE_OUTDOOR_SPORTS_ACT_LOC_DISP         5185
-#define BLE_GAP_APPEARANCE_TYPE_OUTDOOR_SPORTS_ACT_LOC_AND_NAV_DISP 5186
-#define BLE_GAP_APPEARANCE_TYPE_OUTDOOR_SPORTS_ACT_LOC_POD          5187
-#define BLE_GAP_APPEARANCE_TYPE_OUTDOOR_SPORTS_ACT_LOC_AND_NAV_POD  5188
-
-/**
- * DTM commands, opcodes, indexes.
- */
-#define	 DTM_HCI_CMD          0x01
-#define	 DTM_HCI_OPCODE2      0x20
-
-#define DTM_HCI_STATUS_IDX    6
-#define DTM_HCI_LE_END_IDX    (DTM_HCI_STATUS_IDX + 1)
-
-/** @} */
-
-#endif
diff --git a/system/libarc32_arduino101/framework/src/services/ble/ble_service_core_int.h b/system/libarc32_arduino101/framework/src/services/ble/ble_service_core_int.h
deleted file mode 100644
index d2b9311d..00000000
--- a/system/libarc32_arduino101/framework/src/services/ble/ble_service_core_int.h
+++ /dev/null
@@ -1,207 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __BLE_SERVICE_CORE_INT_H__
-#define __BLE_SERVICE_CORE_INT_H__
-
-#include <stdint.h>
-#include "services/ble/ble_service_gap_api.h"
-#include "services/ble/ble_service_gattc_api.h"
-
-/* Forward declarations */
-struct _ble_service_cb;
-struct ble_enable_req_msg;
-
-/**
- * BLE common structures.
- */
-
-struct ble_svc_string {
-	uint8_t *p_string;		/**< String utf8 encoded */
-	uint8_t len;			/**< length of string */
-};
-
-struct ble_svc_sec_mode {
-	uint8_t rd_perm;
-	uint8_t wr_perm;
-};
-
-struct ble_svc_cccd_sec_mode {
-	uint8_t cccd_wr_perm;
-	uint8_t rd_perm;		/**< Read permissions. */
-	uint8_t wr_perm;		/**< Write permissions. */
-};
-
-struct ble_svc_report_reference {
-	uint8_t report_id;	/**< Non-zero value if these is more than one instance of the same Report Type */
-	uint8_t report_type;	/**< Type of Report characteristic */
-};
-
-struct ble_gap_write_config_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t appearance;
-	uint8_t bda_len;
-	uint8_t name_len;
-	int8_t tx_power;
-	struct ble_gap_connection_params peripheral_conn_params;
-	struct ble_gap_connection_params central_conn_params;
-	uint8_t data[];
-};
-
-struct ble_gap_wr_adv_data_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint8_t adv_len;
-	uint8_t scan_rsp_len;
-	uint8_t data[];
-	/* adv_data,
-	 * scan_rsp_dat */
-};
-
-struct ble_gap_start_advertise_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t timeout;
-	uint16_t interval_min;	    /**< min interval 0xffff: use default 0x0800 */
-	uint16_t interval_max;	    /**< max interval 0xffff: use default 0x0800 */
-	uint8_t type;		    /**< advertisement types @ref GAP_ADV_TYPES */
-	uint8_t filter_policy;	    /**< filter policy to apply with white list */
-	uint8_t options;	    /**< options see @ref BLE_GAP_ADV_OPTIONS (to be ORed) */
-	uint8_t bda_len;	/**< non 0 if peer_bda is present */
-	uint8_t peer_bda[];	/**< format ble_addr_t */
-};
-
-struct ble_gap_conn_update_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t conn_handle;
-	struct ble_gap_connection_params conn_params;
-};
-
-struct ble_gap_svc_local_name_req {
-	uint8_t sec_mode;	    /**< security mode for writing device name, @ref BLE_GAP_SEC_MODES (GAP_SEC_NO_PERMISSION: write forbidden) */
-	uint8_t authorization;		    /**< 0: no authorization, 1: authorization required */
-	uint8_t len;			    /**< device name length (0-248) */
-	uint8_t name_array[];		    /**< name to to write */
-};
-
-struct ble_gap_service_write_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t attr_type;		      /**< GAP Characteristics attribute type  @ref BLE_GAP_SVC_ATTR_TYPE */
-	union {
-		struct ble_gap_svc_local_name_req name;
-		uint16_t appearance;	      /**< Appearance UUID */
-		struct ble_gap_connection_params conn_params;
-						 /**< Preferred Peripheral Connection Parameters */
-		uint8_t car;		      /**< Central Address Resolution support 0: no, 1: yes */
-	};
-};
-
-struct ble_gap_disconnect_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t conn_handle; /**< Connection handle*/
-	uint8_t reason; /**< Reason of the disconnect*/
-};
-
-struct ble_gap_sm_config_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	struct ble_gap_sm_config_params params;
-};
-
-struct ble_gap_sm_pairing_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	struct ble_gap_sm_pairing_params params;
-	uint16_t conn_handle;
-};
-
-struct ble_dtm_test_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	struct ble_test_cmd params;
-};
-
-struct ble_gap_set_rssi_report_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	struct rssi_report_params params; /**< RSSI report config @ref rssi_report_cfg */
-};
-
-struct ble_gattc_discover_primary_service_req_msg {
-	struct cfw_message header;
-	uint16_t conn_handle; /**< Connection handle */
-	uint8_t data[]; /**< Variable length data of the request (UUID) */
-};
-
-struct ble_gattc_discover_included_service_req_msg {
-	struct cfw_message header;
-	uint16_t conn_handle;
-	struct ble_gattc_handle_range handle_range;
-};
-
-struct ble_gattc_discover_characteristic_req_msg {
-	struct cfw_message header;
-	uint16_t conn_handle;
-	struct ble_gattc_handle_range handle_range;
-};
-
-struct ble_gattc_discover_descriptor_req_msg {
-	struct cfw_message header;
-	uint16_t conn_handle;
-	struct ble_gattc_handle_range handle_range;
-};
-
-struct ble_gattc_read_characteristic_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t conn_handle; /**< Connection handle*/
-	uint16_t char_handle; /**< handle of the attribute to be read */
-	uint16_t offset; /**< offset into the attribute value to be read */
-};
-
-struct _ble_gattc_wr_characteristic {
-	uint16_t char_handle; /**< handle of characteristic */
-	uint16_t len; /**< if len is bigger then ATT MTU size, the controller fragment buffer itself */
-	uint8_t wr_type; /**< type of write operation @ref BLE_GATT_WR_OP_TYPES */
-	uint8_t value[]; /**< characteristic value to write */
-};
-
-struct ble_gattc_write_char_op_req_msg {
-	struct cfw_message header;
-	uint16_t conn_handle;
-	struct _ble_gattc_wr_characteristic wr_char_param;
-};
-
-/**
- * Generic BLE controller commands.
- */
-struct ble_generic_cmd_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-};
-
-
-void ble_core_resume_enable(struct ble_enable_req_msg *req, struct _ble_service_cb * p_cb, uint8_t * p_name);
-
-void ble_core_delete_conn_params_timer(void);
-
-#endif
diff --git a/system/libarc32_arduino101/framework/src/services/ble/ble_service_gap_api.c b/system/libarc32_arduino101/framework/src/services/ble/ble_service_gap_api.c
deleted file mode 100644
index 6b772864..00000000
--- a/system/libarc32_arduino101/framework/src/services/ble/ble_service_gap_api.c
+++ /dev/null
@@ -1,384 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <string.h>
-#include "services/ble/ble_service_gap_api.h"
-#include "ble_protocol.h"
-#include "ble_service_core_int.h"
-#include "infra/log.h"
-
-int ble_gap_set_enable_config(svc_client_handle_t * p_svc_handle,
-			      const struct ble_wr_config * p_config,
-			      void * p_priv)
-{
-	struct ble_gap_write_config_req_msg *msg;
-	int total_len = sizeof(*msg);
-	int str_len = 0;
-
-	if (p_config->p_bda)
-		total_len += sizeof(ble_addr_t);
-	if (p_config->p_name) {
-		str_len = strlen((char *)p_config->p_name);
-		if (str_len > BLE_MAX_DEVICE_NAME)
-			return -2;
-		total_len += str_len;
-	}
-	msg = (struct ble_gap_write_config_req_msg *)
-			cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GAP_WR_CONF_REQ,
-					  total_len,
-					  p_priv);
-	msg->name_len = str_len;
-	msg->appearance = p_config->appearance;
-	msg->tx_power = p_config->tx_power;
-	msg->central_conn_params = p_config->central_conn_params;
-	msg->peripheral_conn_params = p_config->peripheral_conn_params;
-	if (p_config->p_bda) {
-		msg->bda_len = sizeof(ble_addr_t);
-		memcpy(msg->data, p_config->p_bda, msg->bda_len);
-	} else
-		msg->bda_len = 0;
-	if (p_config->p_name)
-		strcpy((char *)&msg->data[msg->bda_len], (char *)p_config->p_name);
-
-	return cfw_send_message(msg);
-}
-
-int ble_gap_read_bda(svc_client_handle_t * p_svc_handle, void * p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_RD_BDA_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_wr_adv_data(svc_client_handle_t * p_svc_handle,
-			  const struct ble_gap_adv_rsp_data * p_adv_data,
-			  const struct ble_gap_adv_rsp_data * p_scan_data,
-			  void * p_priv)
-{
-	int data_len = sizeof(struct ble_gap_wr_adv_data_req_msg);
-	struct ble_gap_wr_adv_data_req_msg *msg;
-
-	if (NULL != p_adv_data)
-		data_len += p_adv_data->len;
-	if (NULL != p_scan_data)
-		data_len += p_scan_data->len;
-
-	msg = (struct ble_gap_wr_adv_data_req_msg *)cfw_alloc_message_for_service(
-			p_svc_handle, MSG_ID_BLE_GAP_WR_ADV_DATA_REQ,
-				data_len, p_priv);
-
-	if (NULL != p_adv_data) {
-		int i;
-		msg->adv_len = p_adv_data->len;
-		for (i = 0; i < msg->adv_len; i++)
-			msg->data[i] = p_adv_data->p_data[i];
-	} else
-		msg->adv_len = 0;
-
-	if (NULL != p_scan_data) {
-		int i;
-		msg->scan_rsp_len = p_scan_data->len;
-		for (i = 0; i < msg->adv_len; i++)
-			msg->data[msg->adv_len + i] = p_scan_data->p_data[i];
-	} else
-		msg->scan_rsp_len = 0;
-
-	return cfw_send_message(msg);
-}
-
-
-int ble_gap_wr_white_list(svc_client_handle_t * p_svc_handle,
-			  const struct ble_gap_whitelist_info * p_white_list,
-			  void * p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_WR_WHITE_LIST_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_clr_white_list(svc_client_handle_t * p_svc_handle,
-			   uint32_t wl_hdl, void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_CLR_WHITE_LIST_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_start_advertise(svc_client_handle_t * p_svc_handle,
-			    const ble_gap_adv_param_t * p_adv_param,
-			    void *p_priv)
-{
-	struct ble_gap_start_advertise_req_msg *msg;
-	int data_len = sizeof(struct ble_gap_start_advertise_req_msg);
-
-	if ((NULL != p_adv_param) && (NULL != p_adv_param->p_peer_bda)) {
-		data_len += sizeof(ble_addr_t);
-	}
-	msg = (struct ble_gap_start_advertise_req_msg *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GAP_ENABLE_ADV_REQ,
-					  data_len,
-					  p_priv);
-	if (NULL != p_adv_param) {
-		uint8_t i;
-		msg->filter_policy = p_adv_param->filter_policy;
-		msg->interval_max = p_adv_param->interval_max;
-		msg->interval_min = p_adv_param->interval_min;
-		msg->options = p_adv_param->options;
-		msg->timeout = p_adv_param->timeout;
-		msg->type = p_adv_param->type;
-		if (NULL != (p_adv_param->p_peer_bda))
-		{
-			msg->peer_bda[0] = p_adv_param->p_peer_bda->type;
-			for (i = 1; i <= BLE_ADDR_LEN; i++)
-			{
-				msg->peer_bda[i] = p_adv_param->p_peer_bda->addr[i-1];
-			}
-			msg->bda_len = i;
-		} else
-			msg->bda_len = 0;
-	}
-	return cfw_send_message(msg);
-}
-
-int ble_gap_stop_advertise(svc_client_handle_t * p_svc_handle, void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_DISABLE_ADV_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_conn_update(svc_client_handle_t * p_svc_handle,
-			uint16_t conn_handle,
-			const struct ble_gap_connection_params * p_conn_param,
-			void *p_priv)
-{
-	CFW_ALLOC_FOR_SVC(struct ble_gap_conn_update_req_msg, msg, p_svc_handle,
-			MSG_ID_BLE_GAP_CONN_UPDATE_REQ, 0, p_priv);
-
-	msg->conn_handle = conn_handle;
-	msg->conn_params = *p_conn_param;
-
-	return cfw_send_message(msg);
-}
-
-int ble_gap_disconnect(svc_client_handle_t * p_svc_handle,
-		       uint16_t conn_hdl, uint8_t reason,
-		       void *p_priv)
-{
-	struct ble_gap_disconnect_req_msg *msg;
-
-	msg = (struct ble_gap_disconnect_req_msg*)cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_DISCONNECT_REQ,
-								sizeof(*msg),
-								p_priv);
-
-	msg->reason = reason;
-	msg->conn_handle = conn_hdl;
-
-	return cfw_send_message(msg);
-}
-
-int ble_gap_service_write(svc_client_handle_t * p_svc_handle,
-			  const struct ble_gap_service_write_params * p_params,
-			  void *p_priv)
-{
-	struct ble_gap_service_write_req_msg *msg;
-	int total_len = sizeof(*msg);
-	if ((p_params->attr_type == GAP_SVC_ATTR_NAME) && (p_params->name.len)) {
-		total_len += p_params->name.len;
-	}
-	msg = (struct ble_gap_service_write_req_msg *)
-			cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GAP_SERVICE_WRITE_REQ,
-					  total_len,
-					  p_priv);
-	msg->attr_type = p_params->attr_type;
-
-	switch (p_params->attr_type) {
-	case GAP_SVC_ATTR_NAME:
-		msg->name.authorization = p_params->name.authorization;
-		msg->name.len = p_params->name.len;
-		msg->name.sec_mode = p_params->name.sec_mode;
-		if (msg->name.len)
-			strcpy((char *)&msg->name.name_array[0], (char *)p_params->name.p_name);
-		break;
-	case GAP_SVC_ATTR_APPEARANCE:
-		msg->appearance = p_params->appearance;
-		break;
-	case GAP_SVC_ATTR_PPCP:
-		msg->conn_params = p_params->conn_params;
-		break;
-	case GAP_SVC_ATTR_CAR:
-		msg->car = p_params->car;
-		break;
-	default:
-		pr_warning(LOG_MODULE_BLE, "ble_gap_srv_wr: Attr "
-				"not supported : 0x%x", p_params->attr_type);
-	}
-
-	return cfw_send_message(msg);
-}
-
-int ble_gap_service_read(svc_client_handle_t * p_svc_handle,
-			 uint16_t type, void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_SERVICE_READ_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_sm_config(const svc_client_handle_t * h,
-		      const struct ble_gap_sm_config_params * p_params,
-		      void *p_priv)
-{
-	CFW_ALLOC_FOR_SVC(struct ble_gap_sm_config_req_msg, msg, h, MSG_ID_BLE_GAP_SM_CONFIG_REQ, 0, p_priv);
-
-	msg->params = *p_params;
-
-	return cfw_send_message(msg);
-}
-
-int ble_gap_sm_pairing_req(const svc_client_handle_t * h,
-		uint16_t conn_handle,
-		const struct ble_gap_sm_pairing_params * p_params,
-		void *p_priv)
-{
-	CFW_ALLOC_FOR_SVC(struct ble_gap_sm_pairing_req_msg, msg, h, MSG_ID_BLE_GAP_SM_PAIRING_REQ, 0, p_priv);
-
-	msg->params = *p_params;
-	msg->conn_handle = conn_handle;
-
-	return cfw_send_message(msg);
-}
-
-int ble_gap_set_rssi_report(svc_client_handle_t * p_svc_handle,
-		const struct rssi_report_params *params,
-		void *p_priv)
-{
-	CFW_ALLOC_FOR_SVC(struct ble_gap_set_rssi_report_req_msg, msg, p_svc_handle, MSG_ID_BLE_GAP_SET_RSSI_REPORT_REQ, 0, p_priv);
-	msg->params = *params;
-	return cfw_send_message(msg);
-}
-
-int ble_gap_start_scan(svc_client_handle_t * p_svc_handle,
-		       const ble_gap_scan_param_t * p_scan_params, void * p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_SCAN_START_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_stop_scan(svc_client_handle_t * p_svc_handle, void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_SCAN_STOP_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_connect(svc_client_handle_t * p_svc_handle, const ble_addr_t * p_bd,
-		    const ble_gap_scan_param_t * p_scan_params,
-		    const struct ble_gap_connection_params * p_conn_params,
-		    void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_CONNECT_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_cancel_connect(svc_client_handle_t * p_svc_handle,
-			   const ble_addr_t * p_bd, void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_CONNECT_CANCEL_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_set_option(svc_client_handle_t * p_svc_handle, uint8_t op,
-		       const ble_gap_option_t * p_opt, void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GAP_SET_OPTIONS_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_generic_cmd_req(svc_client_handle_t * p_svc_handle,
-			    const struct ble_gap_gen_cmd_params *p_params,
-			    void *p_priv)
-{
-	struct ble_generic_cmd_req_msg *msg = (struct ble_generic_cmd_req_msg *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GAP_GENERIC_CMD_REQ,
-					  sizeof(*msg),
-					  p_priv);
-
-	return cfw_send_message(msg);
-}
-
-int ble_gap_get_version_req(svc_client_handle_t * p_svc_handle, void * p_priv)
-{
-	struct cfw_message *msg = (struct cfw_message *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GAP_GET_VERSION_REQ,
-					  sizeof(*msg),
-					  p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gap_dtm_init_req(svc_client_handle_t * p_svc_handle, void * p_priv)
-{
-	struct cfw_message *msg = (struct cfw_message *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GAP_DTM_INIT_REQ,
-					  sizeof(*msg),
-					  p_priv);
-	return cfw_send_message(msg);
-}
diff --git a/system/libarc32_arduino101/framework/src/services/ble/ble_service_gatt_int.h b/system/libarc32_arduino101/framework/src/services/ble/ble_service_gatt_int.h
deleted file mode 100644
index 3f42b09f..00000000
--- a/system/libarc32_arduino101/framework/src/services/ble/ble_service_gatt_int.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __BLE_SERVICE_GATT_INT_H__
-#define __BLE_SERVICE_GATT_INT_H__
-
-#include "services/ble/ble_service_gatt.h"
-#include "services/ble/ble_service_gatts_api.h"
-
-/**
- * BLE GATT related shared internal structures between master and BLE controller.
- */
-struct ble_gatts_add_service_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_gatts_add_svc_cback_t cback; /**< Callback function to be returnedn in response message */
-	uint8_t type;	/**< Primary or secondary \ref BLE_GATT_SVC_TYPES. */
-	uint8_t data[];	/**< Variable length data of the request (UUID) */
-};
-
-/**
- * Component framework message to add a characteristic to a service
- */
-struct ble_gatts_add_char_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_gatts_add_char_cback_t cback; /**< Callback function, to be returned to sender! */
-	uint16_t svc_handle; /**< Handle of the service to which the characteristic should be added */
-	struct ble_gatts_permissions perms;
-	struct ble_gatt_char_properties props;
-	uint16_t max_len; /**< Maximum length of the characteristic value */
-	uint16_t init_len; /**< Length of the initialization value */
-	uint8_t ud_len; /**< Length of the characteristic User Description */
-	uint8_t pf_len;
-
-	uint8_t data[]; /**< Variable length data of the request (UUID, init value, user descriptor) */
-};
-
-/**
- * Component framework message to add a descriptor to a characteristic
- */
-struct ble_gatts_add_desc_req_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_gatts_add_desc_cback_t cback; /**< Callback function, to be returned to sender! */
-	uint16_t length;
-	struct ble_gatts_permissions perms; /**< Read/Write permissions for descriptor */
-	uint8_t data[]; /**< Variable length data of the request (descriptor value) */
-};
-
-struct ble_gatts_start_service_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t svc_handle;
-};
-
-struct ble_gatts_set_attribute_value_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	uint16_t value_handle;	/* mandatory */
-	uint16_t len;
-	uint16_t offset;	/* by default 0 */
-	uint8_t data[];		/* value to update */
-};
-
-struct ble_gatts_notif_ind_params {
-	uint16_t conn_handle;
-	uint16_t val_handle;
-	uint16_t len;		/* may be 0, in this case already stored data is sent */
-	uint16_t offset;
-	uint8_t data[];
-};
-
-struct ble_gatts_send_notif_ind_msg {
-	struct cfw_message header; /**< Component framework message header (@ref cfw), MUST be first element of structure */
-	ble_gatts_notif_ind_cback_t cback; /**< Callback function to be returned in response message */
-	struct ble_gatts_notif_ind_params params; /* parameters for notification/indication */
-};
-
-#endif
diff --git a/system/libarc32_arduino101/framework/src/services/ble/ble_service_gatts_api.c b/system/libarc32_arduino101/framework/src/services/ble/ble_service_gatts_api.c
deleted file mode 100644
index 17345bde..00000000
--- a/system/libarc32_arduino101/framework/src/services/ble/ble_service_gatts_api.c
+++ /dev/null
@@ -1,287 +0,0 @@
-/*
- * Copyright (c) 2015, Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its contributors
- * may be used to endorse or promote products derived from this software without
- * specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include "services/ble/ble_service_gatt.h"
-#include "services/ble/ble_service_gatts_api.h"
-#include "ble_service_core_int.h"
-#include "ble_service_gatt_int.h"
-#include "ble_service_utils.h"
-#include <string.h>
-
-int ble_gatts_add_service(svc_client_handle_t * p_svc_handle,
-			  const struct bt_uuid * p_uuid,
-			  uint8_t type,
-			  ble_gatts_add_svc_cback_t cback,
-			  void *p_priv)
-{
-	struct ble_gatts_add_service_req_msg *msg =
-	    (struct ble_gatts_add_service_req_msg *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GATTS_ADD_SERVICE_REQ,
-					  sizeof(*msg) + ble_sizeof_bt_uuid(p_uuid),
-					  p_priv);
-	msg->cback = cback;
-	msg->type = type;
-	uint8_t *p = (uint8_t *)&msg->data;
-	ble_encode_bt_uuid(p_uuid, p);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_add_included_svc(svc_client_handle_t * p_svc_handle,
-			       uint16_t svc_handle,
-			       uint16_t svc_handle_to_include,
-			       void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GATTS_ADD_INCL_SVC_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_add_characteristic(svc_client_handle_t * p_svc_handle,
-				 uint16_t svc_handle,
-				 const struct ble_gatts_characteristic * p_char,
-				 ble_gatts_add_char_cback_t cback,
-				 void *p_priv)
-{
-	struct ble_gatts_add_char_req_msg *msg;
-	int data_len = sizeof(struct ble_gatts_add_char_req_msg);
-	uint8_t * p;
-
-	// Sanity check
-	if (!p_char || !p_char->p_uuid)
-		return E_OS_ERR;
-
-	// compute the variable length part of the message
-	data_len += ble_sizeof_bt_uuid(p_char->p_uuid);
-	data_len += p_char->init_len;
-	if (p_char->p_user_desc)
-		data_len += p_char->p_user_desc->len;
-	if (p_char->p_char_pf_desc)
-		data_len += sizeof(struct ble_gatt_pf_desc);
-
-	msg = (struct ble_gatts_add_char_req_msg *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GATTS_ADD_CHARACTERISTIC_REQ,
-					  data_len, p_priv);
-	msg->cback = cback;
-	p = msg->data;
-	p = ble_encode_bt_uuid(p_char->p_uuid, p);
-	msg->svc_handle = svc_handle;
-	msg->perms = p_char->perms;
-	msg->props = p_char->props;
-	msg->max_len = p_char->max_len;
-	msg->init_len = p_char->init_len;
-	memcpy(p, p_char->p_value, p_char->init_len);
-	p += p_char->init_len;
-	if (p_char->p_user_desc) {
-		msg->ud_len = p_char->p_user_desc->len;
-		memcpy(p, p_char->p_user_desc->buffer, p_char->p_user_desc->len);
-		p += p_char->p_user_desc->len;
-	}
-	if (p_char->p_char_pf_desc) {
-		msg->pf_len = sizeof(struct ble_gatt_pf_desc);
-		memcpy(p, p_char->p_char_pf_desc, msg->pf_len);
-		p += msg->pf_len;
-	}
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_add_descriptor(svc_client_handle_t * p_svc_handle,
-			     const struct ble_gatts_descriptor * p_desc,
-			     ble_gatts_add_desc_cback_t cback,
-			     void * p_priv)
-{
-	struct ble_gatts_add_desc_req_msg *req;
-	uint8_t * p;
-	int data_len = sizeof(struct ble_gatts_add_desc_req_msg);
-
-	/* Sanity check */
-	if (!p_desc || !p_desc->length || !p_desc->p_value || !p_desc->p_uuid)
-		return E_OS_ERR;
-
-	// compute the variable length part of the message
-	data_len += ble_sizeof_bt_uuid(p_desc->p_uuid);
-	data_len += p_desc->length;
-
-	req = (struct ble_gatts_add_desc_req_msg *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-			    MSG_ID_BLE_GATTS_ADD_DESCRIPTOR_REQ,
-			    data_len, p_priv);
-	req->cback = cback;
-	p = req->data;
-	p = ble_encode_bt_uuid(p_desc->p_uuid, p);
-	req->length = p_desc->length;
-	req->perms = p_desc->perms;
-	memcpy(p, p_desc->p_value, p_desc->length);
-
-	return cfw_send_message(req);
-}
-
-
-int ble_gatts_start_service(svc_client_handle_t * p_svc_handle,
-			    uint16_t svc_handle,
-			    void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GATTS_START_SERVICE_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_remove_service(svc_client_handle_t * p_svc_handle,
-			     uint16_t svc_handle,
-			     void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GATTS_REMOVE_SERVICE_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_send_svc_changed(svc_client_handle_t * p_svc_handle,
-			       uint16_t conn_handle,
-			       uint16_t start_handle,
-			       uint16_t end_handle,
-			       void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GATTS_INDICATE_SERVICE_CHANGE_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_set_attribute_value(svc_client_handle_t * p_svc_handle,
-				 uint16_t value_handle,
-				 uint16_t len,
-				 const uint8_t * p_value,
-				 uint16_t offset,
-				 void *p_priv)
-{
-	int i;
-	struct ble_gatts_set_attribute_value_msg *msg =
-	    (struct ble_gatts_set_attribute_value_msg *)
-	    cfw_alloc_message_for_service(p_svc_handle,
-					  MSG_ID_BLE_GATTS_SET_ATTRIBUTE_VALUE_REQ,
-					  sizeof(*msg)+len,
-					  p_priv);
-	msg->value_handle = value_handle;
-	msg->len = len;
-	msg->offset = offset;
-
-	for (i = 0; i < len; i++)
-		msg->data[i] = p_value[i];
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_get_attribute_value(svc_client_handle_t * p_svc_handle,
-				 uint16_t value_handle,
-				 uint16_t len,
-				 uint16_t offset,
-				 void *p_priv)
-{
-	struct cfw_message *msg =  cfw_alloc_message_for_service(p_svc_handle,
-			MSG_ID_BLE_GATTS_GET_ATTRIBUTE_VALUE_REQ, sizeof(*msg),
-			p_priv);
-
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_send_notif(svc_client_handle_t * p_svc_handle,
-			     uint16_t conn_handle,
-			     const ble_gatts_ind_params_t * p_params,
-			     ble_gatts_notif_ind_cback_t cback,
-			     void *p_priv)
-{
-	struct ble_gatts_send_notif_ind_msg *msg =
-			(struct ble_gatts_send_notif_ind_msg *)
-			cfw_alloc_message_for_service(p_svc_handle,
-					MSG_ID_BLE_GATTS_SEND_NOTIF_REQ,
-					sizeof(*msg) + p_params->len,
-					p_priv);
-
-	msg->cback = cback;
-	msg->params.conn_handle = conn_handle;
-	msg->params.val_handle = p_params->val_handle;
-	msg->params.len = p_params->len;
-	msg->params.offset = p_params->offset;
-	memcpy(msg->params.data, p_params->p_data, msg->params.len);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_send_ind(svc_client_handle_t * p_svc_handle,
-			     uint16_t conn_handle,
-			     const ble_gatts_ind_params_t * p_params,
-			     ble_gatts_notif_ind_cback_t cback,
-			     void *p_priv)
-{
-	struct ble_gatts_send_notif_ind_msg *msg =
-			(struct ble_gatts_send_notif_ind_msg *)
-			cfw_alloc_message_for_service(p_svc_handle,
-					MSG_ID_BLE_GATTS_SEND_IND_REQ,
-					sizeof(*msg) + p_params->len,
-					p_priv);
-
-	msg->cback = cback;
-	msg->params.conn_handle = conn_handle;
-	msg->params.val_handle = p_params->val_handle;
-	msg->params.len = p_params->len;
-	msg->params.offset = p_params->offset;
-	memcpy(msg->params.data, p_params->p_data, msg->params.len);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_write_conn_attributes(svc_client_handle_t * p_svc_handle,
-				    uint16_t conn_handle,
-				    const uint8_t * p_data,
-				    uint16_t len,
-				    void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GATTS_WR_CONN_ATTRIBUTES_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
-
-int ble_gatts_read_conn_attributes(svc_client_handle_t * p_svc_handle,
-				   uint16_t conn_handle,
-				   void *p_priv)
-{
-	struct cfw_message *msg = cfw_alloc_message_for_service(p_svc_handle,
-								MSG_ID_BLE_GATTS_RD_CONN_ATTRIBUTES_REQ,
-								sizeof(*msg),
-								p_priv);
-	return cfw_send_message(msg);
-}
diff --git a/system/libarc32_arduino101/framework/src/services/ble/conn.c b/system/libarc32_arduino101/framework/src/services/ble/conn.c
new file mode 100644
index 00000000..c0aa9014
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/conn.c
@@ -0,0 +1,927 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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 <errno.h>
+#include <atomic.h>
+
+#include <bluetooth/hci.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/gatt.h>
+#include <misc/byteorder.h>
+
+#include "hci_core.h"
+#include "conn_internal.h"
+#include "gap_internal.h"
+#include "l2cap_internal.h"
+#include "smp.h"
+
+/* #define BT_GATT_DEBUG 1 */
+
+extern void on_nble_curie_log(char *fmt, ...);
+extern void __assert_fail(void);
+#ifdef BT_GATT_DEBUG
+#define BT_DBG(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#else
+#define BT_DBG(fmt, ...) do {} while (0)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#endif
+
+#if defined(CONFIG_BLUETOOTH_SMP) || defined(CONFIG_BLUETOOTH_BREDR)
+const struct bt_conn_auth_cb *bt_auth;
+#endif /* CONFIG_BLUETOOTH_SMP || CONFIG_BLUETOOTH_BREDR */
+
+static struct bt_conn conns[CONFIG_BLUETOOTH_MAX_CONN];
+static struct bt_conn_cb *callback_list;
+
+static void notify_connected(struct bt_conn *conn)
+{
+	struct bt_conn_cb *cb;
+
+	for (cb = callback_list; cb; cb = cb->_next) {
+		if (cb->connected) {
+			cb->connected(conn, conn->err);
+		}
+	}
+}
+
+static void notify_disconnected(struct bt_conn *conn)
+{
+	struct bt_conn_cb *cb;
+
+	for (cb = callback_list; cb; cb = cb->_next) {
+		if (cb->disconnected) {
+			cb->disconnected(conn, conn->err);
+		}
+	}
+}
+
+void notify_le_param_updated(struct bt_conn *conn)
+{
+	struct bt_conn_cb *cb;
+
+	for (cb = callback_list; cb; cb = cb->_next) {
+		if (cb->le_param_updated) {
+			cb->le_param_updated(conn, conn->le.interval,
+					     conn->le.latency,
+					     conn->le.timeout);
+		}
+	}
+}
+
+#if defined(CONFIG_BLUETOOTH_SMP)
+
+void bt_conn_security_changed(struct bt_conn *conn)
+{
+	struct bt_conn_cb *cb;
+
+	for (cb = callback_list; cb; cb = cb->_next) {
+		if (cb->security_changed) {
+			cb->security_changed(conn, conn->sec_level);
+		}
+	}
+}
+
+static int start_security(struct bt_conn *conn)
+{
+	switch (conn->role) {
+#if defined(CONFIG_BLUETOOTH_CENTRAL)
+	case BT_HCI_ROLE_MASTER:
+	{
+#ifdef NOT_APPLICABLE_NBLE
+		if (!conn->keys) {
+			conn->keys = bt_keys_find(BT_KEYS_LTK_P256,
+						  &conn->le.dst);
+			if (!conn->keys) {
+				conn->keys = bt_keys_find(BT_KEYS_LTK,
+							  &conn->le.dst);
+			}
+		}
+
+		if (!conn->keys ||
+		    !(conn->keys->keys & (BT_KEYS_LTK | BT_KEYS_LTK_P256))) {
+			return bt_smp_send_pairing_req(conn);
+		}
+
+		if (conn->required_sec_level > BT_SECURITY_MEDIUM &&
+		    !atomic_test_bit(&conn->keys->flags,
+				     BT_KEYS_AUTHENTICATED)) {
+			return bt_smp_send_pairing_req(conn);
+		}
+
+		if (conn->required_sec_level > BT_SECURITY_HIGH &&
+		    !atomic_test_bit(&conn->keys->flags,
+				     BT_KEYS_AUTHENTICATED) &&
+		    !(conn->keys->keys & BT_KEYS_LTK_P256)) {
+			return bt_smp_send_pairing_req(conn);
+		}
+
+		/* LE SC LTK and legacy master LTK are stored in same place */
+		return bt_conn_le_start_encryption(conn, conn->keys->ltk.rand,
+						   conn->keys->ltk.ediv,
+						   conn->keys->ltk.val,
+						   conn->keys->enc_size);
+#endif
+		return bt_smp_send_pairing_req(conn);
+	}
+#endif /* CONFIG_BLUETOOTH_CENTRAL */
+#if defined(CONFIG_BLUETOOTH_PERIPHERAL)
+	case BT_HCI_ROLE_SLAVE:
+		return bt_smp_send_security_req(conn);
+#endif /* CONFIG_BLUETOOTH_PERIPHERAL */
+	default:
+		return -EINVAL;
+	}
+}
+
+int bt_conn_security(struct bt_conn *conn, bt_security_t sec)
+{
+	int err;
+
+	if (conn->state != BT_CONN_CONNECTED) {
+		return -ENOTCONN;
+	}
+
+#if defined(CONFIG_BLUETOOTH_SMP_SC_ONLY)
+	if (sec < BT_SECURITY_FIPS) {
+		return -EOPNOTSUPP;
+	}
+#endif/* CONFIG_BLUETOOTH_SMP_SC_ONLY */
+
+	/* nothing to do */
+	if (conn->sec_level >= sec || conn->required_sec_level >= sec) {
+		return 0;
+	}
+
+	conn->required_sec_level = sec;
+
+	err = start_security(conn);
+
+	/* reset required security level in case of error */
+	if (err) {
+		conn->required_sec_level = conn->sec_level;
+	}
+	return err;
+}
+#endif /* CONFIG_BLUETOOTH_SMP */
+
+void bt_conn_cb_register(struct bt_conn_cb *cb)
+{
+	cb->_next = callback_list;
+	callback_list = cb;
+}
+
+static struct bt_conn *conn_new(void)
+{
+	struct bt_conn *conn = NULL;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(conns); i++) {
+		if (!atomic_get(&conns[i].ref)) {
+			conn = &conns[i];
+			break;
+		}
+	}
+
+	if (!conn) {
+		return NULL;
+	}
+
+	memset(conn, 0, sizeof(*conn));
+
+	atomic_set(&conn->ref, 1);
+
+	return conn;
+}
+
+struct bt_conn *bt_conn_add_le(const bt_addr_le_t *peer)
+{
+	struct bt_conn *conn = conn_new();
+
+	if (!conn) {
+		return NULL;
+	}
+
+	bt_addr_le_copy(&conn->le.dst, peer);
+#if defined(CONFIG_BLUETOOTH_SMP)
+	conn->sec_level = BT_SECURITY_LOW;
+	conn->required_sec_level = BT_SECURITY_LOW;
+#endif /* CONFIG_BLUETOOTH_SMP */
+	conn->type = BT_CONN_TYPE_LE;
+	conn->le.interval_min = BT_GAP_INIT_CONN_INT_MIN;
+	conn->le.interval_max = BT_GAP_INIT_CONN_INT_MAX;
+
+	return conn;
+}
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+struct bt_conn *bt_conn_lookup_addr_br(const bt_addr_t *peer)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(conns); i++) {
+		if (!atomic_get(&conns[i].ref)) {
+			continue;
+		}
+
+		if (conns[i].type != BT_CONN_TYPE_BR) {
+			continue;
+		}
+
+		if (!bt_addr_cmp(peer, &conns[i].br.dst)) {
+			return bt_conn_ref(&conns[i]);
+		}
+	}
+
+	return NULL;
+}
+
+struct bt_conn *bt_conn_add_br(const bt_addr_t *peer)
+{
+	struct bt_conn *conn = conn_new();
+
+	if (!conn) {
+		return NULL;
+	}
+
+	bt_addr_copy(&conn->br.dst, peer);
+	conn->type = BT_CONN_TYPE_BR;
+
+	return conn;
+}
+#endif
+
+void bt_conn_set_state(struct bt_conn *conn, bt_conn_state_t state)
+{
+	bt_conn_state_t old_state;
+
+	BT_DBG("conn state %d -> %d, err: %d", conn->state, state, conn->err);
+
+	if (conn->state == state) {
+		BT_DBG("no transition");
+		return;
+	}
+
+	old_state = conn->state;
+	conn->state = state;
+
+	/* Actions needed for exiting the old state */
+	switch (old_state) {
+	case BT_CONN_DISCONNECTED:
+		/* Take a reference for the first state transition after
+		 * bt_conn_add_le() and keep it until reaching DISCONNECTED
+		 * again.
+		 */
+		bt_conn_ref(conn);
+		break;
+	case BT_CONN_CONNECT:
+#if 0
+		if (conn->timeout) {
+			fiber_delayed_start_cancel(conn->timeout);
+			conn->timeout = NULL;
+
+			/* Drop the reference taken by timeout fiber */
+			bt_conn_unref(conn);
+		}
+#endif
+		break;
+	default:
+		break;
+	}
+
+	/* Actions needed for entering the new state */
+	switch (conn->state) {
+	case BT_CONN_CONNECTED:
+		bt_l2cap_connected(conn);
+		notify_connected(conn);
+		break;
+	case BT_CONN_DISCONNECTED:
+		/* Notify disconnection and queue a dummy buffer to wake
+		 * up and stop the tx fiber for states where it was
+		 * running.
+		 */
+		if (old_state == BT_CONN_CONNECTED ||
+		    old_state == BT_CONN_DISCONNECT) {
+			bt_l2cap_disconnected(conn);
+			notify_disconnected(conn);
+		} else if (old_state == BT_CONN_CONNECT) {
+			/* conn->err will be set in this case */
+			notify_connected(conn);
+		}
+
+		/* Release the reference we took for the very first
+		 * state transition.
+		 */
+		bt_conn_unref(conn);
+
+		break;
+	case BT_CONN_CONNECT_SCAN:
+		break;
+	case BT_CONN_CONNECT:
+		break;
+	case BT_CONN_DISCONNECT:
+		break;
+	default:
+
+		break;
+	}
+}
+
+struct bt_conn *bt_conn_lookup_handle(uint16_t handle)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(conns); i++) {
+		if (!atomic_get(&conns[i].ref)) {
+			continue;
+		}
+		/* We only care about connections with a valid handle */
+		if (conns[i].state != BT_CONN_CONNECTED &&
+		    conns[i].state != BT_CONN_DISCONNECT) {
+			continue;
+		}
+
+		if (conns[i].handle == handle) {
+			return bt_conn_ref(&conns[i]);
+		}
+	}
+
+	return NULL;
+}
+
+struct bt_conn *bt_conn_lookup_addr_le(const bt_addr_le_t *peer)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(conns); i++) {
+		if (!atomic_get(&conns[i].ref)) {
+			continue;
+		}
+
+		if (conns[i].type != BT_CONN_TYPE_LE) {
+			continue;
+		}
+
+		if (!bt_addr_le_cmp(peer, &conns[i].le.dst)) {
+			return bt_conn_ref(&conns[i]);
+		}
+	}
+
+	return NULL;
+}
+
+struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer,
+					const bt_conn_state_t state)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(conns); i++) {
+		if (!atomic_get(&conns[i].ref)) {
+			continue;
+		}
+
+		if (conns[i].type != BT_CONN_TYPE_LE) {
+			continue;
+		}
+
+		if (bt_addr_le_cmp(peer, BT_ADDR_LE_ANY) &&
+		    bt_addr_le_cmp(peer, &conns[i].le.dst)) {
+			continue;
+		}
+
+		if (conns[i].state == state) {
+			return bt_conn_ref(&conns[i]);
+		}
+	}
+
+	return NULL;
+}
+
+struct bt_conn *bt_conn_ref(struct bt_conn *conn)
+{
+	atomic_inc(&conn->ref);
+
+	return conn;
+}
+
+void bt_conn_unref(struct bt_conn *conn)
+{
+	atomic_dec(&conn->ref);
+}
+
+const bt_addr_le_t *bt_conn_get_dst(const struct bt_conn *conn)
+{
+	return &conn->le.dst;
+}
+
+int bt_conn_get_info(const struct bt_conn *conn, struct bt_conn_info *info)
+{
+	info->type = conn->type;
+	info->role = conn->role;
+
+	switch (conn->type) {
+	case BT_CONN_TYPE_LE:
+		if (conn->role == BT_HCI_ROLE_MASTER) {
+#if 0
+			info->le.src = &conn->le.init_addr;
+			info->le.dst = &conn->le.resp_addr;
+#else
+			info->le.dst = &conn->le.dst;
+#endif
+		} else {
+#if 0
+			info->le.src = &conn->le.resp_addr;
+			info->le.dst = &conn->le.init_addr;
+#else
+			info->le.src = &conn->le.dst;
+#endif
+		}
+		info->le.interval = conn->le.interval;
+		info->le.latency = conn->le.latency;
+		info->le.timeout = conn->le.timeout;
+
+		return 0;
+#if defined(CONFIG_BLUETOOTH_BREDR)
+	case BT_CONN_TYPE_BR:
+		info->br.dst = &conn->br.dst;
+		return 0;
+#endif
+	}
+
+	return -EINVAL;
+}
+
+static int bt_hci_disconnect(struct bt_conn *conn, uint8_t reason)
+{
+	struct nble_gap_disconnect_req_params ble_gap_disconnect;
+
+	ble_gap_disconnect.conn_handle = conn->handle;
+	ble_gap_disconnect.reason = reason;
+	nble_gap_disconnect_req(&ble_gap_disconnect);
+
+	bt_conn_set_state(conn, BT_CONN_DISCONNECT);
+	return 0;
+}
+
+static int bt_hci_connect_le_cancel(struct bt_conn *conn)
+{
+	nble_gap_cancel_connect_req(conn);
+	return 0;
+}
+
+void on_nble_gap_cancel_connect_rsp(const struct nble_response *params)
+{
+	struct bt_conn *conn = params->user_data;
+
+	conn->err = BT_HCI_ERR_INSUFFICIENT_RESOURCES;
+	bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
+}
+
+int bt_conn_disconnect(struct bt_conn *conn, uint8_t reason)
+{
+#if defined(CONFIG_BLUETOOTH_CENTRAL)
+	/* Disconnection is initiated by us, so auto connection shall
+	 * be disabled. Otherwise the passive scan would be enabled
+	 * and we could send LE Create Connection as soon as the remote
+	 * starts advertising.
+	 */
+	if (conn->type == BT_CONN_TYPE_LE) {
+		bt_le_set_auto_conn(&conn->le.dst, NULL);
+	}
+#endif
+
+	switch (conn->state) {
+	case BT_CONN_CONNECT_SCAN:
+		conn->err = BT_HCI_ERR_INSUFFICIENT_RESOURCES;
+		bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
+		/* scan update not yet implemented */
+		return 0;
+	case BT_CONN_CONNECT:
+		return bt_hci_connect_le_cancel(conn);
+	case BT_CONN_CONNECTED:
+		return bt_hci_disconnect(conn, reason);
+	case BT_CONN_DISCONNECT:
+		return 0;
+	case BT_CONN_DISCONNECTED:
+	default:
+		return -ENOTCONN;
+	}
+}
+
+static bool valid_adv_params(const struct nble_gap_adv_params *params)
+{
+	if (params->type == BT_LE_ADV_DIRECT_IND) {
+		/* If high duty, ensure interval is 0 */
+		if (params->interval_max != 0)
+			return false;
+
+		if (params->timeout != 0)
+			return false;
+	} else if (params->type == BT_LE_ADV_DIRECT_IND_LOW_DUTY) {
+		if (params->interval_min < 0x20)
+			return false;
+	} else {
+		return false;
+	}
+
+	if (params->interval_min > params->interval_max)
+		return false;
+
+	if (params->interval_max > 0x4000)
+		return false;
+
+	return true;
+}
+
+struct bt_conn *bt_conn_create_slave_le(const bt_addr_le_t *peer,
+				  const struct bt_le_adv_param *param)
+{
+	struct bt_conn *conn;
+	/* Timeout is handled by application timer */
+	/* forced to none currently (no whitelist support) */
+	struct nble_gap_adv_params params = {
+		.interval_max = param->interval_max,
+		.interval_min = param->interval_min,
+		.type = param->type,
+		.timeout = 0,
+		.filter_policy = 0
+	};
+
+	bt_addr_le_copy(&params.peer_bda, peer);
+
+	if (!valid_adv_params(&params)) {
+		return NULL;
+	}
+
+	if (param->type == BT_LE_ADV_DIRECT_IND_LOW_DUTY) {
+		params.type = BT_LE_ADV_DIRECT_IND;
+	}
+
+	if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) {
+		return NULL;
+	}
+
+	conn = bt_conn_add_le(&params.peer_bda);
+
+	if (!conn) {
+		return NULL;
+	}
+
+	bt_conn_set_state(conn, BT_CONN_CONNECT);
+
+	nble_gap_set_adv_params_req(&params);
+	nble_gap_start_adv_req();
+
+	return conn;
+}
+
+#if defined(CONFIG_BLUETOOTH_CENTRAL)
+static int hci_le_create_conn(struct bt_conn *conn)
+{
+	struct nble_gap_connect_req_params conn_params;
+
+	conn_params.bda = conn->le.dst;
+	conn_params.conn_params.interval_min = conn->le.interval_min;
+	conn_params.conn_params.interval_max = conn->le.interval_max;
+	conn_params.conn_params.slave_latency = conn->le.latency;
+	conn_params.conn_params.link_sup_to = conn->le.timeout;
+
+	conn_params.scan_params.interval = sys_cpu_to_le16(BT_GAP_SCAN_FAST_INTERVAL);
+	conn_params.scan_params.window = conn_params.scan_params.interval;
+	conn_params.scan_params.selective = 0;
+	conn_params.scan_params.active = 1;
+	conn_params.scan_params.timeout = 0;
+
+	nble_gap_connect_req(&conn_params, conn);
+
+	return 0;
+}
+
+void on_nble_gap_connect_rsp(const struct nble_response *params)
+{
+	struct bt_conn *conn = params->user_data;
+
+	/* If the connection request was not issued successfully */
+	if (params->status) {
+		conn->err = BT_HCI_ERR_UNACCEPT_CONN_PARAMS;
+		bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
+	}
+}
+
+struct bt_conn *bt_conn_create_le(const bt_addr_le_t *peer,
+				const struct bt_le_conn_param *param)
+{
+	struct bt_conn *conn;
+
+	if (!bt_le_conn_params_valid(param->interval_min, param->interval_max,
+				param->latency, param->timeout)) {
+		return NULL;
+	}
+
+	/* if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) */
+	/*	return NULL; */
+
+	conn = bt_conn_lookup_addr_le(peer);
+	if (conn) {
+		switch (conn->state) {
+		case BT_CONN_CONNECT_SCAN:
+			bt_conn_set_param_le(conn, param);
+			return conn;
+		case BT_CONN_CONNECT:
+		case BT_CONN_CONNECTED:
+			return conn;
+		default:
+			bt_conn_unref(conn);
+			return NULL;
+		}
+	}
+
+	conn = bt_conn_add_le(peer);
+	if (!conn) {
+		return NULL;
+	}
+#if 0
+	bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN);
+
+	bt_le_scan_update(true);
+#endif
+
+	bt_addr_le_copy(&conn->le.dst, peer);
+
+	bt_conn_set_param_le(conn, param);
+
+	/* for the time being, the implementation bypassed the scan procedure */
+	if (hci_le_create_conn(conn)) {
+		goto done;
+	}
+
+	bt_conn_set_state(conn, BT_CONN_CONNECT);
+
+done:
+	return conn;
+}
+#else
+
+void on_nble_gap_connect_rsp(const struct nble_response *params)
+{
+}
+
+#endif /* CONFIG_BLUETOOTH_CENTRAL */
+
+int bt_conn_le_param_update(struct bt_conn *conn, const struct bt_le_conn_param *param)
+{
+	return bt_conn_update_param_le(conn, param);
+}
+
+#if defined(CONFIG_BLUETOOTH_SMP) || defined(CONFIG_BLUETOOTH_BREDR)
+uint8_t bt_conn_enc_key_size(struct bt_conn *conn)
+{
+	return 0;
+}
+
+int bt_conn_auth_cb_register(const struct bt_conn_auth_cb *cb)
+{
+	if (!cb) {
+		bt_auth = NULL;
+		return 0;
+	}
+
+	/* cancel callback should always be provided */
+	if (!cb->cancel) {
+		return -EINVAL;
+	}
+
+	if (bt_auth) {
+		return -EALREADY;
+	}
+
+	bt_auth = cb;
+	return 0;
+}
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+static int pin_code_neg_reply(const bt_addr_t *bdaddr)
+{
+	struct bt_hci_cp_pin_code_neg_reply *cp;
+	struct net_buf *buf;
+
+	BT_DBG("");
+
+	buf = bt_hci_cmd_create(BT_HCI_OP_PIN_CODE_NEG_REPLY, sizeof(*cp));
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	cp = net_buf_add(buf, sizeof(*cp));
+	bt_addr_copy(&cp->bdaddr, bdaddr);
+
+	return bt_hci_cmd_send_sync(BT_HCI_OP_PIN_CODE_NEG_REPLY, buf, NULL);
+}
+
+static int pin_code_reply(struct bt_conn *conn, const char *pin, uint8_t len)
+{
+	struct bt_hci_cp_pin_code_reply *cp;
+	struct net_buf *buf;
+
+	BT_DBG("");
+
+	buf = bt_hci_cmd_create(BT_HCI_OP_PIN_CODE_REPLY, sizeof(*cp));
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	cp = net_buf_add(buf, sizeof(*cp));
+
+	bt_addr_copy(&cp->bdaddr, &conn->br.dst);
+	cp->pin_len = len;
+	strncpy(cp->pin_code, pin, sizeof(cp->pin_code));
+
+	return bt_hci_cmd_send_sync(BT_HCI_OP_PIN_CODE_REPLY, buf, NULL);
+}
+
+int bt_conn_auth_pincode_entry(struct bt_conn *conn, const char *pin)
+{
+	size_t len;
+
+	if (!bt_auth) {
+		return -EINVAL;
+	}
+
+	if (conn->type != BT_CONN_TYPE_BR) {
+		return -EINVAL;
+	}
+
+	len = strlen(pin);
+	if (len > 16) {
+		return -EINVAL;
+	}
+
+	if (conn->required_sec_level == BT_SECURITY_HIGH && len < 16) {
+		BT_WARN("PIN code for %s is not 16 bytes wide",
+			bt_addr_str(&conn->br.dst));
+		return -EPERM;
+	}
+
+	return pin_code_reply(conn, pin, len);
+}
+
+void bt_conn_pin_code_req(struct bt_conn *conn)
+{
+	if (bt_auth && bt_auth->pincode_entry) {
+		bool secure = false;
+
+		if (conn->required_sec_level == BT_SECURITY_HIGH) {
+			secure = true;
+		}
+
+		bt_auth->pincode_entry(conn, secure);
+	} else {
+		pin_code_neg_reply(&conn->br.dst);
+	}
+
+}
+#endif /* CONFIG_BLUETOOTH_BREDR */
+
+int bt_conn_auth_passkey_entry(struct bt_conn *conn, unsigned int passkey)
+{
+	if (!bt_auth) {
+		return -EINVAL;
+	}
+#if defined(CONFIG_BLUETOOTH_SMP)
+	if (conn->type == BT_CONN_TYPE_LE) {
+		return bt_smp_auth_passkey_entry(conn, passkey);
+	}
+#endif /* CONFIG_BLUETOOTH_SMP */
+
+	return -EINVAL;
+}
+
+int bt_conn_auth_passkey_confirm(struct bt_conn *conn, bool match)
+{
+	if (!bt_auth) {
+		return -EINVAL;
+	};
+#if defined(CONFIG_BLUETOOTH_SMP)
+	if (conn->type == BT_CONN_TYPE_LE) {
+		return bt_smp_auth_passkey_confirm(conn, match);
+	}
+#endif /* CONFIG_BLUETOOTH_SMP */
+
+	return -EINVAL;
+}
+
+int bt_conn_auth_cancel(struct bt_conn *conn)
+{
+	if (!bt_auth) {
+		return -EINVAL;
+	}
+#if defined(CONFIG_BLUETOOTH_SMP)
+	if (conn->type == BT_CONN_TYPE_LE) {
+		return bt_smp_auth_cancel(conn);
+	}
+#endif /* CONFIG_BLUETOOTH_SMP */
+#if defined(CONFIG_BLUETOOTH_BREDR)
+	if (conn->type == BT_CONN_TYPE_BR) {
+		return pin_code_neg_reply(&conn->br.dst);
+	}
+#endif /* CONFIG_BLUETOOTH_BREDR */
+
+	return -EINVAL;
+}
+
+int bt_conn_remove_info(const bt_addr_le_t *addr)
+{
+	struct bt_conn *conn;
+
+	/* TODO: implement address specific removal */
+	if (bt_addr_le_cmp(addr, BT_ADDR_LE_ANY))
+		return -EINVAL;
+
+	do {
+		conn = bt_conn_lookup_state_le(addr, BT_CONN_CONNECTED);
+		if (conn) {
+			bt_conn_unref(conn);
+			bt_conn_disconnect(conn,
+					   BT_HCI_ERR_REMOTE_USER_TERM_CONN);
+		}
+	} while(conn);
+
+	return bt_smp_remove_info(addr);
+}
+#endif /* CONFIG_BLUETOOTH_SMP || CONFIG_BLUETOOTH_BREDR */
+
+int bt_conn_init(void)
+{
+	int err;
+#if NOT_USED_FOR_NOW
+
+	net_buf_pool_init(frag_pool);
+	net_buf_pool_init(dummy_pool);
+
+	bt_att_init();
+#endif
+
+	err = bt_smp_init();
+	if (err) {
+		return err;
+	}
+
+#if NOT_USED_FOR_NOW
+	bt_l2cap_init();
+
+	background_scan_init();
+#endif
+	return 0;
+}
+
+int bt_conn_le_conn_update(struct bt_conn *conn,
+			   const struct bt_le_conn_param *param)
+{
+	struct nble_gap_connect_update_params ble_gap_connect_update;
+#if 0
+	struct hci_cp_le_conn_update *conn_update;
+	struct net_buf *buf;
+
+	buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_UPDATE,
+				sizeof(*conn_update));
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	conn_update = net_buf_add(buf, sizeof(*conn_update));
+	memset(conn_update, 0, sizeof(*conn_update));
+	conn_update->handle = sys_cpu_to_le16(conn->handle);
+	conn_update->conn_interval_min = sys_cpu_to_le16(param->interval_min);
+	conn_update->conn_interval_max = sys_cpu_to_le16(param->interval_max);
+	conn_update->conn_latency = sys_cpu_to_le16(param->latency);
+	conn_update->supervision_timeout = sys_cpu_to_le16(param->timeout);
+#endif
+	ble_gap_connect_update.conn_handle = conn->handle;
+	ble_gap_connect_update.params.interval_min = param->interval_min;
+	ble_gap_connect_update.params.interval_max = param->interval_max;
+	ble_gap_connect_update.params.slave_latency = param->latency;
+	ble_gap_connect_update.params.link_sup_to = param->timeout;
+
+	nble_gap_conn_update_req(&ble_gap_connect_update);
+
+	return 0;
+}
diff --git a/system/libarc32_arduino101/framework/src/services/ble/conn_internal.h b/system/libarc32_arduino101/framework/src/services/ble/conn_internal.h
new file mode 100644
index 00000000..04c08839
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/conn_internal.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+typedef enum {
+	BT_CONN_DISCONNECTED,
+	BT_CONN_CONNECT_SCAN,
+	BT_CONN_CONNECT,
+	BT_CONN_CONNECTED,
+	BT_CONN_DISCONNECT,
+} bt_conn_state_t;
+
+/* bt_conn flags: the flags defined here represent connection parameters */
+enum {
+	BT_CONN_AUTO_CONNECT,
+};
+
+struct bt_conn_le {
+	bt_addr_le_t		dst;
+
+#if 0
+	bt_addr_le_t		init_addr;
+	bt_addr_le_t		resp_addr;
+#endif
+	uint16_t		interval;
+	uint16_t		interval_min;
+	uint16_t		interval_max;
+
+	uint16_t		latency;
+	uint16_t		timeout;
+#if 0
+	uint8_t			features[8];
+#endif
+};
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+struct bt_conn_br {
+	bt_addr_t		dst;
+};
+#endif
+
+struct bt_conn {
+	uint16_t		handle;
+	uint8_t			type;
+	uint8_t			role;
+
+#if defined(CONFIG_BLUETOOTH_SMP)
+	uint8_t			encrypt;
+	bt_security_t		sec_level;
+	bt_security_t		required_sec_level;
+#endif /* CONFIG_BLUETOOTH_SMP */
+
+	atomic_t		ref;
+
+	/* Connection error or reason for disconnect */
+	uint8_t			err;
+
+	bt_conn_state_t		state;
+	union {
+		struct bt_conn_le	le;
+#if defined(CONFIG_BLUETOOTH_BREDR)
+		struct bt_conn_br	br;
+#endif
+	};
+};
+
+/* Add a new LE connection */
+struct bt_conn *bt_conn_add_le(const bt_addr_le_t *peer);
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+/* Add a new BR/EDR connection */
+struct bt_conn *bt_conn_add_br(const bt_addr_t *peer);
+
+/* Look up an existing connection by BT address */
+struct bt_conn *bt_conn_lookup_addr_br(const bt_addr_t *peer);
+#endif
+
+/* Look up an existing connection */
+struct bt_conn *bt_conn_lookup_handle(uint16_t handle);
+
+/* Look up a connection state. For BT_ADDR_LE_ANY, returns the first connection
+ * with the specific state
+ */
+struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer,
+					const bt_conn_state_t state);
+
+/* Set connection object in certain state and perform action related to state */
+void bt_conn_set_state(struct bt_conn *conn, bt_conn_state_t state);
+
+void bt_conn_set_param_le(struct bt_conn *conn,
+			  const struct bt_le_conn_param *param);
+
+int bt_conn_update_param_le(struct bt_conn *conn,
+			    const struct bt_le_conn_param *param);
+
+int bt_conn_le_conn_update(struct bt_conn *conn,
+			   const struct bt_le_conn_param *param);
+
+void notify_le_param_updated(struct bt_conn *conn);
+
+#if defined(CONFIG_BLUETOOTH_SMP)
+/* rand and ediv should be in BT order */
+int bt_conn_le_start_encryption(struct bt_conn *conn, uint64_t rand,
+				uint16_t ediv, const uint8_t *ltk, size_t len);
+
+/* Notify higher layers that RPA was resolved */
+void bt_conn_identity_resolved(struct bt_conn *conn);
+
+/* Notify higher layers that connection security changed */
+void bt_conn_security_changed(struct bt_conn *conn);
+#endif /* CONFIG_BLUETOOTH_SMP */
+/* Initialize connection management */
+int bt_conn_init(void);
diff --git a/system/libarc32_arduino101/framework/src/services/ble/dtm_tcmd.c b/system/libarc32_arduino101/framework/src/services/ble/dtm_tcmd.c
new file mode 100644
index 00000000..4a0c5b0c
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/dtm_tcmd.c
@@ -0,0 +1,12 @@
+
+#include <os/os.h>
+#include "uart.h"
+#include "ipc_uart_ns16550.h"
+
+void on_nble_gap_dtm_init_rsp(void *user_data)
+{
+#ifdef CONFIG_UART_NS16550
+	uart_ipc_disable();
+#endif
+}
+
diff --git a/system/libarc32_arduino101/framework/src/services/ble/gap.c b/system/libarc32_arduino101/framework/src/services/ble/gap.c
new file mode 100644
index 00000000..26858281
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/gap.c
@@ -0,0 +1,923 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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 <errno.h>
+#include <stddef.h>
+#include <atomic.h>
+
+#include <bluetooth/gatt.h>
+#include <bluetooth/bluetooth.h>
+#include "gap_internal.h"
+#include "conn_internal.h"
+
+#include "hci_core.h"
+
+#if defined(CONFIG_BLUETOOTH_SMP)
+#include "smp.h"
+#endif /* CONFIG_BLUETOOTH_SMP */
+
+/* #define BT_GATT_DEBUG 1 */
+
+extern void on_nble_curie_log(char *fmt, ...);
+extern void __assert_fail(void);
+#ifdef BT_GATT_DEBUG
+#define BT_DBG(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#else
+#define BT_DBG(fmt, ...) do {} while (0)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#endif
+
+static bt_ready_cb_t bt_ready_cb;
+static bt_le_scan_cb_t *scan_dev_found_cb;
+static rssi_report_t rssi_report_cb;
+
+struct bt_dev bt_dev;
+
+static int set_advertise_enable(void)
+{
+#if 0
+	struct net_buf *buf;
+	int err;
+#endif
+	if (atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) {
+		return 0;
+	}
+#if 0
+	buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_ENABLE, 1);
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	net_buf_add_u8(buf, BT_HCI_LE_ADV_ENABLE);
+	err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_ENABLE, buf, NULL);
+	if (err) {
+		return err;
+	}
+#endif
+	nble_gap_start_adv_req();
+
+	return 0;
+}
+
+static int set_advertise_disable(void)
+{
+#if 0
+	struct net_buf *buf;
+	int err;
+#endif
+	if (!atomic_test_bit(bt_dev.flags, BT_DEV_ADVERTISING)) {
+		return 0;
+	}
+#if 0
+	buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_ADV_ENABLE, 1);
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	net_buf_add_u8(buf, BT_HCI_LE_ADV_DISABLE);
+	err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_ADV_ENABLE, buf, NULL);
+	if (err) {
+		return err;
+	}
+#endif
+	nble_gap_stop_adv_req(NULL);
+
+	atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING);
+	return 0;
+}
+
+void ble_gap_get_bonding_info(ble_bond_info_cb_t func, void *user_data,
+			      bool include_bonded_addrs)
+{
+	struct nble_gap_sm_bond_info_param params;
+
+	params.cb = func;
+	params.user_data = user_data;
+	params.include_bonded_addrs = include_bonded_addrs;
+
+	nble_gap_sm_bond_info_req(&params);
+}
+
+void on_nble_gap_start_advertise_rsp(const struct nble_response *params)
+{
+	if (params->status == 0)
+		atomic_set_bit(bt_dev.flags, BT_DEV_ADVERTISING);
+	else
+		BT_WARN("start advertise failed with %d", params->status);
+}
+
+void on_nble_gap_disconnect_evt(const struct nble_gap_disconnect_evt *evt)
+{
+	struct bt_conn *conn;
+
+#if 0
+	/* Nordic has no disconnection error */
+	if (evt->status) {
+		return;
+	}
+#endif
+
+	conn = bt_conn_lookup_handle(evt->conn_handle);
+	if (!conn) {
+		BT_DBG("Unable to look up conn with handle %u",
+		       evt->conn_handle);
+		return;
+	}
+#if 0
+	/* Check stacks usage (no-ops if not enabled) */
+	stack_analyze("rx stack", rx_fiber_stack, sizeof(rx_fiber_stack));
+	stack_analyze("cmd rx stack", rx_prio_fiber_stack,
+		      sizeof(rx_prio_fiber_stack));
+	stack_analyze("cmd tx stack", cmd_tx_fiber_stack,
+		      sizeof(cmd_tx_fiber_stack));
+	stack_analyze("conn tx stack", conn->stack, sizeof(conn->stack));
+
+#endif
+
+	conn->err = evt->hci_reason;
+	bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
+	conn->handle = 0;
+
+#if 0
+	/* Only LE supported */
+	if (conn->type != BT_CONN_TYPE_LE) {
+		bt_conn_unref(conn);
+		return;
+	}
+	/* TODO enabled when autoconn is supported */
+	if (atomic_test_bit(conn->flags, BT_CONN_AUTO_CONNECT)) {
+		bt_conn_set_state(conn, BT_CONN_CONNECT_SCAN);
+		bt_le_scan_update(false);
+	}
+#endif
+
+	bt_conn_unref(conn);
+	if (atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING)) {
+		set_advertise_enable();
+	}
+}
+
+void on_nble_gap_connect_evt(const struct nble_gap_connect_evt *evt)
+{
+	struct bt_conn *conn;
+
+	/* Make lookup to check if there's a connection object in CONNECT state
+	 * associated with passed peer LE address.
+	 */
+	conn = bt_conn_lookup_state_le(&evt->peer_bda, BT_CONN_CONNECT);
+
+#if 0
+	/* Nordic has no connection error */
+	if (evt->status) {
+		if (!conn) {
+			return;
+		}
+
+		conn->err = BT_HCI_ERR_UNACCEPT_CONN_PARAMS;
+		bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
+
+		/* Drop the reference got by lookup call in CONNECT state.
+		 * We are now in DISCONNECTED state since no successful LE
+		 * link been made.
+		 */
+		bt_conn_unref(conn);
+
+		return;
+	}
+#endif
+	/*
+	 * clear advertising even if we are not able to add connection object
+	 * to keep host in sync with controller state
+	 */
+	if (evt->role_slave == BT_CONN_ROLE_SLAVE) {
+		atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING);
+	}
+
+	if (!conn) {
+		conn = bt_conn_add_le(&evt->peer_bda);
+	}
+
+	if (!conn) {
+		BT_DBG("Unable to add new conn for handle %u",
+		       evt->conn_handle);
+		return;
+	}
+
+	conn->handle = evt->conn_handle;
+	bt_addr_le_copy(&conn->le.dst, &evt->peer_bda);
+	conn->le.interval = evt->conn_values.interval;
+	conn->le.latency = evt->conn_values.latency;
+	conn->le.timeout = evt->conn_values.supervision_to;
+	conn->role = evt->role_slave;
+
+#if 0
+	src.type = BT_ADDR_LE_PUBLIC;
+	memcpy(src.val, bt_dev.bdaddr.val, sizeof(bt_dev.bdaddr.val));
+
+	/* use connection address (instead of identity address) as initiator
+	 * or responder address
+	 */
+	if (conn->role == BT_HCI_ROLE_MASTER) {
+		bt_addr_le_copy(&conn->le.init_addr, &src);
+		bt_addr_le_copy(&conn->le.resp_addr, &evt->peer_addr);
+	} else {
+		bt_addr_le_copy(&conn->le.init_addr, &evt->peer_addr);
+		bt_addr_le_copy(&conn->le.resp_addr, &src);
+	}
+#endif
+	bt_conn_set_state(conn, BT_CONN_CONNECTED);
+
+	/* Note: Connection update removed because Windows interop and BT spec recommendations */
+
+	bt_conn_unref(conn);
+#if 0
+	bt_le_scan_update(false);
+#endif
+
+}
+
+void on_nble_gap_adv_report_evt(const struct nble_gap_adv_report_evt *evt,
+				const uint8_t *buf, uint8_t len)
+{
+#if TODO_IMPLEMENT_CONNECTION
+	uint8_t num_reports = buf->data[0];
+	struct bt_hci_ev_le_advertising_info *info;
+
+	BT_DBG("Adv number of reports %u",  num_reports);
+
+	info = net_buf_pull(buf, sizeof(num_reports));
+
+	while (num_reports--) {
+		int8_t rssi = info->data[info->length];
+		const bt_addr_le_t *addr;
+
+		BT_DBG("%s event %u, len %u, rssi %d dBm",
+		       bt_addr_le_str(&info->addr),
+		       info->evt_type, info->length, rssi);
+
+		addr = find_id_addr(&info->addr);
+#endif
+
+		BT_DBG("nble gap: event:%u, len %u", evt->adv_type, len);
+
+		if (scan_dev_found_cb) {
+			scan_dev_found_cb(&evt->addr, evt->rssi, evt->adv_type,
+					  buf, len);
+		}
+#if TODO_IMPLEMENT_CONNECTION
+#if defined(CONFIG_BLUETOOTH_CONN)
+		check_pending_conn(addr, &info->addr, info->evt_type);
+#endif /* CONFIG_BLUETOOTH_CONN */
+		/* Get next report iteration by moving pointer to right offset
+		 * in buf according to spec 4.2, Vol 2, Part E, 7.7.65.2.
+		 */
+		info = net_buf_pull(buf, sizeof(*info) + info->length +
+				    sizeof(rssi));
+	}
+#endif
+}
+
+void on_nble_gap_conn_update_evt(const struct nble_gap_conn_update_evt *evt)
+{
+	struct bt_conn *conn;
+	uint16_t handle, interval;
+
+	handle = evt->conn_handle;
+	interval = evt->conn_values.interval;
+
+/*	BT_DBG("status %u, handle %u", evt->status, handle); */
+
+	conn = bt_conn_lookup_handle(handle);
+	if (!conn) {
+/*		BT_ERR("Unable to lookup conn for handle %u", handle); */
+		return;
+	}
+
+/*	if (!evt->status) { */
+		conn->le.interval = interval;
+		conn->le.latency = evt->conn_values.latency;
+		conn->le.timeout = evt->conn_values.supervision_to;
+		notify_le_param_updated(conn);
+/*	} */
+
+
+	bt_conn_unref(conn);
+}
+
+void bt_conn_set_param_le(struct bt_conn *conn,
+			  const struct bt_le_conn_param *param)
+{
+	conn->le.interval_min = param->interval_min;
+	conn->le.interval_max = param->interval_max;
+	conn->le.latency = param->latency;
+	conn->le.timeout = param->timeout;
+}
+int bt_conn_update_param_le(struct bt_conn *conn,
+			    const struct bt_le_conn_param *param)
+{
+#if 0
+	BT_DBG("conn %p features 0x%x params (%d-%d %d %d)", conn,
+	       conn->le.features[0], param->interval_min, param->interval_max,
+	       param->latency, param->timeout);
+#endif
+	/* Check if there's a need to update conn params */
+	if (conn->le.interval >= param->interval_min &&
+	    conn->le.interval <= param->interval_max) {
+		return -EALREADY;
+	}
+#if 0
+	if ((conn->role == BT_HCI_ROLE_SLAVE) &&
+	    !(bt_dev.le.features[0] & BT_HCI_LE_CONN_PARAM_REQ_PROC)) {
+		return bt_l2cap_update_conn_param(conn, param);
+	}
+
+	if ((conn->le.features[0] & BT_HCI_LE_CONN_PARAM_REQ_PROC) &&
+	    (bt_dev.le.features[0] & BT_HCI_LE_CONN_PARAM_REQ_PROC)) {
+#endif
+		return bt_conn_le_conn_update(conn, param);
+#if 0
+	}
+	return -EBUSY;
+#endif
+}
+
+void on_nble_gap_scan_start_stop_rsp(const struct nble_response *rsp)
+{
+	if (rsp->status)
+		BT_INFO("scan start/stop failed: %d", rsp->status);
+	/* TODO: clear scanning bit atomic_clear_bit(bt_dev.flags, BT_DEV_SCANNING) */
+}
+
+static int bt_hci_stop_scanning(void)
+{
+#ifdef NOT_USED_FOR_NOW
+	struct net_buf *buf, *rsp;
+	struct bt_hci_cp_le_set_scan_enable *scan_enable;
+	int err;
+
+	if (!atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) {
+		return -EALREADY;
+	}
+
+	buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_ENABLE,
+				sizeof(*scan_enable));
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	scan_enable = net_buf_add(buf, sizeof(*scan_enable));
+	memset(scan_enable, 0, sizeof(*scan_enable));
+	scan_enable->filter_dup = BT_HCI_LE_SCAN_FILTER_DUP_DISABLE;
+	scan_enable->enable = BT_HCI_LE_SCAN_DISABLE;
+
+	err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_ENABLE, buf, &rsp);
+	if (err) {
+		return err;
+	}
+
+	/* Update scan state in case of success (0) status */
+	err = rsp->data[0];
+	if (!err) {
+		atomic_clear_bit(bt_dev.flags, BT_DEV_SCANNING);
+	}
+
+	net_buf_unref(rsp);
+
+	return err;
+#endif
+
+	nble_gap_stop_scan_req();
+
+	return 0;
+}
+
+#if defined(CONFIG_BLUETOOTH_CENTRAL)
+int bt_le_set_auto_conn(bt_addr_le_t *addr,
+			const struct bt_le_conn_param *param)
+{
+	return -EINVAL;
+}
+#endif /* CONFIG_BLUETOOTH_CENTRAL */
+
+
+static int start_le_scan(uint8_t scan_type, uint16_t interval, uint16_t window,
+			 uint8_t filter_dup)
+{
+	struct nble_gap_scan_params params = {
+			.interval = interval,
+			.window = window,
+			.scan_type = scan_type,
+	};
+
+#ifdef NOT_USED_FOR_NOW
+	struct net_buf *buf, *rsp;
+	struct bt_hci_cp_le_set_scan_params *set_param;
+	struct bt_hci_cp_le_set_scan_enable *scan_enable;
+	int err;
+
+	buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_PARAMS,
+				sizeof(*set_param));
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+
+	set_param = net_buf_add(buf, sizeof(*set_param));
+	memset(set_param, 0, sizeof(*set_param));
+	set_param->scan_type = scan_type;
+
+	/* for the rest parameters apply default values according to
+	 *  spec 4.2, vol2, part E, 7.8.10
+	 */
+	set_param->interval = sys_cpu_to_le16(interval);
+	set_param->window = sys_cpu_to_le16(window);
+	set_param->filter_policy = 0x00;
+
+	if (scan_type == BT_HCI_LE_SCAN_ACTIVE) {
+		err = le_set_nrpa();
+		if (err) {
+			net_buf_unref(buf);
+			return err;
+		}
+
+		set_param->addr_type = BT_ADDR_LE_RANDOM;
+	} else {
+		set_param->addr_type = BT_ADDR_LE_PUBLIC;
+	}
+
+	bt_hci_cmd_send(BT_HCI_OP_LE_SET_SCAN_PARAMS, buf);
+	buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_SCAN_ENABLE,
+				sizeof(*scan_enable));
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	scan_enable = net_buf_add(buf, sizeof(*scan_enable));
+	memset(scan_enable, 0, sizeof(*scan_enable));
+	scan_enable->filter_dup = filter_dup;
+	scan_enable->enable = BT_HCI_LE_SCAN_ENABLE;
+
+	err = bt_hci_cmd_send_sync(BT_HCI_OP_LE_SET_SCAN_ENABLE, buf, &rsp);
+	if (err) {
+		return err;
+	}
+	/* Update scan state in case of success (0) status */
+	err = rsp->data[0];
+	if (!err) {
+		atomic_set_bit(bt_dev.flags, BT_DEV_SCANNING);
+	}
+
+	net_buf_unref(rsp);
+#endif
+
+	nble_gap_start_scan_req(&params);
+
+	return 0;
+}
+
+#if NOT_USED_FOR_NOW
+/* Used to determine whether to start scan and which scan type should be used */
+int bt_le_scan_update(bool fast_scan)
+{
+#if defined(CONFIG_BLUETOOTH_CENTRAL)
+	uint16_t interval, window;
+	struct bt_conn *conn;
+#endif /* CONFIG_BLUETOOTH_CENTRAL */
+
+	if (atomic_test_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) {
+		return 0;
+	}
+
+	if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) {
+		int err;
+
+		err = bt_hci_stop_scanning();
+		if (err) {
+			return err;
+		}
+	}
+
+#if defined(CONFIG_BLUETOOTH_CENTRAL)
+	conn = bt_conn_lookup_state_le(NULL, BT_CONN_CONNECT_SCAN);
+	if (!conn) {
+		return 0;
+	}
+
+	bt_conn_unref(conn);
+
+	if (fast_scan) {
+		interval = BT_GAP_SCAN_FAST_INTERVAL;
+		window = BT_GAP_SCAN_FAST_WINDOW;
+	} else {
+		interval = BT_GAP_SCAN_SLOW_INTERVAL_1;
+		window = BT_GAP_SCAN_SLOW_WINDOW_1;
+	}
+
+	return start_le_scan(BT_HCI_LE_SCAN_PASSIVE, interval, window, 0x01);
+#else
+	return 0;
+#endif /* CONFIG_BLUETOOTH_CENTRAL */
+}
+#endif
+
+static int bt_init(void)
+{
+#if NOT_USED_FOR_NOW
+	struct bt_driver *drv = bt_dev.drv;
+#endif
+	int err = 0;
+
+#if NOT_USED_FOR_NOW
+	err = drv->open();
+	if (err) {
+		BT_ERR("HCI driver open failed (%d)", err);
+		return err;
+	}
+
+	err = hci_init();
+#endif
+
+	if (!err) {
+		err = bt_conn_init();
+	}
+
+	scan_dev_found_cb = NULL;
+	if (!err) {
+		atomic_set_bit(bt_dev.flags, BT_DEV_READY);
+#if 0
+		bt_le_scan_update(false);
+#endif
+	}
+
+	return err;
+}
+
+void on_nble_up(void)
+{
+    BT_DBG("%s", __FUNCTION__);
+	if (bt_ready_cb)
+		bt_ready_cb(bt_init());
+}
+
+extern void on_nble_curie_init(void);
+
+int bt_enable(bt_ready_cb_t cb)
+{
+	bt_ready_cb = cb;
+
+	on_nble_curie_init();
+
+	if (!cb) {
+		return bt_init();
+	}
+
+	return 0;
+}
+
+
+static bool valid_adv_param(const struct bt_le_adv_param *param)
+{
+	switch (param->type) {
+	case BT_LE_ADV_IND:
+	case BT_LE_ADV_SCAN_IND:
+	case BT_LE_ADV_NONCONN_IND:
+		break;
+	default:
+		return false;
+	}
+
+#if 0
+	/* checks done in Nordic */
+	switch (param->addr_type) {
+	case BT_LE_ADV_ADDR_IDENTITY:
+	case BT_LE_ADV_ADDR_NRPA:
+		break;
+	default:
+		return false;
+	}
+
+	if (param->interval_min > param->interval_max ||
+	    param->interval_min < 0x0020 || param->interval_max > 0x4000) {
+		return false;
+	}
+#endif
+
+	return true;
+}
+
+static int set_ad(struct bt_eir_data *p_ad_data,
+		const struct bt_data *ad, size_t ad_len)
+{
+	int i;
+
+	for (i = 0; i < ad_len; i++) {
+		/* Check if ad fit in the remaining buffer */
+		if (p_ad_data->len + ad[i].data_len + 2 > 31) {
+			return -EINVAL;
+		}
+
+		p_ad_data->data[p_ad_data->len++] = ad[i].data_len + 1;
+		p_ad_data->data[p_ad_data->len++] = ad[i].type;
+
+		memcpy(&p_ad_data->data[p_ad_data->len], ad[i].data,
+				ad[i].data_len);
+		p_ad_data->len += ad[i].data_len;
+	}
+
+	return 0;
+}
+
+int bt_le_adv_start(const struct bt_le_adv_param *param,
+			const struct bt_data *ad, size_t ad_len,
+			const struct bt_data *sd, size_t sd_len)
+{
+	int err;
+	struct nble_gap_adv_params set_param = {0};
+	struct nble_gap_ad_data_params data;
+
+	if (!valid_adv_param(param)) {
+		return -EINVAL;
+	}
+
+	memset(&data, 0, sizeof(data));
+
+	if (atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING)) {
+		return -EALREADY;
+	}
+
+	err = set_advertise_disable();
+	if (err) {
+		return err;
+	}
+	err = set_ad(&data.ad, ad, ad_len);
+	if (err) {
+		return err;
+	}
+
+	/*
+	 * Don't bother with scan response if the advertising type isn't
+	 * a scannable one.
+	 */
+	if (param->type == BT_LE_ADV_IND || param->type == BT_LE_ADV_SCAN_IND) {
+		err = set_ad(&data.sd, sd, sd_len);
+		if (err) {
+			return err;
+		}
+	}
+	nble_gap_set_adv_data_req(&data);
+
+	/* Timeout is handled by application timer */
+	set_param.timeout = 0;
+	/* forced to none currently (no whitelist support) */
+	set_param.filter_policy = 0;
+	set_param.interval_max = param->interval_max;
+	set_param.interval_min = param->interval_min;
+	set_param.type = param->type;
+	nble_gap_set_adv_params_req(&set_param);
+
+#if 0
+	if (param->addr_type == BT_LE_ADV_ADDR_NRPA) {
+		err = le_set_nrpa();
+		if (err) {
+			net_buf_unref(buf);
+			return err;
+		}
+
+		set_param->own_addr_type = BT_ADDR_LE_RANDOM;
+	} else {
+		set_param->own_addr_type = BT_ADDR_LE_PUBLIC;
+	}
+
+	bt_hci_cmd_send(BT_HCI_OP_LE_SET_ADV_PARAMETERS, buf);
+#endif
+
+	err = set_advertise_enable();
+	if (err) {
+		return err;
+	}
+
+	atomic_set_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING);
+
+	return 0;
+}
+
+void on_nble_gap_dir_adv_timeout_evt(const struct nble_gap_dir_adv_timeout_evt *p_evt)
+{
+	struct bt_conn *conn = bt_conn_lookup_state_le(BT_ADDR_LE_ANY, BT_CONN_CONNECT);
+
+	if (conn) {
+		atomic_clear_bit(bt_dev.flags, BT_DEV_ADVERTISING);
+		conn->err = p_evt->error;
+		bt_conn_set_state(conn, BT_CONN_DISCONNECTED);
+		bt_conn_unref(conn);
+	}
+}
+
+int bt_le_adv_stop(void)
+{
+	int err;
+
+	if (!atomic_test_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING)) {
+		return -EALREADY;
+	}
+
+	err = set_advertise_disable();
+	if (err) {
+		return err;
+	}
+	atomic_clear_bit(bt_dev.flags, BT_DEV_KEEP_ADVERTISING);
+
+	return 0;
+}
+
+static bool valid_le_scan_param(const struct bt_le_scan_param *param)
+{
+	if (param->type != BT_HCI_LE_SCAN_PASSIVE &&
+	    param->type != BT_HCI_LE_SCAN_ACTIVE) {
+		return false;
+	}
+
+	if (/* param->filter_dup != BT_HCI_LE_SCAN_FILTER_DUP_DISABLE */
+	    /* && nble always filters duplicates */
+	    param->filter_dup != BT_HCI_LE_SCAN_FILTER_DUP_ENABLE) {
+		return false;
+	}
+
+	if (param->interval < 0x0004 || param->interval > 0x4000) {
+		return false;
+	}
+
+	if (param->window < 0x0004 || param->window > 0x4000) {
+		return false;
+	}
+
+	if (param->window > param->interval) {
+		return false;
+	}
+
+	return true;
+}
+
+int bt_le_scan_start(const struct bt_le_scan_param *param, bt_le_scan_cb_t cb)
+{
+
+	int err;
+
+	/* Check that the parameters have valid values */
+	if (!valid_le_scan_param(param)) {
+		return -EINVAL;
+	}
+#if NOT_USED_FOR_NOW
+	/* Return if active scan is already enabled */
+	if (atomic_test_and_set_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) {
+		return -EALREADY;
+	}
+
+	if (atomic_test_bit(bt_dev.flags, BT_DEV_SCANNING)) {
+		err = bt_hci_stop_scanning();
+		if (err) {
+			atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN);
+			return err;
+		}
+	}
+#endif
+
+	err = start_le_scan(param->type, param->interval, param->window,
+			param->filter_dup);
+
+
+	if (err) {
+#if NOT_USED_FOR_NOW
+		atomic_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN);
+#endif
+		return err;
+	}
+
+	scan_dev_found_cb = cb;
+
+	return 0;
+}
+
+int bt_le_scan_stop(void)
+{
+#if NOT_USED_FOR_NOW
+	/* Return if active scanning is already disabled */
+	if (!atomic_test_and_clear_bit(bt_dev.flags, BT_DEV_EXPLICIT_SCAN)) {
+		return -EALREADY;
+	}
+#endif
+	scan_dev_found_cb = NULL;
+
+#if NOT_USED_FOR_NOW
+	return bt_le_scan_update(false);
+#else
+	return bt_hci_stop_scanning();
+#endif
+}
+
+/* Temporary RSSI patch for UAS: RPC need definition if UAS not compiled */
+__attribute__((weak))
+void on_nble_uas_bucket_change(const struct nble_uas_bucket_change *p_params)
+{
+}
+
+void ble_gap_set_rssi_report(struct nble_rssi_report_params *params,
+				struct bt_conn *conn,
+				rssi_report_resp_t resp_cb, rssi_report_t evt_cb)
+{
+	rssi_report_cb = evt_cb;
+
+	params->conn_handle = conn->handle;
+
+	nble_gap_set_rssi_report_req(params, resp_cb);
+}
+
+void on_nble_gap_set_rssi_report_rsp(const struct nble_response *params)
+{
+	rssi_report_resp_t resp_cb = params->user_data;
+
+	if (resp_cb)
+		resp_cb(params->status);
+}
+
+void on_nble_gap_rssi_evt(const struct nble_gap_rssi_evt *event)
+{
+	if (rssi_report_cb)
+		rssi_report_cb(event->rssi_data);
+}
+
+void ble_gap_set_tx_power(int8_t tx_power)
+{
+	struct nble_gap_tx_power_params params = {
+		.tx_power = tx_power,
+	};
+	nble_gap_tx_power_req(&params);
+}
+
+void on_nble_gap_tx_power_rsp(const struct nble_response *params)
+{
+}
+
+void ble_gap_get_version(ble_get_version_cb_t func)
+{
+	struct nble_gap_get_version_param params;
+
+	params.cb = func;
+
+	nble_get_version_req(&params);
+}
+
+void on_nble_get_version_rsp(const struct nble_version_response *par)
+{
+	struct nble_gap_get_version_param param = par->params;
+	ble_get_version_cb_t cb = param.cb;
+
+	if (cb) {
+		cb(&par->ver);
+	}
+}
+
+void bt_le_set_device_name(char *device_name, int len)
+{
+    struct nble_gap_service_write_params gap_service_params;
+    if (len > 20)
+        len = 20;
+    memset(&gap_service_params, 0, sizeof(gap_service_params));
+    gap_service_params.attr_type = NBLE_GAP_SVC_ATTR_NAME;
+    gap_service_params.name.len = len;
+    gap_service_params.name.sec_mode = 0x11;// GAP_SEC_LEVEL_1 | GAP_SEC_MODE_1;
+    memcpy(gap_service_params.name.name_array, device_name, len);
+    nble_gap_service_write_req(&gap_service_params);
+}
+
+void bt_le_set_mac_address(bt_addr_le_t bda)
+{
+    // Update the MAC addr
+    struct nble_set_bda_params params;
+    params.cb = NULL;
+    params.user_data = NULL;
+    params.bda = bda;
+
+    nble_set_bda_req(&params);
+}
+
diff --git a/system/libarc32_arduino101/framework/src/services/ble/gatt.c b/system/libarc32_arduino101/framework/src/services/ble/gatt.c
new file mode 100644
index 00000000..fe5ce8e0
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/gatt.c
@@ -0,0 +1,1570 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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 <limits.h>
+#include <string.h>
+#include <errno.h>
+#include <atomic.h>
+
+#include <misc/byteorder.h>
+
+#include <bluetooth/gatt.h>
+#include "gatt_internal.h"
+#include "hci_core.h"
+#include "conn_internal.h"
+
+/* #define BT_GATT_DEBUG 1 */
+
+extern void on_nble_curie_log(char *fmt, ...);
+extern void __assert_fail(void);
+#ifdef BT_GATT_DEBUG
+#define BT_DBG(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#else
+#define BT_DBG(fmt, ...) do {} while (0)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#endif
+
+#define N_BLE_BUF_SIZE 512
+
+struct ble_gatt_service {
+	struct bt_gatt_attr *attrs; /* Pointer to the array of attributes */
+	uint16_t attr_count; /* Number of attributes in the array */
+};
+
+struct ble_gatts_flush_all {
+	struct bt_conn *conn;
+	int status;
+	uint8_t flag;
+};
+
+static struct ble_gatt_service db[CONFIG_BT_GATT_BLE_MAX_SERVICES];
+
+static uint8_t db_cnt;
+
+#if defined(CONFIG_BLUETOOTH_GATT_CLIENT)
+static struct bt_gatt_subscribe_params *subscriptions;
+#endif
+
+/**
+ * Copy a UUID in a buffer using the smallest memory length
+ * @param buf Pointer to the memory where the UUID shall be copied
+ * @param uuid Pointer to the UUID to copy
+ * @return The length required to store the UUID in the memory
+ */
+static uint8_t bt_gatt_uuid_memcpy(uint8_t *buf,
+				   const struct bt_uuid *uuid)
+{
+	uint8_t *ptr = buf;
+
+	/* Store the type of the UUID */
+	*ptr = uuid->type;
+	ptr++;
+
+	/* Store the UUID data */
+	if (uuid->type == BT_UUID_TYPE_16) {
+		uint16_t le16;
+        
+        memcpy(&le16, &BT_UUID_16(uuid)->val, sizeof(le16));
+		le16 = sys_cpu_to_le16(le16);
+		memcpy(ptr, &le16, sizeof(le16));
+		ptr += sizeof(le16);
+	} else {
+		memcpy(ptr, BT_UUID_128(uuid)->val,
+		       sizeof(BT_UUID_128(uuid)->val));
+		ptr += sizeof(BT_UUID_128(uuid)->val);
+	}
+	return ptr - buf;
+}
+
+/* These attributes need the value to be read */
+static struct bt_uuid *whitelist[] = {
+	BT_UUID_GATT_PRIMARY,
+	BT_UUID_GATT_SECONDARY,
+	BT_UUID_GATT_INCLUDE,
+	BT_UUID_GATT_CHRC,
+	BT_UUID_GATT_CEP,
+	BT_UUID_GATT_CUD,
+	BT_UUID_GATT_CPF,
+	BT_UUID_GAP_DEVICE_NAME,
+	BT_UUID_GAP_APPEARANCE,
+	BT_UUID_GAP_PPCP
+};
+
+static int attr_read(struct bt_gatt_attr *attr, uint8_t *data, size_t len)
+{
+	uint8_t i;
+	int data_size;
+
+	if (!data || len < 0) {
+		return -ENOMEM;
+	}
+
+	data_size = bt_gatt_uuid_memcpy(data, attr->uuid);
+
+	for (i = 0; i < ARRAY_SIZE(whitelist); i++) {
+		if (!bt_uuid_cmp(attr->uuid, whitelist[i])) {
+			int read;
+
+			read = attr->read(NULL, attr, data + data_size, len,
+					  0);
+			if (read < 0) {
+				return read;
+			}
+
+			data_size += read;
+			break;
+		}
+	}
+
+	return data_size;
+}
+
+int bt_gatt_register(struct bt_gatt_attr *attrs, size_t count)
+{
+	size_t attr_table_size, i;
+	struct nble_gatt_register_req param;
+	/* TODO: Replace the following with net_buf */
+	uint8_t attr_table[N_BLE_BUF_SIZE];
+
+	if (!attrs || !count) {
+		return -EINVAL;
+	}
+	BT_ASSERT(db_cnt < ARRAY_SIZE(db));
+
+	db[db_cnt].attrs = attrs;
+	db[db_cnt].attr_count = count;
+	db_cnt++;
+	param.attr_base = attrs;
+	param.attr_count = count;
+
+	attr_table_size = 0;
+
+	for (i = 0; i < count; i++) {
+		struct bt_gatt_attr *attr = &attrs[i];
+		struct ble_gatt_attr *att;
+		int data_size;
+
+		if (attr_table_size + sizeof(*att) > sizeof(attr_table)) {
+			return -ENOMEM;
+		}
+
+		att = (void *)&attr_table[attr_table_size];
+		att->perm = attr->perm;
+
+		/* Read attribute data */
+		data_size = attr_read(attr, att->data,
+				      sizeof(attr_table) -
+				      (attr_table_size + sizeof(*att)));
+		if (data_size < 0) {
+			return data_size;
+		}
+		att->data_size = data_size;
+
+		BT_DBG("table size = %u attr data_size = %u", attr_table_size,
+		       att->data_size);
+
+		/* Compute the new element size and align it on upper 4 bytes
+		 * boundary.
+		 */
+		attr_table_size += (sizeof(*att) + att->data_size + 3) & ~3;
+	}
+
+	nble_gatt_register_req(&param, attr_table, attr_table_size);
+	return 0;
+}
+
+void on_nble_gatt_register_rsp(const struct nble_gatt_register_rsp *rsp,
+		const struct nble_gatt_attr_handles *handles, uint8_t len)
+{
+
+	if (rsp->status != 0) {
+		BT_ERR("failure registering table: %d - %u - %p", rsp->status,
+		       rsp->attr_count, rsp->attr_base);
+	}
+#ifdef BT_GATT_DEBUG
+	BT_DBG("register rsp : s=%d - b=%p - c=%u", rsp->status,
+	       rsp->attr_base, rsp->attr_count);
+	{
+		int i;
+
+		for (i = 0; i < rsp->attr_count; i++) {
+			if (handles[i].handle != 0) {
+                uint16_t le16;
+                memcpy(&le16, &BT_UUID_16(rsp->attr_base[i].uuid)->val, sizeof(le16));
+				BT_DBG("gatt: i %d, h %d, type %d, u16 0x%x",
+				       i, handles[i].handle,
+				       rsp->attr_base[i].uuid->type,
+				       le16);
+			}
+		}
+	}
+#endif
+}
+
+ssize_t bt_gatt_attr_read(struct bt_conn *conn, const struct bt_gatt_attr *attr,
+			  void *buf, uint16_t buf_len, uint16_t offset,
+			  const void *value, uint16_t value_len)
+{
+	uint16_t len;
+
+	if (offset > value_len) {
+		return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
+	}
+
+	len = min(buf_len, value_len - offset);
+
+	BT_DBG("handle 0x%04x offset %u length %u", attr->handle, offset,
+	       len);
+
+	memcpy(buf, value + offset, len);
+
+	return len;
+}
+
+ssize_t bt_gatt_attr_read_service(struct bt_conn *conn,
+				  const struct bt_gatt_attr *attr,
+				  void *buf, uint16_t len, uint16_t offset)
+{
+	struct bt_uuid *uuid = attr->user_data;
+
+	if (uuid->type == BT_UUID_TYPE_16) {
+		uint16_t uuid16;
+
+        memcpy(&uuid16, &BT_UUID_16(uuid)->val, sizeof(uuid16));
+        uuid16 = sys_cpu_to_le16(uuid16);
+
+		return bt_gatt_attr_read(conn, attr, buf, len, offset,
+					 &uuid16, 2);
+	}
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset,
+				 BT_UUID_128(uuid)->val, 16);
+}
+
+struct gatt_incl {
+	uint16_t start_handle;
+	uint16_t end_handle;
+	uint16_t uuid16;
+} __packed;
+
+ssize_t bt_gatt_attr_read_included(struct bt_conn *conn,
+				   const struct bt_gatt_attr *attr,
+				   void *buf, uint16_t len, uint16_t offset)
+{
+	struct bt_gatt_include *incl = attr->user_data;
+	struct gatt_incl pdu;
+	uint8_t value_len;
+
+	pdu.start_handle = sys_cpu_to_le16(incl->start_handle);
+	pdu.end_handle = sys_cpu_to_le16(incl->end_handle);
+	value_len = sizeof(pdu.start_handle) + sizeof(pdu.end_handle);
+
+	/*
+	 * Core 4.2, Vol 3, Part G, 3.2,
+	 * The Service UUID shall only be present when the UUID is a 16-bit
+	 * Bluetooth UUID.
+	 */
+	if (incl->uuid->type == BT_UUID_TYPE_16) {
+        memcpy(&pdu.uuid16, &BT_UUID_16(incl->uuid)->val, sizeof(pdu.uuid16));
+		pdu.uuid16 = sys_cpu_to_le16(pdu.uuid16);
+		value_len += sizeof(pdu.uuid16);
+	}
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset, &pdu, value_len);
+}
+
+struct gatt_chrc {
+	uint8_t properties;
+	uint16_t value_handle;
+	union {
+		uint16_t uuid16;
+		uint8_t  uuid[16];
+	};
+} __packed;
+
+ssize_t bt_gatt_attr_read_chrc(struct bt_conn *conn,
+			       const struct bt_gatt_attr *attr, void *buf,
+			       uint16_t len, uint16_t offset)
+{
+	struct bt_gatt_chrc *chrc = attr->user_data;
+	struct gatt_chrc pdu;
+	uint8_t value_len;
+
+	pdu.properties = chrc->properties;
+	/* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part G] page 534:
+	 * 3.3.2 Characteristic Value Declaration
+	 * The Characteristic Value declaration contains the value of the
+	 * characteristic. It is the first Attribute after the characteristic
+	 * declaration. All characteristic definitions shall have a
+	 * Characteristic Value declaration.
+	 */
+#if 0
+	next = bt_gatt_attr_next(attr);
+	if (!next) {
+		BT_WARN("No value for characteristic at 0x%04x", attr->handle);
+		pdu.value_handle = 0x0000;
+	} else {
+		pdu.value_handle = sys_cpu_to_le16(next->handle);
+	}
+#else
+	pdu.value_handle = 0x0000;
+#endif
+	value_len = sizeof(pdu.properties) + sizeof(pdu.value_handle);
+
+	if (chrc->uuid->type == BT_UUID_TYPE_16) {
+        memcpy(&pdu.uuid16, &BT_UUID_16(chrc->uuid)->val, sizeof(pdu.uuid16));
+		pdu.uuid16 = sys_cpu_to_le16(pdu.uuid16);
+		value_len += 2;
+	} else {
+		memcpy(pdu.uuid, BT_UUID_128(chrc->uuid)->val, 16);
+		value_len += 16;
+	}
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset, &pdu, value_len);
+}
+
+void bt_gatt_foreach_attr(uint16_t start_handle, uint16_t end_handle,
+			  bt_gatt_attr_func_t func, void *user_data)
+{
+
+	const struct bt_gatt_attr *attr;
+
+	BT_ASSERT(start_handle == 1 && end_handle == 0xFFFF);
+
+	for (attr = db[0].attrs; attr; attr = bt_gatt_attr_next(attr)) {
+#if 0
+		/* Check if attribute handle is within range */
+		if (attr->handle < start_handle || attr->handle > end_handle) {
+			continue;
+		}
+#endif
+
+		if (func(attr, user_data) == BT_GATT_ITER_STOP) {
+			break;
+		}
+	}
+}
+
+struct bt_gatt_attr *bt_gatt_attr_next(const struct bt_gatt_attr *attr)
+{
+	struct ble_gatt_service *svc, *svc_last;
+
+	svc_last = &db[db_cnt];
+
+	for (svc = db; svc < svc_last; svc++) {
+		if (attr >= svc->attrs && attr < &svc->attrs[svc->attr_count]) {
+			int index = attr - &svc->attrs[0];
+
+			if (index < (svc->attr_count - 1)) {
+				return (struct bt_gatt_attr *)&attr[1];
+			} else if ((svc + 1) < svc_last) {
+				return (svc + 1)->attrs;
+			} else {
+				return NULL;
+			}
+		}
+	}
+	/* Normally, we should not reach here */
+	return NULL;
+}
+
+ssize_t bt_gatt_attr_read_ccc(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset)
+{
+	return BT_GATT_ERR(BT_ATT_ERR_NOT_SUPPORTED);
+
+#if 0
+	struct _bt_gatt_ccc *ccc = attr->user_data;
+	uint16_t value;
+	size_t i;
+
+	for (i = 0; i < ccc->cfg_len; i++) {
+		if (bt_addr_le_cmp(&ccc->cfg[i].peer, &conn->le.dst)) {
+			continue;
+		}
+
+		value = sys_cpu_to_le16(ccc->cfg[i].value);
+		break;
+	}
+
+	/* Default to disable if there is no cfg for the peer */
+	if (i == ccc->cfg_len) {
+		value = 0x0000;
+	}
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset, &value,
+				 sizeof(value));
+#endif
+}
+
+static void gatt_ccc_changed(struct _bt_gatt_ccc *ccc)
+{
+	int i;
+	uint16_t value = 0x0000;
+
+	for (i = 0; i < ccc->cfg_len; i++) {
+		if (ccc->cfg[i].value > value) {
+			value = ccc->cfg[i].value;
+		}
+	}
+
+	BT_DBG("ccc %p value 0x%04x", ccc, value);
+
+	if (value != ccc->value) {
+		ccc->value = value;
+		if (ccc->cfg_changed)
+			ccc->cfg_changed(value);
+	}
+}
+
+#if defined(CONFIG_BLUETOOTH_GATT_CLIENT)
+static bool is_bonded(const bt_addr_le_t *addr)
+{
+#if defined(CONFIG_BLUETOOTH_SMP)
+	struct bt_conn *conn = bt_conn_lookup_addr_le(addr);
+
+	/*
+	 *  this is a temporary workaround. if encrypt is set, we know we are
+	 *  paired. nble does not yet report if device is bonded.
+	 */
+	if (conn) {
+		uint8_t encrypt = conn->encrypt;
+
+		bt_conn_unref(conn);
+
+		return encrypt;
+	}
+	return false;
+#else
+	return false;
+#endif /* defined(CONFIG_BLUETOOTH_SMP) */
+}
+#endif
+
+ssize_t bt_gatt_attr_write_ccc(struct bt_conn *conn,
+			       const struct bt_gatt_attr *attr, const void *buf,
+			       uint16_t len, uint16_t offset)
+{
+	struct _bt_gatt_ccc *ccc = attr->user_data;
+	const uint16_t *data = buf;
+	size_t i;
+
+	//BT_DBG("%s", __FUNCTION__);
+
+	if (offset > sizeof(*data)) {
+		return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
+	}
+
+	//BT_DBG("%s1", __FUNCTION__);
+
+	if (offset + len > sizeof(*data)) {
+		return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN);
+	}
+
+	//BT_DBG("%s2", __FUNCTION__);
+
+	for (i = 0; i < ccc->cfg_len; i++) {
+		/* Check for existing configuration */
+		if (!bt_addr_le_cmp(&ccc->cfg[i].peer, &conn->le.dst)) {
+			break;
+		}
+	}
+
+	//BT_DBG("%s3", __FUNCTION__);
+
+	if (i == ccc->cfg_len) {
+		for (i = 0; i < ccc->cfg_len; i++) {
+			/* Check for unused configuration */
+			if (!ccc->cfg[i].valid) {
+				bt_addr_le_copy(&ccc->cfg[i].peer, &conn->le.dst);
+#if NOT_USED_FOR_THE_TIME_BEING
+				/* Only set valid if bonded */
+				ccc->cfg[i].valid = is_bonded(&conn->le.dst);
+#endif
+				break;
+			}
+		}
+
+		if ((i == ccc->cfg_len) && (ccc->cfg_len)) {
+			BT_WARN("No space to store CCC cfg");
+			return -ENOMEM;
+		}
+	}
+
+	//BT_DBG("%s len-%p", __FUNCTION__, ccc);
+    
+	ccc->cfg[i].value = sys_le16_to_cpu(*data);
+	//BT_DBG("%s 5len-%d", __FUNCTION__, len);
+
+	//BT_DBG("handle 0x%04x value %u", attr->handle, *data);
+
+	/* Update cfg if don't match */
+	if (ccc->value != *data) {
+		gatt_ccc_changed(ccc);
+	}
+
+	return len;
+}
+
+ssize_t bt_gatt_attr_read_cep(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset)
+{
+	struct bt_gatt_cep *value = attr->user_data;
+	uint16_t props = sys_cpu_to_le16(value->properties);
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset, &props,
+				 sizeof(props));
+}
+
+ssize_t bt_gatt_attr_read_cud(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset)
+{
+	char *value = attr->user_data;
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset, value, strlen(value));
+}
+
+ssize_t bt_gatt_attr_read_cpf(struct bt_conn *conn,
+			      const struct bt_gatt_attr *attr, void *buf,
+			      uint16_t len, uint16_t offset)
+{
+	struct bt_gatt_cpf *value = attr->user_data;
+
+	return bt_gatt_attr_read(conn, attr, buf, len, offset, value,
+				 sizeof(*value));
+}
+
+struct notify_data {
+	uint16_t state;
+	uint16_t type;
+	const struct bt_gatt_attr *attr;
+	const void *data;
+	uint16_t len;
+	bt_gatt_notify_sent_func_t notify_cb;
+	struct bt_gatt_indicate_params *params;
+};
+
+static int att_notify(struct bt_conn *conn, const struct bt_gatt_attr *attr,
+		      const void *data, size_t len,
+		      bt_gatt_notify_sent_func_t cb)
+{
+	struct nble_gatt_send_notif_params notif;
+
+	notif.conn_handle = conn->handle;
+	notif.params.attr = attr;
+	notif.params.offset = 0;
+	notif.cback = cb;
+
+	nble_gatt_send_notif_req(&notif, data, len);
+
+	return 0;
+}
+
+void on_nble_gatts_send_notif_rsp(const struct nble_gatt_notif_rsp *rsp)
+{
+	struct bt_conn *conn;
+
+	conn = bt_conn_lookup_handle(rsp->conn_handle);
+
+	if (conn) {
+		if (rsp->cback) {
+			rsp->cback(conn, rsp->attr, rsp->status);
+		}
+		bt_conn_unref(conn);
+	}
+}
+
+void on_nble_gatts_send_ind_rsp(const struct nble_gatt_ind_rsp *rsp)
+{
+	struct bt_conn *conn;
+
+	conn = bt_conn_lookup_handle(rsp->conn_handle);
+
+	if (conn) {
+		if (rsp->cback) {
+			rsp->cback(conn, rsp->attr, rsp->status);
+		}
+		bt_conn_unref(conn);
+	}
+}
+
+static int att_indicate(struct bt_conn *conn,
+			struct bt_gatt_indicate_params *params)
+{
+	struct nble_gatt_send_ind_params ind;
+
+	ind.conn_handle = conn->handle;
+	ind.cback = params->func;
+	ind.params.attr = params->attr;
+	ind.params.offset = 0;
+
+	nble_gatt_send_ind_req(&ind, params->data, params->len);
+
+	return 0;
+}
+
+static uint8_t notify_cb(const struct bt_gatt_attr *attr, void *user_data)
+{
+	struct notify_data *data = user_data;
+	struct _bt_gatt_ccc *ccc;
+	size_t i;
+
+	/* Check if the attribute was reached */
+	if (data->state == 0) {
+		if (attr == data->attr)
+			data->state = 1;
+		return BT_GATT_ITER_CONTINUE;
+	}
+
+	if (bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CCC)) {
+		/* Stop if we reach the next characteristic */
+		if (!bt_uuid_cmp(attr->uuid, BT_UUID_GATT_CHRC)) {
+			return BT_GATT_ITER_STOP;
+		}
+		return BT_GATT_ITER_CONTINUE;
+	}
+
+	/* Check attribute user_data must be of type struct _bt_gatt_ccc */
+	if (attr->write != bt_gatt_attr_write_ccc) {
+		return BT_GATT_ITER_CONTINUE;
+	}
+
+	ccc = attr->user_data;
+
+	/* Notify all peers configured */
+	for (i = 0; i < ccc->cfg_len; i++) {
+		struct bt_conn *conn;
+		int err;
+
+		if (ccc->value != data->type) {
+			continue;
+		}
+
+		conn = bt_conn_lookup_addr_le(&ccc->cfg[i].peer);
+		if (!conn || conn->state != BT_CONN_CONNECTED) {//Bug here
+			continue;
+		}
+
+		if (data->type == BT_GATT_CCC_INDICATE) {
+			err = att_indicate(conn, data->params);
+
+		} else {
+			err = att_notify(conn, data->attr, data->data,
+					 data->len, data->notify_cb);
+		}
+
+		bt_conn_unref(conn);
+
+		if (err < 0) {
+			return BT_GATT_ITER_STOP;
+		}
+	}
+
+	return BT_GATT_ITER_CONTINUE;
+}
+
+int bt_gatt_notify(struct bt_conn *conn, const struct bt_gatt_attr *attr,
+		   const void *data, uint16_t len,
+		   bt_gatt_notify_sent_func_t cb)
+{
+	struct notify_data nfy;
+
+	if (!attr) {
+		return -EINVAL;
+	}
+
+	if (conn) {
+		return att_notify(conn, attr, data, len, cb);
+	}
+
+	nfy.state = 0;
+	nfy.attr = attr;
+	nfy.type = BT_GATT_CCC_NOTIFY;
+	nfy.data = data;
+	nfy.len = len;
+	nfy.notify_cb = cb;
+
+	bt_gatt_foreach_attr(1, 0xffff, notify_cb, &nfy);
+
+	return 0;
+}
+
+int bt_gatt_indicate(struct bt_conn *conn,
+		     struct bt_gatt_indicate_params *params)
+{
+	struct notify_data nfy;
+
+	if (!params || !params->attr) {
+		return -EINVAL;
+	}
+
+	if (conn) {
+		return att_indicate(conn, params);
+	}
+
+	nfy.state = 0;
+	nfy.type = BT_GATT_CCC_INDICATE;
+	nfy.params = params;
+
+	bt_gatt_foreach_attr(1, 0xffff, notify_cb, &nfy);
+
+	return 0;
+}
+
+static uint8_t connected_cb(const struct bt_gatt_attr *attr, void *user_data)
+{
+	struct bt_conn *conn = user_data;
+	struct _bt_gatt_ccc *ccc;
+	size_t i;
+
+	/* Check attribute user_data must be of type struct _bt_gatt_ccc */
+	if (attr->write != bt_gatt_attr_write_ccc) {
+		return BT_GATT_ITER_CONTINUE;
+	}
+
+	ccc = attr->user_data;
+
+	/* If already enabled skip */
+	if (ccc->value) {
+		return BT_GATT_ITER_CONTINUE;
+	}
+
+	for (i = 0; i < ccc->cfg_len; i++) {
+		/* Ignore configuration for different peer */
+		if (bt_addr_le_cmp(&conn->le.dst, &ccc->cfg[i].peer)) {
+			continue;
+		}
+
+		if (ccc->cfg[i].value) {
+			gatt_ccc_changed(ccc);
+			return BT_GATT_ITER_CONTINUE;
+		}
+	}
+
+	return BT_GATT_ITER_CONTINUE;
+}
+
+static uint8_t disconnected_cb(const struct bt_gatt_attr *attr, void *user_data)
+{
+	struct bt_conn *conn = user_data;
+	struct _bt_gatt_ccc *ccc;
+	size_t i;
+
+	/* Check attribute user_data must be of type struct _bt_gatt_ccc */
+	if (attr->write != bt_gatt_attr_write_ccc) {
+		return BT_GATT_ITER_CONTINUE;
+	}
+
+	ccc = attr->user_data;
+
+	/* If already disabled skip */
+	if (!ccc->value) {
+		return BT_GATT_ITER_CONTINUE;
+	}
+
+	for (i = 0; i < ccc->cfg_len; i++) {
+		/* Ignore configurations with disabled value */
+		if (!ccc->cfg[i].value) {
+			continue;
+		}
+
+		if (bt_addr_le_cmp(&conn->le.dst, &ccc->cfg[i].peer)) {
+			struct bt_conn *tmp;
+
+			/* Skip if there is another peer connected */
+			tmp = bt_conn_lookup_addr_le(&ccc->cfg[i].peer);
+			if (tmp) {
+				if (tmp->state == BT_CONN_CONNECTED) {
+					bt_conn_unref(tmp);
+					return BT_GATT_ITER_CONTINUE;
+				}
+
+				bt_conn_unref(tmp);
+			}
+		}
+	}
+
+	/* Reset value while disconnected */
+	memset(&ccc->value, 0, sizeof(ccc->value));
+
+	if (ccc->cfg_changed) {
+		ccc->cfg_changed(ccc->value);
+	}
+
+	BT_DBG("ccc %p reseted", ccc);
+
+	return BT_GATT_ITER_CONTINUE;
+}
+
+void on_nble_gatts_write_evt(const struct nble_gatt_wr_evt *evt,
+			    const uint8_t *buf, uint8_t buflen)
+{
+	const struct bt_gatt_attr *attr = evt->attr;
+	struct bt_conn *conn = bt_conn_lookup_handle(evt->conn_handle);
+	struct nble_gatts_wr_reply_params reply_data;
+
+	//BT_DBG("write_evt %p", attr);
+
+	/* Check for write support and flush support in case of prepare */
+	if (!attr->write ||
+	    ((evt->flag & NBLE_GATT_WR_FLAG_PREP) && !attr->flush)) {
+		reply_data.status = BT_GATT_ERR(BT_ATT_ERR_WRITE_NOT_PERMITTED);
+
+		goto reply;
+	}
+	//BT_DBG("%s", __FUNCTION__);
+
+	reply_data.status = attr->write(conn, attr, buf, buflen, evt->offset);
+	if (reply_data.status < 0) {
+        
+        //BT_DBG("%s1-1", __FUNCTION__);
+
+		goto reply;
+	}
+
+	//BT_DBG("%s1", __FUNCTION__);
+
+	/* Return an error if not all data has been written */
+	if (reply_data.status != buflen) {
+		reply_data.status = BT_GATT_ERR(BT_ATT_ERR_UNLIKELY);
+
+		goto reply;
+	}
+
+	//BT_DBG("%s2", __FUNCTION__);
+
+	if (attr->flush && !(evt->flag & NBLE_GATT_WR_FLAG_PREP))
+		reply_data.status = attr->flush(conn, attr, BT_GATT_FLUSH_SYNC);
+
+	//BT_DBG("%s3", __FUNCTION__);
+
+reply:
+	//BT_DBG("%s4", __FUNCTION__);
+	if (evt->flag & NBLE_GATT_WR_FLAG_REPLY) {
+		reply_data.conn_handle = evt->conn_handle;
+
+		nble_gatts_wr_reply_req(&reply_data);
+	}
+
+	if (conn)
+		bt_conn_unref(conn);
+}
+
+static uint8_t flush_all(const struct bt_gatt_attr *attr, void *user_data)
+{
+	struct ble_gatts_flush_all *flush_data = user_data;
+
+	if (attr->flush) {
+		int status = attr->flush(flush_data->conn, attr, flush_data->flag);
+
+		if (status < 0 && flush_data->status == 0)
+			flush_data->status = status;
+	}
+
+	return BT_GATT_ITER_CONTINUE;
+}
+
+void on_nble_gatts_write_exec_evt(const struct nble_gatt_wr_exec_evt *evt)
+{
+	BT_DBG("write_exec_evt");
+
+	struct ble_gatts_flush_all flush_data = {
+		.conn = bt_conn_lookup_handle(evt->conn_handle),
+		.flag = evt->flag,
+		.status = 0,
+	};
+
+	bt_gatt_foreach_attr(0x0001, 0xFFFF, flush_all, &flush_data);
+
+	struct nble_gatts_wr_reply_params reply_data = {
+		.conn_handle = evt->conn_handle,
+		.status = flush_data.status,
+	};
+	nble_gatts_wr_reply_req(&reply_data);
+
+	bt_conn_unref(flush_data.conn);
+}
+
+void on_nble_gatts_read_evt(const struct nble_gatt_rd_evt *evt)
+{
+	struct nble_gatts_rd_reply_params reply_data;
+	const struct bt_gatt_attr *attr;
+	/* The length of the value sent back in the response is unknown because
+	 * of NRF API limitation, so we use the max possible one: ATT_MTU-1 */
+	uint8_t data[BLE_GATT_MTU_SIZE - 1] = { 0 };
+	int len;
+
+	attr = evt->attr;
+
+	BT_DBG("read_evt %p", attr);
+
+	memset(&reply_data, 0, sizeof(reply_data));
+
+	if (attr->read) {
+		struct bt_conn *conn = bt_conn_lookup_handle(evt->conn_handle);
+
+		len = attr->read(conn, attr, data, sizeof(data), evt->offset);
+
+		if (conn)
+			bt_conn_unref(conn);
+
+	} else {
+		len = BT_GATT_ERR(BT_ATT_ERR_READ_NOT_PERMITTED);
+	}
+
+	/* status >= 0 is considered as success by nble */
+	reply_data.status = len;
+
+	if (len < 0) {
+		len = 0;
+	}
+
+	reply_data.conn_handle = evt->conn_handle;
+
+	/* offset is needed by nble even in error case */
+	reply_data.offset = evt->offset;
+
+	nble_gatts_rd_reply_req(&reply_data, data, len);
+}
+
+#if defined(CONFIG_BLUETOOTH_GATT_CLIENT)
+void on_nble_gattc_value_evt(const struct ble_gattc_value_evt *evt,
+		uint8_t *data, uint8_t length)
+{
+	struct bt_gatt_subscribe_params *params;
+	struct bt_conn *conn;
+
+	conn = bt_conn_lookup_handle(evt->conn_handle);
+
+	//BT_DBG("FUNC %s", __FUNCTION__);
+
+	if (conn) {
+		for (params = subscriptions; params; params = params->_next) {
+			if (evt->handle != params->value_handle) {
+				continue;
+			}
+
+			if (params->notify(conn, params, data, length) ==
+			    BT_GATT_ITER_STOP) {
+				bt_gatt_unsubscribe(conn, params);
+			}
+		}
+		bt_conn_unref(conn);
+	}
+}
+
+static void gatt_subscription_remove(struct bt_conn *conn,
+				     struct bt_gatt_subscribe_params *prev,
+				     struct bt_gatt_subscribe_params *params)
+{
+	/* Remove subscription from the list*/
+	if (!prev) {
+		subscriptions = params->_next;
+	} else {
+		prev->_next = params->_next;
+	}
+
+	params->notify(conn, params, NULL, 0);
+}
+
+static void remove_subscribtions(struct bt_conn *conn)
+{
+	struct bt_gatt_subscribe_params *params, *prev;
+
+	/* Lookup existing subscriptions */
+	for (params = subscriptions, prev = NULL; params;
+	     prev = params, params = params->_next) {
+		if (bt_addr_le_cmp(&params->_peer, &conn->le.dst)) {
+			continue;
+		}
+
+		/* Remove subscription */
+		gatt_subscription_remove(conn, prev, params);
+	}
+}
+
+int bt_gatt_exchange_mtu(struct bt_conn *conn, bt_gatt_rsp_func_t func)
+{
+	return -EINVAL;
+}
+
+void on_nble_gattc_discover_rsp(const struct nble_gattc_discover_rsp *rsp,
+		const uint8_t *data, uint8_t data_len)
+{
+	size_t i;
+	uint8_t attr_count;
+	uint16_t last_handle;
+	int status = BT_GATT_ITER_STOP;
+	struct bt_gatt_discover_params *params;
+	struct bt_gatt_service svc_value;
+	struct bt_gatt_include inc_value;
+	struct bt_conn *conn = bt_conn_lookup_handle(rsp->conn_handle);
+
+	BT_ASSERT(conn);
+
+	params = rsp->user_data;
+
+	BT_DBG("disc_rsp: s=%d", rsp->status);
+
+	if (rsp->status)
+		goto complete;
+
+	if (rsp->type == BT_GATT_DISCOVER_PRIMARY) {
+		attr_count = (data_len / sizeof(struct nble_gattc_primary));
+	} else if (rsp->type == BT_GATT_DISCOVER_INCLUDE) {
+		attr_count = (data_len / sizeof(struct nble_gattc_included));
+	} else if (rsp->type == BT_GATT_DISCOVER_CHARACTERISTIC) {
+		attr_count = (data_len / sizeof(struct nble_gattc_characteristic));
+	} else if (rsp->type == BT_GATT_DISCOVER_DESCRIPTOR) {
+		attr_count = (data_len / sizeof(struct nble_gattc_descriptor));
+	} else
+		goto complete;
+	BT_DBG("disc_rsp: c=%d", attr_count);
+	last_handle = params->end_handle;
+	for (i = 0; i < attr_count; i++) {
+		struct bt_gatt_attr *attr = NULL;
+
+		if (rsp->type == BT_GATT_DISCOVER_PRIMARY) {
+    //BT_DBG("%s-%d", __FUNCTION__, __LINE__);
+			const struct nble_gattc_primary *gattr =
+					(void *)&data[i * sizeof(*gattr)];
+			if ((gattr->range.start_handle < params->start_handle) &&
+			    (gattr->range.end_handle > params->end_handle)) {
+				/*
+				 * Only the attributes with attribute handles
+				 * between and including the Starting
+				 * Handle and the Ending Handle is returned
+				 */
+				goto complete;
+			}
+			svc_value.end_handle = gattr->range.end_handle;
+			svc_value.uuid = (struct bt_uuid*)(&(gattr->uuid));//params->uuid;
+			attr = (&(struct bt_gatt_attr)BT_GATT_PRIMARY_SERVICE(&svc_value));
+			attr->handle = gattr->handle;
+			last_handle = svc_value.end_handle;
+		} else if (rsp->type == BT_GATT_DISCOVER_INCLUDE) {
+			const struct nble_gattc_included *gattr =
+					(void *)&data[i * sizeof(*gattr)];
+
+			inc_value.start_handle = gattr->range.start_handle;
+			inc_value.end_handle = gattr->range.end_handle;
+			/*
+			 * 4.5.1 If the service UUID is a 16-bit Bluetooth UUID
+			 *  it is also returned in the response.
+			 */
+			switch (gattr->uuid.uuid.type) {
+			case BT_UUID_TYPE_16:
+				inc_value.uuid = &gattr->uuid.uuid;
+				break;
+			case BT_UUID_TYPE_128:
+				/* Data is not available at this point */
+				break;
+			}
+			attr = (&(struct bt_gatt_attr)
+					BT_GATT_INCLUDE_SERVICE(&inc_value));
+			attr->handle = gattr->handle;
+			last_handle = gattr->handle;
+		} else if (rsp->type == BT_GATT_DISCOVER_CHARACTERISTIC) {
+			const struct nble_gattc_characteristic *gattr =
+					(void *)&data[i * sizeof(*gattr)];
+			attr = (&(struct bt_gatt_attr)
+				BT_GATT_CHARACTERISTIC(&gattr->uuid.uuid,
+				gattr->prop));
+			attr->handle = gattr->handle;
+			last_handle = gattr->handle;
+			/* Skip if UUID is set but doesn't match */
+			if (params->uuid && bt_uuid_cmp(&gattr->uuid.uuid, params->uuid)) {
+				continue;
+			}
+		} else if (rsp->type == BT_GATT_DISCOVER_DESCRIPTOR) {
+			const struct nble_gattc_descriptor *gattr =
+					(void *)&data[i * sizeof(*gattr)];
+
+			attr = (&(struct bt_gatt_attr)
+				BT_GATT_DESCRIPTOR(&gattr->uuid.uuid, 0, NULL, NULL, NULL));
+			attr->handle = gattr->handle;
+			last_handle = gattr->handle;
+		} else {
+			/* Error case */
+			goto complete;
+		}
+		status = params->func(conn, attr, params);
+		if (status == BT_GATT_ITER_STOP) {
+			/* Not required to call complete */
+			goto done;
+		}
+	}
+	if (last_handle < UINT16_MAX) {
+		last_handle++;
+	}
+	BT_DBG("disc_rsp: l=%d", last_handle);
+	params->start_handle = last_handle;
+	if (params->start_handle < params->end_handle) {
+		if (!bt_gatt_discover(conn, params))
+			goto not_done;
+	}
+
+complete:
+	/* Indicate that there are no more attributes found */
+	params->func(conn, NULL, params);
+
+done:
+	BT_DBG("disc_rsp: done");
+
+not_done:
+	bt_conn_unref(conn);
+}
+
+int bt_gatt_discover(struct bt_conn *conn,
+		     struct bt_gatt_discover_params *params)
+{
+	struct nble_discover_params discover_params;
+
+	if (!conn || !params || !params->func || !params->start_handle ||
+	    !params->end_handle || params->start_handle > params->end_handle) {
+		return -EINVAL;
+	}
+
+	if (conn->state != BT_CONN_CONNECTED) {
+		return -EINVAL;
+	}
+
+	BT_DBG("disc: %d", params->start_handle);
+
+	memset(&discover_params, 0, sizeof(discover_params));
+
+	switch (params->type) {
+	case BT_GATT_DISCOVER_PRIMARY:
+	case BT_GATT_DISCOVER_CHARACTERISTIC:
+		if (params->uuid) {
+			/* Always copy a full 128 bit UUID */
+			discover_params.uuid = *BT_UUID_128(params->uuid);
+			discover_params.flags = DISCOVER_FLAGS_UUID_PRESENT;
+		}
+		break;
+
+	case BT_GATT_DISCOVER_INCLUDE:
+	case BT_GATT_DISCOVER_DESCRIPTOR:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	discover_params.conn_handle = conn->handle;
+	discover_params.type = params->type;
+	discover_params.handle_range.start_handle = params->start_handle;
+	discover_params.handle_range.end_handle = params->end_handle;
+
+	discover_params.user_data = params;
+
+	nble_gattc_discover_req(&discover_params);
+
+	return 0;
+}
+
+void on_nble_gattc_read_multiple_rsp(const struct ble_gattc_read_rsp *rsp,
+		uint8_t *pdu, uint8_t length)
+{
+	struct bt_gatt_read_params *params;
+	struct bt_conn *conn = bt_conn_lookup_handle(rsp->conn_handle);
+
+	BT_ASSERT(conn);
+
+	params = rsp->user_data;
+
+	BT_DBG("err 0x%02x", rsp->status);
+
+	if (rsp->status) {
+		params->func(conn, rsp->status, params, NULL, 0);
+		bt_conn_unref(conn);
+		return;
+	}
+
+	params->func(conn, 0, params, pdu, length);
+
+	/* mark read as complete since read multiple is single response */
+	params->func(conn, 0, params, NULL, 0);
+
+	bt_conn_unref(conn);
+}
+
+void on_nble_gattc_read_rsp(const struct ble_gattc_read_rsp *rsp,
+		uint8_t *pdu, uint8_t length)
+{
+	struct bt_gatt_read_params *params;
+	struct bt_conn *conn = bt_conn_lookup_handle(rsp->conn_handle);
+
+	BT_ASSERT(conn);
+
+	params = rsp->user_data;
+
+	if (rsp->status) {
+		params->func(conn, rsp->status, params, NULL, 0);
+		bt_conn_unref(conn);
+		return;
+	}
+
+	if (params->func(conn, 0, params, pdu, length) == BT_GATT_ITER_STOP) {
+		bt_conn_unref(conn);
+		return;
+	}
+
+	/*
+	 * Core Spec 4.2, Vol. 3, Part G, 4.8.1
+	 * If the Characteristic Value is greater than (ATT_MTU – 1) octets
+	 * in length, the Read Long Characteristic Value procedure may be used
+	 * if the rest of the Characteristic Value is required.
+	 */
+	if (length < BLE_GATT_MTU_SIZE - 1) {
+		params->func(conn, 0, params, NULL, 0);
+		bt_conn_unref(conn);
+		return;
+	}
+
+	params->single.offset += length;
+
+	/* Continue reading the attribute */
+	if (bt_gatt_read(conn, params)) {
+		params->func(conn, BT_ATT_ERR_UNLIKELY, params, NULL, 0);
+	}
+	bt_conn_unref(conn);
+}
+
+int bt_gatt_read(struct bt_conn *conn, struct bt_gatt_read_params *params)
+{
+
+	struct ble_gattc_read_params single_req;
+	struct ble_gattc_read_multiple_params mutiple_req;
+
+	if (!conn || conn->state != BT_CONN_CONNECTED || !params ||
+	    params->handle_count == 0 || !params->func) {
+		return -EINVAL;
+	}
+
+	single_req.conn_handle = conn->handle;
+
+	if (1 == params->handle_count) {
+		single_req.handle = params->single.handle;
+		single_req.offset = params->single.offset;
+		single_req.user_data = params;
+
+		nble_gattc_read_req(&single_req);
+	} else {
+		mutiple_req.conn_handle = conn->handle;
+		mutiple_req.user_data = params;
+
+		nble_gattc_read_multiple_req(&mutiple_req, params->handles, 2 * params->handle_count);
+	}
+	return 0;
+}
+
+static void on_write_no_rsp_complete(struct bt_conn *conn, uint8_t err,
+				     const void *data)
+{
+}
+
+static void on_write_complete(struct bt_conn *conn, uint8_t err,
+			      const struct bt_gatt_write_params *wr_params)
+{
+	bt_gatt_write_rsp_func_t func = wr_params->user_data[0];
+	const void *data = wr_params->user_data[1];
+
+	BT_ASSERT(func);
+	func(conn, err, data);
+}
+
+static int _bt_gatt_write(struct bt_conn *conn, uint16_t handle, bool with_resp,
+			  uint16_t offset, const void *data, uint16_t length,
+			  struct bt_gatt_write_params *wr_params)
+{
+	struct ble_gattc_write_params req;
+
+	req.conn_handle = conn->handle;
+	req.handle = handle;
+	req.offset = offset;
+	req.with_resp = with_resp;
+	req.wr_params = *wr_params;
+
+	nble_gattc_write_req(&req, data, length);
+
+	return 0;
+}
+
+void on_nble_gattc_write_rsp(const struct ble_gattc_write_rsp *rsp)
+{
+
+	struct bt_conn *conn = bt_conn_lookup_handle(rsp->conn_handle);
+
+	BT_ASSERT(conn);
+
+	if (rsp->wr_params.func) {
+		rsp->wr_params.func(conn, rsp->status, &rsp->wr_params);
+	}
+
+	bt_conn_unref(conn);
+}
+
+
+int bt_gatt_write_without_response(struct bt_conn *conn, uint16_t handle,
+				   const void *data, uint16_t length, bool sign)
+{
+	return bt_gatt_write(conn, handle, 0, data, length,
+			     on_write_no_rsp_complete);
+}
+
+int bt_gatt_write(struct bt_conn *conn, uint16_t handle, uint16_t offset,
+		  const void *data, uint16_t length,
+		  bt_gatt_write_rsp_func_t func)
+{
+	struct bt_gatt_write_params wr_params;
+
+	if (!conn || conn->state != BT_CONN_CONNECTED  || !handle || !func) {
+		return -EINVAL;
+	}
+
+	wr_params.func = on_write_complete;
+	wr_params.user_data[0] = func;
+	wr_params.user_data[1] = (void *)data;
+
+	return _bt_gatt_write(conn, handle,
+			      (func == on_write_no_rsp_complete)
+			      ? false : true,
+			      offset, data, length, &wr_params);
+}
+
+static void gatt_subscription_add(struct bt_conn *conn,
+				  struct bt_gatt_subscribe_params *params)
+{
+	bt_addr_le_copy(&params->_peer, &conn->le.dst);
+
+	/* Prepend subscription */
+	params->_next = subscriptions;
+	subscriptions = params;
+}
+
+static void att_write_ccc_rsp(struct bt_conn *conn, uint8_t err,
+			      const struct bt_gatt_write_params *wr_params)
+{
+	struct bt_gatt_subscribe_params *params = wr_params->user_data[0];
+
+	/* if write to CCC failed we remove subscription and notify app */
+	if (err) {
+		struct bt_gatt_subscribe_params *cur, *prev;
+
+		for (cur = subscriptions, prev = NULL; cur;
+		     prev = cur, cur = cur->_next) {
+
+			if (cur == params) {
+				gatt_subscription_remove(conn, prev, params);
+				break;
+			}
+		}
+	}
+}
+
+static int gatt_write_ccc(struct bt_conn *conn, uint16_t handle, uint16_t value,
+			  bt_att_func_t func,
+			  struct bt_gatt_subscribe_params *params)
+{
+	struct bt_gatt_write_params wr_params;
+
+	wr_params.func = func;
+	wr_params.user_data[0] = params;
+
+	BT_DBG("handle 0x%04x value 0x%04x", handle, value);
+
+	return _bt_gatt_write(conn, handle, true, 0, &value, sizeof(value),
+			     &wr_params);
+}
+
+int bt_gatt_subscribe(struct bt_conn *conn,
+		      struct bt_gatt_subscribe_params *params)
+{
+	struct bt_gatt_subscribe_params *tmp;
+	bool has_subscription = false;
+
+	if (!conn || conn->state != BT_CONN_CONNECTED) {
+		return -ENOTCONN;
+	}
+
+	if (!params || !params->notify ||
+	    !params->value || !params->ccc_handle) {
+		return -EINVAL;
+	}
+
+	/* Lookup existing subscriptions */
+	for (tmp = subscriptions; tmp; tmp = tmp->_next) {
+		/* Fail if entry already exists */
+		if (tmp == params) {
+			return -EALREADY;
+		}
+
+		/* Check if another subscription exists */
+		if (!bt_addr_le_cmp(&tmp->_peer, &conn->le.dst) &&
+		    tmp->value_handle == params->value_handle &&
+		    tmp->value >= params->value) {
+			has_subscription = true;
+		}
+	}
+
+	/* Skip write if already subscribed */
+	if (!has_subscription) {
+		int err;
+
+		err = gatt_write_ccc(conn, params->ccc_handle, params->value,
+				     att_write_ccc_rsp, params);
+		if (err) {
+			return err;
+		}
+	}
+
+	/*
+	 * Add subscription before write complete as some implementation were
+	 * reported to send notification before reply to CCC write.
+	 */
+	gatt_subscription_add(conn, params);
+
+	return 0;
+}
+
+int bt_gatt_unsubscribe(struct bt_conn *conn,
+			struct bt_gatt_subscribe_params *params)
+{
+	struct bt_gatt_subscribe_params *tmp;
+	bool has_subscription = false, found = false;
+
+	if (!conn || conn->state != BT_CONN_CONNECTED) {
+		return -ENOTCONN;
+	}
+
+	if (!params) {
+		return -EINVAL;
+	}
+
+	/* Check head */
+	if (subscriptions == params) {
+		subscriptions = params->_next;
+		found = true;
+	}
+
+	/* Lookup existing subscriptions */
+	for (tmp = subscriptions; tmp; tmp = tmp->_next) {
+		/* Remove subscription */
+		if (tmp->_next == params) {
+			tmp->_next = params->_next;
+			found = true;
+		}
+
+		/* Check if there still remains any other subscription */
+		if (!bt_addr_le_cmp(&tmp->_peer, &conn->le.dst) &&
+		    tmp->value_handle == params->value_handle) {
+			has_subscription = true;
+		}
+	}
+
+	if (!found) {
+		return -EINVAL;
+	}
+
+	if (has_subscription) {
+		return 0;
+	}
+
+	params->value = 0;
+
+	return gatt_write_ccc(conn, params->ccc_handle, params->value, NULL,
+			      NULL);
+}
+
+static void add_subscriptions(struct bt_conn *conn)
+{
+	struct bt_gatt_subscribe_params *params, *prev;
+
+	/* Lookup existing subscriptions */
+	for (params = subscriptions, prev = NULL; params;
+	     prev = params, params = params->_next) {
+		if (bt_addr_le_cmp(&params->_peer, &conn->le.dst)) {
+			continue;
+		}
+
+		/* Force write to CCC to workaround devices that don't track
+		 * it properly.
+		 */
+		gatt_write_ccc(conn, params->ccc_handle, params->value,
+			       att_write_ccc_rsp, params);
+	}
+}
+#else
+
+void on_nble_gattc_discover_rsp(const struct nble_gattc_discover_rsp *rsp,
+		const uint8_t *data, uint8_t data_len)
+{
+
+}
+void on_nble_gattc_write_rsp(const struct ble_gattc_write_rsp *rsp)
+{
+}
+
+void on_nble_gattc_value_evt(const struct ble_gattc_value_evt *evt,
+		uint8_t *buf, uint8_t buflen)
+{
+}
+
+void on_nble_gattc_read_rsp(const struct ble_gattc_read_rsp *rsp,
+		uint8_t *data, uint8_t data_len)
+{
+}
+
+void on_nble_gattc_read_multiple_rsp(const struct ble_gattc_read_rsp *rsp,
+		uint8_t *data, uint8_t data_len)
+{
+}
+
+#endif
+
+void bt_gatt_connected(struct bt_conn *conn)
+{
+	BT_DBG("conn %p", conn);
+	bt_gatt_foreach_attr(0x0001, 0xffff, connected_cb, conn);
+#if defined(CONFIG_BLUETOOTH_GATT_CLIENT)
+	add_subscriptions(conn);
+#endif /* CONFIG_BLUETOOTH_GATT_CLIENT */
+}
+
+void bt_gatt_disconnected(struct bt_conn *conn)
+{
+	BT_DBG("conn %p", conn);
+	bt_gatt_foreach_attr(0x0001, 0xffff, disconnected_cb, conn);
+
+#if defined(CONFIG_BLUETOOTH_GATT_CLIENT)
+	/* If bonded don't remove subscriptions */
+	if (is_bonded(&conn->le.dst)) {
+		return;
+	}
+
+	remove_subscribtions(conn);
+#endif /* CONFIG_BLUETOOTH_GATT_CLIENT */
+}
diff --git a/system/libarc32_arduino101/framework/src/services/ble/hci_core.h b/system/libarc32_arduino101/framework/src/services/ble/hci_core.h
new file mode 100644
index 00000000..dc7374b0
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/hci_core.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+/* State tracking for the local Bluetooth controller */
+struct bt_dev {
+	atomic_t flags[1];
+};
+extern struct bt_dev bt_dev;
+
+#if defined(CONFIG_BLUETOOTH_SMP) || defined(CONFIG_BLUETOOTH_BREDR)
+extern const struct bt_conn_auth_cb *bt_auth;
+#endif /* CONFIG_BLUETOOTH_SMP || CONFIG_BLUETOOTH_BREDR */
+
+static inline bool bt_addr_le_is_rpa(const bt_addr_le_t *addr)
+{
+	if (addr->type != BT_ADDR_LE_RANDOM)
+		return false;
+
+	if ((addr->val[5] & 0xc0) == 0x40)
+	       return true;
+
+	return false;
+}
+
+static inline bool bt_addr_le_is_identity(const bt_addr_le_t *addr)
+{
+	if (addr->type == BT_ADDR_LE_PUBLIC)
+		return true;
+
+	/* Check for Random Static address type */
+	if ((addr->val[5] & 0xc0) == 0xc0)
+		return true;
+
+	return false;
+}
+
+static inline bool bt_le_conn_params_valid(uint16_t min, uint16_t max,
+					uint16_t latency, uint16_t timeout)
+{
+	if (min > max || min < 6 || max > 3200) {
+		return false;
+	}
+
+	/* Limits according to BT Core spec 4.2 [Vol 2, Part E, 7.8.12] */
+	if (timeout < 10 || timeout > 3200) {
+		return false;
+	}
+
+	/* Limits according to BT Core spec 4.2 [Vol 6, Part B, 4.5.1] */
+	if (latency > 499 || ((latency + 1) * max) > (timeout * 4)) {
+		return false;
+	}
+
+	return true;
+}
+
+
diff --git a/system/libarc32_arduino101/framework/src/services/ble/l2cap.c b/system/libarc32_arduino101/framework/src/services/ble/l2cap.c
new file mode 100644
index 00000000..488cfd0e
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/l2cap.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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 <errno.h>
+#include <atomic.h>
+
+#include <bluetooth/gatt.h>
+
+#include "conn_internal.h"
+#include "gatt_internal.h"
+#include "smp.h"
+
+/* #define BT_GATT_DEBUG 1 */
+
+extern void on_nble_curie_log(char *fmt, ...);
+extern void __assert_fail(void);
+#ifdef BT_GATT_DEBUG
+#define BT_DBG(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#else
+#define BT_DBG(fmt, ...) do {} while (0)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#endif
+
+void bt_l2cap_connected(struct bt_conn *conn)
+{
+	/* TODO Add support for fixed channels on BR/EDR transport */
+	if (conn->type != BT_CONN_TYPE_LE) {
+		return;
+	}
+#if defined(CONFIG_BLUETOOTH_SMP)
+	bt_smp_connected(conn);
+#endif
+	bt_gatt_connected(conn);
+}
+
+void bt_l2cap_disconnected(struct bt_conn *conn)
+{
+#if defined(CONFIG_BLUETOOTH_SMP)
+	bt_smp_disconnected(conn);
+#endif
+	bt_gatt_disconnected(conn);
+}
diff --git a/system/libarc32_arduino101/framework/src/services/ble/l2cap_internal.h b/system/libarc32_arduino101/framework/src/services/ble/l2cap_internal.h
new file mode 100644
index 00000000..2f593fa5
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/l2cap_internal.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef L2CAP_INTERNAL_H_
+#define L2CAP_INTERNAL_H_
+
+/* Notify L2CAP channels of a new connection */
+void bt_l2cap_connected(struct bt_conn *conn);
+
+/* Notify L2CAP channels of a disconnect event */
+void bt_l2cap_disconnected(struct bt_conn *conn);
+
+
+#endif /* L2CAP_INTERNAL_H_ */
diff --git a/system/libarc32_arduino101/framework/src/services/ble/smp.h b/system/libarc32_arduino101/framework/src/services/ble/smp.h
new file mode 100644
index 00000000..4f52c002
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/smp.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+#define BT_SMP_ERR_PASSKEY_ENTRY_FAILED		0x01
+#define BT_SMP_ERR_OOB_NOT_AVAIL		0x02
+#define BT_SMP_ERR_AUTH_REQUIREMENTS		0x03
+#define BT_SMP_ERR_CONFIRM_FAILED		0x04
+#define BT_SMP_ERR_PAIRING_NOTSUPP		0x05
+#define BT_SMP_ERR_ENC_KEY_SIZE			0x06
+#define BT_SMP_ERR_CMD_NOTSUPP			0x07
+#define BT_SMP_ERR_UNSPECIFIED			0x08
+#define BT_SMP_ERR_REPEATED_ATTEMPTS		0x09
+#define BT_SMP_ERR_INVALID_PARAMS		0x0a
+#define BT_SMP_ERR_DHKEY_CHECK_FAILED		0x0b
+#define BT_SMP_ERR_NUMERIC_COMP_FAILED		0x0c
+#define BT_SMP_ERR_BREDR_PAIRING_IN_PROGRESS	0x0d
+#define BT_SMP_ERR_CROSS_TRANSP_NOT_ALLOWED	0x0e
+
+#define BT_SMP_IO_DISPLAY_ONLY			0x00
+#define BT_SMP_IO_DISPLAY_YESNO			0x01
+#define BT_SMP_IO_KEYBOARD_ONLY			0x02
+#define BT_SMP_IO_NO_INPUT_OUTPUT		0x03
+#define BT_SMP_IO_KEYBOARD_DISPLAY		0x04
+
+#define BT_SMP_OOB_NOT_PRESENT			0x00
+#define BT_SMP_OOB_PRESENT			0x01
+
+#define BT_SMP_MIN_ENC_KEY_SIZE			7
+#define BT_SMP_MAX_ENC_KEY_SIZE			16
+
+#define BT_SMP_DIST_ENC_KEY			0x01
+#define BT_SMP_DIST_ID_KEY			0x02
+#define BT_SMP_DIST_SIGN			0x04
+#define BT_SMP_DIST_LINK_KEY			0x08
+
+#define BT_SMP_DIST_MASK			0x0f
+
+#define BT_SMP_AUTH_NONE			0x00
+#define BT_SMP_AUTH_BONDING			0x01
+#define BT_SMP_AUTH_MITM			0x04
+#define BT_SMP_AUTH_SC				0x08
+
+#if 0
+bool bt_smp_irk_matches(const uint8_t irk[16], const bt_addr_t *addr);
+#endif
+int bt_smp_send_pairing_req(struct bt_conn *conn);
+int bt_smp_send_security_req(struct bt_conn *conn);
+#if 0
+void bt_smp_update_keys(struct bt_conn *conn);
+bool bt_smp_get_tk(struct bt_conn *conn, uint8_t *tk);
+
+void bt_smp_dhkey_ready(const uint8_t *dhkey);
+void bt_smp_pkey_ready(void);
+#endif
+
+int bt_smp_init(void);
+
+int bt_smp_auth_passkey_entry(struct bt_conn *conn, unsigned int passkey);
+int bt_smp_auth_passkey_confirm(struct bt_conn *conn, bool match);
+int bt_smp_auth_cancel(struct bt_conn *conn);
+
+int bt_smp_remove_info(const bt_addr_le_t *addr);
+
+#ifdef CONFIG_BLUETOOTH_SMP
+void bt_smp_connected(struct bt_conn *conn);
+void bt_smp_disconnected(struct bt_conn *conn);
+#endif
+
+#if 0
+/** brief Verify signed message
+ *
+ *  @param conn Bluetooth connection
+ *  @param buf received packet buffer with message and signature
+ *
+ *  @return 0 in success, error code otherwise
+ */
+int bt_smp_sign_verify(struct bt_conn *conn, struct net_buf *buf);
+
+/** brief Sign message
+ *
+ *  @param conn Bluetooth connection
+ *  @param buf message buffer
+ *
+ *  @return 0 in success, error code otherwise
+ */
+int bt_smp_sign(struct bt_conn *conn, struct net_buf *buf);
+#endif
diff --git a/system/libarc32_arduino101/framework/src/services/ble/smp_null.c b/system/libarc32_arduino101/framework/src/services/ble/smp_null.c
new file mode 100644
index 00000000..cdc3a4d5
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/smp_null.c
@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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 <errno.h>
+
+#include <bluetooth/hci.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/conn.h>
+
+#include "smp.h"
+
+/* nble internal APIs */
+#include "gap_internal.h"
+
+/* #define BT_GATT_DEBUG 1 */
+
+extern void on_nble_curie_log(char *fmt, ...);
+extern void __assert_fail(void);
+#ifdef BT_GATT_DEBUG
+#define BT_DBG(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#else
+#define BT_DBG(fmt, ...) do {} while (0)
+#define BT_ERR(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_WARN(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_INFO(fmt, ...) on_nble_curie_log(fmt, ##__VA_ARGS__)
+#define BT_ASSERT(cond) ((cond) ? (void)0 : __assert_fail())
+#endif
+
+/**
+ * Compile time IO capabilities and OOB support settings for nble.
+ *
+ * This compile options must match the application registered callbacks
+ * (bt_conn_auth_cb_register)
+ */
+#define NBLE_SMP_IO_CAPS BT_SMP_IO_NO_INPUT_OUTPUT
+#define NBLE_SMP_AUTH_OPTIONS 0
+#define NBLE_SMP_OBB_PRESENT BT_SMP_OOB_NOT_PRESENT
+
+#if NOT_USED_FOR_NOW
+int bt_smp_sign_verify(struct bt_conn *conn, struct net_buf *buf)
+{
+	return -ENOTSUP;
+}
+
+int bt_smp_sign(struct bt_conn *conn, struct net_buf *buf)
+{
+	return -ENOTSUP;
+}
+#endif
+
+void on_nble_gap_sm_bond_info_rsp(const struct nble_gap_sm_bond_info_rsp *rsp,
+		const bt_addr_le_t *peer_addr, uint16_t len)
+{
+	/* stub */
+}
+
+void on_nble_gap_sm_passkey_req_evt(const struct nble_gap_sm_passkey_req_evt * p_evt)
+{
+	/* stub */
+}
+
+void on_nble_gap_sm_passkey_display_evt(
+		const struct nble_gap_sm_passkey_disp_evt *p_evt)
+{
+	/* stub */
+}
+
+void on_nble_gap_sm_status_evt(const struct nble_gap_sm_status_evt *evt)
+{
+	/* stub */
+	BT_INFO("nble_gap_sm_status_evt: %d, gap_status: %d",
+		evt->evt_type, evt->status);
+}
+
+int bt_smp_init(void)
+{
+	struct nble_gap_sm_config_params params = {
+			.options = NBLE_SMP_AUTH_OPTIONS,
+			.io_caps = NBLE_SMP_IO_CAPS,
+			.key_size = BT_SMP_MAX_ENC_KEY_SIZE,
+			.oob_present = NBLE_SMP_OBB_PRESENT,
+	};
+
+	nble_gap_sm_config_req(&params);
+
+	return 0;
+}
+
+void on_nble_gap_sm_config_rsp(struct nble_gap_sm_config_rsp *p_params)
+{
+	if (p_params->status) {
+		BT_ERR("sm_config failed: %d", p_params->status);
+	}
+}
+
+void on_nble_gap_sm_common_rsp(const struct nble_gap_sm_response *rsp)
+{
+	if (rsp->status) {
+		BT_WARN("gap sm request failed: %d", rsp->status);
+	}
+}
diff --git a/system/libarc32_arduino101/framework/src/services/ble/uuid.c b/system/libarc32_arduino101/framework/src/services/ble/uuid.c
new file mode 100644
index 00000000..71748f50
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble/uuid.c
@@ -0,0 +1,137 @@
+/* uuid.c - Bluetooth UUID handling */
+
+/*
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * 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 <nanokernel.h>
+//#include <toolchain.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <string.h>
+#include <errno.h>
+#include <misc/byteorder.h>
+
+#include <bluetooth/uuid.h>
+
+#if defined(CONFIG_BLUETOOTH_DEBUG)
+#include <stdio.h>
+#endif /* CONFIG_BLUETOOTH_DEBUG */
+
+#define UUID_16_BASE_OFFSET 12
+
+/* TODO: Decide whether to continue using BLE format or switch to RFC 4122 */
+
+/* Base UUID : 0000[0000]-0000-1000-8000-00805F9B34FB ->
+ * { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
+ *   0x00, 0x10, 0x00, 0x00, [0x00, 0x00], 0x00, 0x00 }
+ * 0x2800    : 0000[2800]-0000-1000-8000-00805F9B34FB ->
+ * { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
+ *   0x00, 0x10, 0x00, 0x00, [0x00, 0x28], 0x00, 0x00 }
+ *  little endian 0x2800 : [00 28] -> no swapping required
+ *  big endian 0x2800    : [28 00] -> swapping required
+ */
+static const struct bt_uuid_128 uuid128_base = {
+	.uuid.type = BT_UUID_TYPE_128,
+	.val = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
+		 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+};
+
+static inline void u16_to_uuid128(void *dst, uint16_t u16)
+{
+#if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+	u16 = bswap_16(u16);
+#endif
+	memcpy(dst, &u16, 2);
+}
+
+static void uuid_to_uuid128(const struct bt_uuid *src, struct bt_uuid_128 *dst)
+{
+	switch (src->type) {
+	case BT_UUID_TYPE_16:
+		//*dst = uuid128_base;
+		memcpy(dst, &uuid128_base, sizeof(*dst));
+		//u16_to_uuid128(&dst->val[UUID_16_BASE_OFFSET],
+		//	       BT_UUID_16(src)->val);
+		memcpy(&dst->val[UUID_16_BASE_OFFSET], &BT_UUID_16(src)->val, 2);
+		return;
+	case BT_UUID_TYPE_128:
+		memcpy(dst, src, sizeof(*dst));
+		return;
+	}
+}
+
+static int uuid128_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2)
+{
+	struct bt_uuid_128 uuid1, uuid2;
+
+	uuid_to_uuid128(u1, &uuid1);
+	uuid_to_uuid128(u2, &uuid2);
+
+	return memcmp(uuid1.val, uuid2.val, 16);
+}
+
+int bt_uuid_cmp(const struct bt_uuid *u1, const struct bt_uuid *u2)
+{
+	/* Convert to 128 bit if types don't match */
+	if (u1->type != u2->type)
+		return uuid128_cmp(u1, u2);
+
+	switch (u1->type) {
+	case BT_UUID_TYPE_16:
+		return memcmp(&BT_UUID_16(u1)->val, &BT_UUID_16(u2)->val, 2);//(int)BT_UUID_16(u1)->val - (int)BT_UUID_16(u2)->val;
+	case BT_UUID_TYPE_128:
+		return memcmp(BT_UUID_128(u1)->val, BT_UUID_128(u2)->val, 16);
+	}
+
+	return -EINVAL;
+}
+
+#if defined(CONFIG_BLUETOOTH_DEBUG)
+void bt_uuid_to_str(const struct bt_uuid *uuid, char *str, size_t len)
+{
+	uint32_t tmp1, tmp5;
+	uint16_t tmp0, tmp2, tmp3, tmp4;
+
+	switch (uuid->type) {
+	case BT_UUID_TYPE_16:
+		snprintf(str, len, "%.4x", BT_UUID_16(uuid)->val);
+		break;
+	case BT_UUID_TYPE_128:
+		memcpy(&tmp0, &BT_UUID_128(uuid)->val[0], sizeof(tmp0));
+		memcpy(&tmp1, &BT_UUID_128(uuid)->val[2], sizeof(tmp1));
+		memcpy(&tmp2, &BT_UUID_128(uuid)->val[6], sizeof(tmp2));
+		memcpy(&tmp3, &BT_UUID_128(uuid)->val[8], sizeof(tmp3));
+		memcpy(&tmp4, &BT_UUID_128(uuid)->val[10], sizeof(tmp4));
+		memcpy(&tmp5, &BT_UUID_128(uuid)->val[12], sizeof(tmp5));
+
+		snprintf(str, len, "%.8x-%.4x-%.4x-%.4x-%.8x%.4x",
+			 tmp5, tmp4, tmp3, tmp2, tmp1, tmp0);
+		break;
+	default:
+		memset(str, 0, len);
+		return;
+	}
+}
+
+const char *bt_uuid_str(const struct bt_uuid *uuid)
+{
+	static char str[37];
+
+	bt_uuid_to_str(uuid, str, sizeof(str));
+
+	return str;
+}
+#endif /* CONFIG_BLUETOOTH_DEBUG */
diff --git a/system/libarc32_arduino101/framework/include/services/ble/ble_service_msg.h b/system/libarc32_arduino101/framework/src/services/ble_service/ble_protocol.h
similarity index 70%
rename from system/libarc32_arduino101/framework/include/services/ble/ble_service_msg.h
rename to system/libarc32_arduino101/framework/src/services/ble_service/ble_protocol.h
index 6893731a..51109f9d 100644
--- a/system/libarc32_arduino101/framework/include/services/ble/ble_service_msg.h
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/ble_protocol.h
@@ -28,23 +28,30 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef __BLE_SERVICE_MSG_H__
-#define __BLE_SERVICE_MSG_H__
+#ifndef __BLE_PROTOCOL_H__
+#define __BLE_PROTOCOL_H__
 
-#include "services/services_ids.h"
-
-/*
- * CFW Message ID base definitions for BLE services.
+/**
+ *  @defgroup ble_protocol BLE protocol definitions
+ *
+ * BT Spec definitions.
+ * @ingroup ble_service
+ * @{
+ *
+ * Bluetooth SIG defined macros and enum extracted from Bluetooth Spec 4.2
  */
+#define BLE_MAX_DEVICE_NAME  20 /**< Max BLE device name length 20 + NULL, spec size: 248 */
+#define BLE_MAX_ADV_SIZE     31
+
+#define BLE_GATT_MTU_SIZE 23 /**< Default MTU size */
+
+/** Manufacturer IDs */
+#define INTEL_MANUFACTURER 0x0002
 
-/* BLE Service Message ID definitions. */
-#define MSG_ID_BLE_BASE                     BLE_SERVICE_MSG_BASE
-#define MSG_ID_BLE_RSP                      (BLE_SERVICE_MSG_BASE + 0x40)
-#define MSG_ID_BLE_EVT                      (BLE_SERVICE_MSG_BASE + 0x80)
+/* HCI status (error) codes as per BT spec */
+#define HCI_REMOTE_DEV_TERMINATION_DUE_TO_POWER_OFF     0x15
+#define HCI_LOCAL_HOST_TERMINATED_CONNECTION            0x16
 
-/* BLE Core Service Message ID base (GAP/GATT) */
-#define MSG_ID_BLE_GAP_BASE                 BLE_SERVICE_GAP_MSG_BASE
-#define MSG_ID_BLE_GAP_RSP                  (BLE_SERVICE_GAP_MSG_BASE + 0x40)
-#define MSG_ID_BLE_GAP_EVT                  (BLE_SERVICE_GAP_MSG_BASE + 0x80)
+/** @} */
 
 #endif
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/ble_service.c b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service.c
new file mode 100644
index 00000000..07231cb1
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (c) 2015, Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "ble_service.h"
+#include "os/os.h"
+
+//#include "util/assert.h"
+#include <errno.h>
+//#include <atomic.h>
+#include "cfw/cfw_service.h"
+#include "ble_protocol.h"
+#include "ble_service_int.h"
+#include "ble_service_internal.h"
+#include "infra/log.h"
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/gatt.h>
+#include <bluetooth/conn.h>
+#include <bluetooth/hci.h>
+#include "gap_internal.h"
+
+#include "nble_driver.h"
+
+#include "rpc.h"
+
+#include "util/misc.h"
+
+struct _ble_service_cb _ble_cb = { 0 };
+volatile uint8_t ble_inited = false;
+
+
+static void ble_client_connected(conn_handle_t *instance);
+static void ble_client_disconnected(conn_handle_t *instance);
+
+#if defined(CONFIG_BLUETOOTH_SMP)
+static const struct bt_conn_auth_cb auth_callbacks;
+#endif
+
+static service_t ble_service = {
+	.service_id = BLE_SERVICE_ID,
+	.client_connected = ble_client_connected,
+	.client_disconnected = ble_client_disconnected,
+};
+
+static void ble_is_not_enabled_rsp(struct cfw_message *msg, int status)
+{
+	struct ble_enable_rsp *resp =
+	    (struct ble_enable_rsp *)cfw_alloc_rsp_msg(msg,
+			    /* translate msg from req to rsp */
+						(CFW_MESSAGE_ID(msg) ^ MSG_ID_BLE_SERVICE_BASE)
+						| MSG_ID_BLE_SERVICE_RSP,
+						sizeof(*resp));
+	resp->status = status;
+	cfw_send_message(resp);
+}
+
+#ifdef CONFIG_TCMD_BLE_DEBUG
+static void handle_msg_id_ble_dbg(struct cfw_message *msg)
+{
+	struct nble_debug_params params;
+	struct ble_dbg_req_rsp *resp = (void *)
+		cfw_alloc_rsp_msg(msg, MSG_ID_BLE_DBG_RSP, sizeof(*resp));
+	struct ble_dbg_req_rsp *req = (struct ble_dbg_req_rsp *) msg;
+
+	params.u0 = req->u0;
+	params.u1 = req->u1;
+
+	nble_gap_dbg_req(&params, resp);
+}
+#endif /* CONFIG_TCMD_BLE_DEBUG */
+
+void on_nble_gap_dbg_rsp(const struct nble_debug_resp *params)
+{
+#ifdef CONFIG_TCMD_BLE_DEBUG
+	struct ble_dbg_req_rsp *resp = params->user_data;
+	if (!resp)
+		return;
+	resp->u0 = params->u0;
+	resp->u1 = params->u1;
+	cfw_send_message(resp);
+#endif /* CONFIG_TCMD_BLE_DEBUG */
+}
+
+
+static void handle_msg_id_ble_rpc_callin(struct message *msg, void *priv)
+{
+	struct ble_rpc_callin *rpc = container_of(msg, struct ble_rpc_callin, msg);
+	/* handle incoming message */
+    //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+	rpc_deserialize(rpc->p_data, rpc->len);
+	bfree(rpc->p_data);
+	message_free(msg);
+    //pr_debug(LOG_MODULE_BLE, "%s-%d", __FUNCTION__, __LINE__);
+}
+
+static void ble_set_bda_cb(int status, void *user_data)
+{
+	struct ble_enable_req *req = user_data;
+
+	if (!req)
+		return;
+
+	struct ble_enable_rsp *resp = (void *)cfw_alloc_rsp_msg(&req->header,
+			    MSG_ID_BLE_ENABLE_RSP, sizeof(*resp));
+	resp->status = status;
+
+	if (status == 0) {
+		resp->enable = 1;
+
+		nble_gap_read_bda_req(resp);
+	} else {
+		/* error case */
+		resp->enable = 0;
+		cfw_send_message(resp);
+	}
+	bfree(req);
+}
+
+
+void on_nble_gap_read_bda_rsp(const struct nble_service_read_bda_response *params)
+{
+	struct cfw_message *rsp = params->user_data;
+
+	if (rsp) {
+		struct ble_enable_rsp *r = container_of(rsp, struct ble_enable_rsp, header);
+		r->bd_addr = params->bd;
+		cfw_send_message(rsp);
+	}
+}
+
+static void handle_ble_enable(struct ble_enable_req *req,
+		struct _ble_service_cb *p_cb)
+{
+	pr_info(LOG_MODULE_BLE, "ble_enable: state %d", p_cb->ble_state);
+
+	p_cb->ble_state = BLE_ST_ENABLED;
+
+	if (req->bda_present) {
+		struct nble_set_bda_params params;
+
+		params.cb = ble_set_bda_cb;
+		params.user_data = req;
+		params.bda = req->bda;
+
+		nble_set_bda_req(&params);
+	} else {
+		ble_set_bda_cb(0, req);
+	}
+}
+
+void on_nble_set_bda_rsp(const struct nble_set_bda_rsp *params)
+{
+	if (params->cb) {
+		params->cb(params->status, params->user_data);
+	}
+}
+
+static void handle_ble_disable(struct ble_enable_req *req, struct _ble_service_cb *p_cb)
+{
+	struct ble_enable_rsp *resp;
+
+	pr_debug(LOG_MODULE_BLE, "ble_disable");
+	p_cb->ble_state = BLE_ST_DISABLED;
+
+	bt_le_adv_stop();
+
+	resp = (void *)cfw_alloc_rsp_msg(&req->header,
+				MSG_ID_BLE_ENABLE_RSP,
+				sizeof(*resp));
+	cfw_send_message(resp);  // Sid. KW warning ack.
+
+}
+
+static void ble_service_message_handler(struct cfw_message *msg, void *param)
+{
+	bool free_msg = true;
+	struct _ble_service_cb *p_cb = param;
+	uint16_t msg_id = CFW_MESSAGE_ID(msg);
+
+	if (p_cb->ble_state < BLE_ST_ENABLED &&
+		msg_id != MSG_ID_BLE_ENABLE_REQ) {
+		ble_is_not_enabled_rsp(msg, -ENODEV);
+		goto out;
+	}
+
+	switch (msg_id) {
+	case MSG_ID_BLE_ENABLE_REQ: {
+		struct ble_enable_req *req =
+			container_of(msg, struct ble_enable_req, header);
+		if (p_cb->ble_state) {
+			if (req->enable) {
+				handle_ble_enable(req, p_cb);
+				free_msg = false;
+			} else
+				handle_ble_disable(req, p_cb);
+		} else {
+			pr_debug(LOG_MODULE_BLE, "ble_hdl_msg: core service not opened!");
+			/* core service is not yet up */
+			struct ble_enable_rsp *resp = (void *)cfw_alloc_rsp_msg(msg,
+				MSG_ID_BLE_ENABLE_RSP, sizeof(*resp));
+			resp->status = -EINPROGRESS;
+			resp->enable = 0;
+			cfw_send_message(resp);  // Sid. KW warning ack.
+		}
+	}
+		break;
+#ifdef CONFIG_TCMD_BLE_DEBUG
+	case MSG_ID_BLE_DBG_REQ:
+		handle_msg_id_ble_dbg(msg);
+		break;
+#endif
+	default:
+		pr_warning(LOG_MODULE_BLE, "unsupported %d", msg_id);
+		break;
+	}
+out:
+	if (free_msg)
+		cfw_msg_free(msg);
+}
+
+static void ble_client_connected(conn_handle_t *instance)
+{
+	if (_ble_cb.ble_state == BLE_ST_NOT_READY)
+		pr_warning(LOG_MODULE_BLE, "BLE_CORE service is not registered");
+}
+
+static void ble_client_disconnected(conn_handle_t *instance)
+{
+}
+
+void ble_bt_rdy(int err)
+{
+	_ble_cb.ble_state = BLE_ST_DISABLED;
+    ble_inited = true;
+
+	/* register BLE service */
+	if (cfw_register_service(_ble_cb.queue, &ble_service,
+			ble_service_message_handler, &_ble_cb) == -1) {
+		panic(0xb1eb1e);
+	}
+}
+
+void ble_cfw_service_init(int service_id, T_QUEUE queue)
+{
+	_ble_cb.queue = queue;
+	_ble_cb.ble_state = BLE_ST_NOT_READY;
+
+#ifdef CONFIG_IPC_UART_NS16550
+	nble_driver_configure(queue, handle_msg_id_ble_rpc_callin);
+#endif
+
+    ble_inited = false;
+
+    bt_enable(ble_bt_rdy);
+    do{}
+    while (ble_inited == false);
+}
+
+void nble_log(const struct nble_log_s *param, char *buf, uint8_t buflen)
+{
+	pr_info(LOG_MODULE_BLE, buf, param->param0, param->param1, param->param2, param->param3);
+}
+
+void on_nble_common_rsp(const struct nble_response *params)
+{
+	struct ble_rsp *resp = params->user_data;
+
+	if (!resp)
+		return;
+	resp->status = params->status;
+	cfw_send_message(resp);
+}
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_api.c b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_api.c
new file mode 100644
index 00000000..e70b15c8
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_api.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2015, Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "ble_service.h"
+
+#include <errno.h>
+#include <string.h>
+//#include <atomic.h>
+//#include "util/assert.h"
+
+#include "ble_service_int.h"
+#include "ble_service_internal.h"
+#include "cfw/cfw_service.h"
+#include "cfw/cfw_client.h"
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+
+#include "ble_protocol.h"
+#include "ble_service_utils.h"
+
+int ble_service_enable(cfw_service_conn_t * p_service_conn, uint8_t enable,
+			const struct ble_enable_config * p_config,
+			void *p_priv)
+{
+	struct ble_enable_req * msg =
+			(void *) cfw_alloc_message_for_service(p_service_conn,
+				    MSG_ID_BLE_ENABLE_REQ,
+				    sizeof(*msg), p_priv);
+	msg->central_conn_params = p_config->central_conn_params;
+	msg->enable = enable;
+
+	if (p_config->p_bda) {
+		msg->bda_present = 1;
+		msg->bda = *p_config->p_bda;
+	}
+
+	return cfw_send_message(msg);
+}
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_int.h b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_int.h
new file mode 100644
index 00000000..857270c7
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_int.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2015, Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef BLE_SERVICE_INT_H_
+#define BLE_SERVICE_INT_H_
+
+#include <stdint.h>
+
+/* For cfw_service_conn_t, cfw_client_t, T_QUEUE */
+#include "cfw/cfw.h"
+
+/* Forward declarations */
+struct ble_init_svc_req;
+struct ble_init_svc_rsp;
+struct bt_conn_cb;
+struct bt_gatt_attr;
+
+enum BLE_STATE {
+	BLE_ST_NOT_READY = 0,
+	BLE_ST_DISABLED,
+	BLE_ST_ENABLED,
+	BLE_ST_DTM
+};
+
+struct _ble_service_cb {
+	T_QUEUE queue; /* Queue for the messages */
+	uint8_t ble_state;
+};
+
+extern struct _ble_service_cb _ble_cb;
+
+/** Send advertisement timeout event to application */
+void ble_gap_advertisement_timeout(void);
+
+#ifdef CONFIG_BLE_CORE_TEST
+void test_ble_service_init(void);
+T_QUEUE get_service_queue(void);
+#endif
+
+#endif /* BLE_SERVICE_INT_H_ */
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_internal.h b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_internal.h
new file mode 100644
index 00000000..013043a5
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_internal.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2016, Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __BLE_SERVICE_INTERNAL_H__
+#define __BLE_SERVICE_INTERNAL_H__
+
+// For MSG_ID_BLE_SERVICE_BASE
+#include "services/services_ids.h"
+
+/* BLE High level Message IDs used for request, response, events. */
+enum BLE_MSG_ID_REQ {
+	MSG_ID_BLE_ENABLE_REQ = MSG_ID_BLE_SERVICE_BASE,  /* Message ID for enable request */
+	MSG_ID_BLE_SET_NAME_REQ,                  /* Message ID for set name request */
+	MSG_ID_BLE_START_SCAN_REQ,            /* Message ID for start scan request */
+	MSG_ID_BLE_STOP_SCAN_REQ,             /* Message ID for stop scan request */
+	MSG_ID_BLE_INIT_SVC_REQ,                  /* Message ID for init service request */
+
+	/* BLE debug command */
+	MSG_ID_BLE_DBG_REQ,                       /* Message ID for BLE debug command request */
+
+	MSG_ID_BLE_REQ_LAST,
+};
+
+/* Parameters of MSG_ID_BLE_ENABLE_REQ. */
+struct ble_enable_req {
+	struct cfw_message header; /* Component framework message header, MUST be first element of structure */
+	struct bt_le_conn_param central_conn_params;
+	uint8_t enable;
+	uint8_t bda_present;
+	bt_addr_le_t bda;
+};
+
+struct ble_disconnect_req {
+	struct cfw_message header; /* Component framework message header, MUST be first element of structure */
+	struct bt_conn *conn;      /* Connection reference */
+	uint8_t reason;            /* Reason of the disconnect*/
+};
+
+struct ble_connect_req {
+	struct cfw_message header; /* Component framework message header, MUST be first element of structure */
+	struct bt_le_conn_param conn_params;
+	bt_addr_le_t bd_addr;
+};
+
+struct ble_init_svc_req {
+	struct cfw_message header; /* Component framework message header, MUST be first element of structure */
+	int (*init_svc)(struct ble_init_svc_req *msg, struct _ble_service_cb *p_cb);
+	void (*init_svc_complete)(struct ble_init_svc_req *req);
+	uint8_t status;
+};
+
+#endif /* __BLE_SERVICE_INTERNAL_H__ */
diff --git a/system/libarc32_arduino101/framework/src/services/ble/ble_service_utils.c b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_utils.c
similarity index 50%
rename from system/libarc32_arduino101/framework/src/services/ble/ble_service_utils.c
rename to system/libarc32_arduino101/framework/src/services/ble_service/ble_service_utils.c
index d40c1b72..0275e79b 100644
--- a/system/libarc32_arduino101/framework/src/services/ble/ble_service_utils.c
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_utils.c
@@ -31,77 +31,48 @@
 #include <string.h>
 #include "os/os.h"
 #include "ble_protocol.h"
-#include "services/ble/ble_service_gap_api.h"
+#include "ble_service.h"
+#include "ble_service_int.h"
 #include "ble_service_utils.h"
 
-uint8_t ble_sizeof_bt_uuid(const struct bt_uuid * p_uuid)
+void uint8_to_ascii(uint8_t in, uint8_t *p)
 {
-	return p_uuid->type + 1;
+	uint8_t hi = (in & 0xF0) >> 4;
+	uint8_t lo = in & 0x0F;
+
+	if (hi < 0x0A)
+		*p = '0' + hi;
+	else
+		*p = 'A' + (hi - 0x0A);
+
+	p++;
+
+	if (lo < 10)
+		*p = '0' + lo;
+	else
+		*p = 'A' + (lo - 0x0A);
 }
 
-uint8_t * ble_encode_bt_uuid(const struct bt_uuid * p_uuid, uint8_t * p_data)
+void uint8buf_to_ascii(uint8_t * dst, const uint8_t * src, int len)
 {
-	// start with the type (length)
-	UINT8_TO_LESTREAM(p_data, p_uuid->type);
-	switch (p_uuid->type) {
-	case BT_UUID16:
-		UINT16_TO_LESTREAM(p_data, p_uuid->uuid16);
-		break;
-	case BT_UUID32:
-		UINT32_TO_LESTREAM(p_data, p_uuid->uuid32);
-		break;
-	case BT_UUID128:
-		memcpy(p_data, p_uuid->uuid128, MAX_UUID_SIZE);
-		p_data += MAX_UUID_SIZE;
-		break;
+	int i;
+	for (i = 0; i < len; ++i) {
+		uint8_to_ascii(src[i], dst);
+		dst += 2;
 	}
-	return p_data;
 }
 
-uint8_t ble_cpy_bt_uuid(uint8_t * p_uuid_dst,
-		    const struct bt_uuid * p_uuid_src,
-		    uint8_t type)
+size_t adv_data_len(const struct bt_data *ad, size_t count)
 {
-	uint8_t *p = p_uuid_dst;
-	uint8_t res_type = (p_uuid_src->type >= type) ? type : 0;
+	int i;
+	size_t data_len = 0;
 
-	switch (res_type) {
-	case BT_UUID16:
-		switch (p_uuid_src->type) {
-		case BT_UUID16:
-			UINT16_TO_LESTREAM(p, p_uuid_src->uuid16);
-			break;
-		case BT_UUID32:
-			UINT16_TO_LESTREAM(p, p_uuid_src->uuid16);
-			break;
-		case BT_UUID128: {
-			uint16_t uuid16;
-			p = (uint8_t *)&p_uuid_src->uuid128[BLE_BASE_UUID_OCTET_OFFSET];
-			LESTREAM_TO_UINT16(p, uuid16);
-			p = p_uuid_dst;
-			UINT16_TO_LESTREAM(p, p_uuid_src->uuid16);
-			break;
-		}
-		}
-		break;
-	case BT_UUID32:
-		switch (p_uuid_src->type) {
-		case BT_UUID32:
-			UINT32_TO_LESTREAM(p, p_uuid_src->uuid32);
-			break;
-		case BT_UUID128: {
-			uint32_t uuid32;
-			p = (uint8_t *)&p_uuid_src->uuid128[BLE_BASE_UUID_OCTET_OFFSET];
-			LESTREAM_TO_UINT32(p, uuid32);
-			p = p_uuid_dst;
-			UINT32_TO_LESTREAM(p, uuid32);
-			break;
-		}
-		}
-		break;
-	case BT_UUID128:
-		memcpy(p_uuid_dst, p_uuid_src->uuid128, MAX_UUID_SIZE);
-		break;
+	if (ad == NULL)
+		return 0;
+
+	for (i = 0; i < count ; i++) {
+		data_len += ad[i].data_len;
 	}
-	return res_type;
+
+	return data_len;
 }
diff --git a/system/libarc32_arduino101/framework/src/services/ble/ble_service_utils.h b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_utils.h
similarity index 66%
rename from system/libarc32_arduino101/framework/src/services/ble/ble_service_utils.h
rename to system/libarc32_arduino101/framework/src/services/ble_service/ble_service_utils.h
index a51f8dcd..8d5d9378 100644
--- a/system/libarc32_arduino101/framework/src/services/ble/ble_service_utils.h
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/ble_service_utils.h
@@ -28,20 +28,19 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef __BLE_SERVICE_UTILS_H__
-#define __BLE_SERVICE_UTILS_H__
+#ifndef BLE_SERVICE_UTILS_H_
+#define BLE_SERVICE_UTILS_H_
 
-#include "services/ble/ble_service_gap_api.h"
-#include "services/ble/ble_service_gatt.h"
-//#include "ble_service_int.h"
+#include <stdint.h>
 
-#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) + __must_be_array(a)
+// For HAVE_SAME_TYPE
+#include "compiler.h"
 
 #define IS_BIG_ENDIAN (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
 
 #define UINT8_TO_LESTREAM(p, i) \
     do { *(p)++ = (uint8_t)(i); } \
-    while (sizeof(char[1 - 2 * !(__builtin_types_compatible_p(__typeof__(p), uint8_t *))]) - 1)
+    while (sizeof(char[1 - 2 * !(HAVE_SAME_TYPE(p, uint8_t *))]) - 1)
 #define UINT16_TO_LESTREAM(p, i) UINT8_TO_LESTREAM(p, i); UINT8_TO_LESTREAM(p, (i)>>8)
 #define UINT32_TO_LESTREAM(p, i) UINT16_TO_LESTREAM(p, i); UINT16_TO_LESTREAM(p, (i)>>16)
 
@@ -59,8 +58,8 @@
 
 #define LESTREAM_TO_UINT8(p, i) \
     do { i = *p; p++; } \
-    while (sizeof(char[1 - 2 * !(__builtin_types_compatible_p(__typeof__(p), const uint8_t *) || \
-                                 __builtin_types_compatible_p(__typeof__(p), uint8_t *))]) - 1)
+    while (sizeof(char[1 - 2 * !(HAVE_SAME_TYPE(p, const uint8_t *) || \
+                                 HAVE_SAME_TYPE(p, uint8_t *))]) - 1)
 #define LESTREAM_TO_UINT16(p, i) \
     do { uint16_t temp16; LESTREAM_TO_UINT8(p, i); LESTREAM_TO_UINT8(p, temp16); i |= (temp16 << 8); } \
     while (0)
@@ -90,53 +89,38 @@
 #define BESTREAM_TO_INT32(p, i) \
     do {uint32_t __i; BESTREAM_TO_UINT32(p, __i); i = (int32_t)__i; } while (0)
 
-#define between(a, b, c) (((a) >= (b)) || ((a) <= (c)))
-
-#define strict_between(a, b, c) (((a) > (b)) || ((a) < (c)))
-
 /**
  * BLE helper functions.
  */
-#define BLE_BASE_UUID_OCTET_OFFSET 12
 
-/**
- * Compute the size required to encode a UUID in a buffer
+/**@brief Represent an unsigned 8 bit value in ASCII hexadecimal
+ *
+ * @param[in] in The value to represent
+ * @param[inout] p Pointer to the buffer to fill
  *
- * @param p_uuid Pointer to the UUID
+ * @note The representation requires 2 bytes
  */
-uint8_t ble_sizeof_bt_uuid(const struct bt_uuid * p_uuid);
+void uint8_to_ascii(uint8_t in, uint8_t * p);
 
-/**
- * Encode a UUID in a buffer
+/** Converts buffer from hex to ascii.
  *
- * @param p_uuid Pointer to the UUID to encode
- * @param p_data Pointer to the buffer to store the encoded UUID
+ * @param dst buffer converted
+ * @param src buffer which is converted
+ * @param len length of src (dst shall be large enough)
  *
- * @return The pointer to the buffer after the encoded UUID
+ * @return None
  */
-uint8_t * ble_encode_bt_uuid(const struct bt_uuid * p_uuid, uint8_t * p_data);
+void uint8buf_to_ascii(uint8_t * dst, const uint8_t * src, int len);
 
-/** Copy BT/BLE address and eventually reduce size.
- *
- * Copies a uuid from src to dst. type may modify the resulting uuid type. only
- * going from bigger to small type is supported. typically 128 bits to 16 bits
- * If it is different from src type, a transformation is applied:
- * IN: BT_UUID128: dd97c415-fed9-4766-b18f-ba690d24a06a (stored in little endian)
- * OUT: BT_UUID16: c415
- * OUT: BT_UUID32: dd97c415
- * or
- * IN: BT_UUID32: dd97c415
- * OUT: BT_UUID16: c415
- *
- * @param p_uuid_src source uuid
- * @param[out] p_uuid_dst: destination to copy uuid to (excluding type). little endian
- * @param type output type
+struct bt_data;
+/**
+ * Get advertisement data length.
  *
- * \return size/type of copied uuid: BT_UUID16, BT_UUID32, BT_UUID128 or 0 for failure!
+ * @param ad advertisement data
+ * @param count array length
+ * @return length of advertisement data
  *
  */
-uint8_t ble_cpy_bt_uuid(uint8_t * p_uuid_dst,
-                        const struct bt_uuid * p_uuid_src,
-                        uint8_t type);
+size_t adv_data_len(const struct bt_data *ad, size_t count);
 
-#endif
+#endif /* BLE_SERVICE_UTILS_H_ */
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/gap_internal.h b/system/libarc32_arduino101/framework/src/services/ble_service/gap_internal.h
new file mode 100644
index 00000000..333ad36e
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/gap_internal.h
@@ -0,0 +1,629 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef GAP_INTERNAL_H_
+#define GAP_INTERNAL_H_
+
+#include <stdint.h>
+/* For bt_addr_le_t */
+#include <bluetooth/hci.h>
+/* For bt_security_t */
+#include <bluetooth/conn.h>
+
+/* Directed advertisement timeout error after 1.28s */
+#define ERR_DIRECTED_ADVERTISING_TIMEOUT 0x3C
+
+enum NBLE_GAP_SM_PASSKEY_TYPE {
+	NBLE_GAP_SM_REJECT = 0,
+	NBLE_GAP_SM_PK_PASSKEY,
+	NBLE_GAP_SM_PK_OOB,
+};
+
+enum NBLE_GAP_SM_EVT {
+	NBLE_GAP_SM_EVT_START_PAIRING,
+	NBLE_GAP_SM_EVT_BONDING_COMPLETE,
+	NBLE_GAP_SM_EVT_LINK_ENCRYPTED,
+	NBLE_GAP_SM_EVT_LINK_SECURITY_CHANGE,
+};
+
+enum NBLE_GAP_RSSI_OPS {
+	NBLE_GAP_RSSI_DISABLE_REPORT = 0,
+	NBLE_GAP_RSSI_ENABLE_REPORT
+};
+
+enum NBLE_TEST_OPCODES {
+	NBLE_TEST_INIT_DTM = 0x01,
+	NBLE_TEST_START_DTM_RX = 0x1d,
+	NBLE_TEST_START_DTM_TX = 0x1e,
+	NBLE_TEST_END_DTM = 0x1f,
+	/* vendor specific commands start at 0x80 */
+	/* Set Tx power. To be called before start of tx test */
+	NBLE_TEST_SET_TXPOWER = 0x80,
+	NBLE_TEST_START_TX_CARRIER,
+};
+
+/*  DTM commands, opcodes, indexes. */
+#define H4_CMD              0x01
+#define HCI_OGF_LE_CMD      0x20
+
+#define DTM_HCI_STATUS_IDX    6
+#define DTM_HCI_LE_END_IDX    (DTM_HCI_STATUS_IDX + 1)
+
+
+struct nble_response {
+	int status;
+	void *user_data;
+};
+
+struct nble_gap_device_name {
+	/* Security mode for writing device name, @ref BLE_GAP_SEC_MODES */
+	uint8_t sec_mode;
+	/* 0: no authorization, 1: authorization required */
+	uint8_t authorization;
+	/* Device name length (0-248) */
+	uint8_t len;
+	uint8_t name_array[20];
+};
+
+struct nble_gap_connection_values {
+	/* Connection interval (unit 1.25 ms) */
+	uint16_t interval;
+	/* Connection latency (unit interval) */
+	uint16_t latency;
+	/* Connection supervision timeout (unit 10ms)*/
+	uint16_t supervision_to;
+};
+
+
+enum NBLE_GAP_SVC_ATTR_TYPE {
+	/* Device Name, UUID 0x2a00 */
+	NBLE_GAP_SVC_ATTR_NAME = 0,
+	/* Appearance, UUID 0x2a01 */
+	NBLE_GAP_SVC_ATTR_APPEARANCE,
+	/* Peripheral Preferred Connection Parameters (PPCP), UUID 0x2a04 */
+	NBLE_GAP_SVC_ATTR_PPCP = 4,
+	/* Central Address Resolution (CAR), UUID 0x2aa6, BT 4.2 */
+	NBLE_GAP_SVC_ATTR_CAR = 0xa6,
+};
+
+struct nble_gap_connection_params {
+	/* minimal connection interval: range 0x0006 to 0x0c80 (unit 1.25ms) */
+	uint16_t interval_min;
+	/* maximum connection interval: range 0x0006 to 0x0c80 must be bigger then min! */
+	uint16_t interval_max;
+	/* maximum connection slave latency: 0x0000 to 0x01f3 */
+	uint16_t slave_latency;
+	/* link supervision timeout: 0x000a to 0x0c80 (unit 10ms) */
+	uint16_t link_sup_to;
+};
+
+struct nble_gap_scan_parameters {
+	/* If 1, perform active scanning (scan requests). */
+	uint8_t     active;
+	/* If 1, ignore unknown devices (non whitelisted). */
+	uint8_t     selective;
+	/* Scan interval between 0x0004 and 0x4000 in 0.625ms units (2.5ms to 10.24s). */
+	uint16_t    interval;
+	/* Scan window between 0x0004 and 0x4000 in 0.625ms units (2.5ms to 10.24s). */
+	uint16_t    window;
+	/* Scan timeout between 0x0001 and 0xFFFF in seconds, 0x0000 disables timeout. */
+	uint16_t    timeout;
+};
+
+struct nble_gap_service_write_params {
+	/* GAP Characteristics attribute type  @ref BLE_GAP_SVC_ATTR_TYPE */
+	uint16_t attr_type;
+	union {
+		struct nble_gap_device_name name;
+		/* Appearance UUID */
+		uint16_t appearance;
+		/* Preferred Peripheral Connection Parameters */
+		struct nble_gap_connection_params conn_params;
+		/* Central Address Resolution support 0: no, 1: yes */
+		uint8_t car;
+	};
+};
+
+struct nble_service_read_bda_response {
+	int status;
+	/* If @ref status ok */
+	bt_addr_le_t bd;
+	void *user_data;
+};
+
+struct nble_service_write_response {
+	int status;
+	/* GAP Characteristics attribute type  @ref BLE_GAP_SVC_ATTR_TYPE */
+	uint16_t attr_type;
+	void *user_data;
+};
+
+struct nble_gap_service_read_params {
+	/* Type of GAP data characteristic to read @ref BLE_GAP_SVC_ATTR_TYPE */
+	uint16_t attr_type;
+};
+
+struct nble_debug_params {
+	uint32_t u0;
+	uint32_t u1;
+};
+
+struct nble_debug_resp {
+	int status;
+	uint32_t u0;
+	uint32_t u1;
+	void *user_data;
+};
+
+typedef void (*nble_set_bda_cb_t)(int status, void *user_data);
+
+struct nble_set_bda_params {
+	bt_addr_le_t bda;
+	nble_set_bda_cb_t cb;
+	void *user_data;
+};
+
+struct nble_set_bda_rsp {
+	nble_set_bda_cb_t cb;
+	void *user_data;
+	int status;
+};
+
+struct bt_eir_data {
+	uint8_t len;
+	uint8_t data[31];
+};
+
+struct nble_gap_adv_params {
+	uint16_t timeout;
+	/* min interval 0xffff: use default 0x0800 */
+	uint16_t interval_min;
+	/* max interval 0xffff: use default 0x0800 */
+	uint16_t interval_max;
+	/* advertisement types @ref GAP_ADV_TYPES */
+	uint8_t type;
+	/* filter policy to apply with white list */
+	uint8_t filter_policy;
+	/* bd address of peer device in case of directed advertisement */
+	bt_addr_le_t peer_bda;
+};
+
+struct nble_gap_ad_data_params {
+	/* Advertisement data, maybe 0 (length) */
+	struct bt_eir_data ad;
+	/* Scan response data, maybe 0 (length) */
+	struct bt_eir_data sd;
+};
+
+struct nble_log_s {
+	uint8_t param0;
+	uint8_t param1;
+	uint8_t param2;
+	uint8_t param3;
+};
+
+/* bt_dev flags: the flags defined here represent BT controller state */
+enum {
+	BT_DEV_READY,
+
+	BT_DEV_ADVERTISING,
+	BT_DEV_KEEP_ADVERTISING,
+	BT_DEV_SCANNING,
+	BT_DEV_EXPLICIT_SCAN,
+
+#if defined(CONFIG_BLUETOOTH_BREDR)
+	BT_DEV_ISCAN,
+	BT_DEV_PSCAN,
+#endif /* CONFIG_BLUETOOTH_BREDR */
+};
+
+void nble_log(const struct nble_log_s *param, char *buf, uint8_t buflen);
+
+void on_nble_up(void);
+
+void nble_gap_service_write_req(const struct nble_gap_service_write_params *params);
+
+void on_nble_gap_read_bda_rsp(const struct nble_service_read_bda_response *params);
+
+void nble_gap_dbg_req(const struct nble_debug_params *params, void *user_data);
+
+void on_nble_gap_dbg_rsp(const struct nble_debug_resp *params);
+
+void on_nble_set_bda_rsp(const struct nble_set_bda_rsp *params);
+
+void nble_set_bda_req(const struct nble_set_bda_params *params);
+
+void nble_gap_set_adv_data_req(struct nble_gap_ad_data_params *ad_data_params);
+
+void nble_gap_set_adv_params_req(struct nble_gap_adv_params *adv_params);
+
+void nble_gap_start_adv_req(void);
+
+void on_nble_gap_start_advertise_rsp(const struct nble_response *params);
+
+void nble_gap_stop_adv_req(void *user_data);
+
+void nble_gap_read_bda_req(void *priv);
+
+struct nble_gap_irk_info {
+	/* Identity Resolving Key (IRK) */
+	uint8_t irk[16];
+};
+
+void on_nble_common_rsp(const struct nble_response *params);
+
+struct nble_gap_connect_update_params {
+	uint16_t conn_handle;
+	struct nble_gap_connection_params params;
+};
+
+void nble_gap_conn_update_req(const struct nble_gap_connect_update_params *params);
+
+void on_nble_gap_conn_update_rsp(const struct nble_response *params);
+
+struct nble_gap_connect_req_params {
+	bt_addr_le_t bda;
+	struct nble_gap_connection_params conn_params;
+	struct nble_gap_scan_parameters scan_params;
+};
+
+struct nble_gap_disconnect_req_params {
+	uint16_t conn_handle;
+	uint8_t reason;
+};
+
+void nble_gap_disconnect_req(const struct nble_gap_disconnect_req_params *params);
+
+struct nble_gap_sm_config_params {
+	/* Security options (@ref BLE_GAP_SM_OPTIONS) */
+	uint8_t options;
+	/* I/O Capabilities to allow passkey exchange (@ref BLE_GAP_IO_CAPABILITIES) */
+	uint8_t io_caps;
+	/* Maximum encryption key size (7-16) */
+	uint8_t key_size;
+	uint8_t oob_present;
+};
+
+void nble_gap_sm_config_req(const struct nble_gap_sm_config_params *params);
+
+struct nble_gap_sm_config_rsp {
+	void *user_data;
+	int status;
+	bool sm_bond_dev_avail;
+};
+
+void on_nble_gap_sm_config_rsp(struct nble_gap_sm_config_rsp *params);
+
+
+struct nble_gap_sm_pairing_params {
+	/* authentication level see @ref BLE_GAP_SM_OPTIONS */
+	uint8_t auth_level;
+};
+
+struct nble_gap_sm_security_params {
+	struct bt_conn *conn;
+	uint16_t conn_handle;
+	/* Local authentication/bonding parameters */
+	struct nble_gap_sm_pairing_params params;
+};
+
+void nble_gap_sm_security_req(const struct nble_gap_sm_security_params *
+			    params);
+
+struct nble_gap_sm_passkey {
+	uint8_t type;
+	union {
+		uint32_t passkey;
+		uint8_t oob[16];
+		uint8_t reason;
+	};
+};
+
+struct nble_gap_sm_key_reply_req_params {
+	struct bt_conn *conn;
+	uint16_t conn_handle;
+	struct nble_gap_sm_passkey params;
+};
+
+void nble_gap_sm_passkey_reply_req(const struct nble_gap_sm_key_reply_req_params
+				   *params);
+
+struct nble_gap_sm_clear_bond_req_params {
+	bt_addr_le_t addr;
+};
+
+void nble_gap_sm_clear_bonds_req(const struct nble_gap_sm_clear_bond_req_params
+				 *params);
+
+struct nble_gap_sm_response {
+	int status;
+	struct bt_conn *conn;
+};
+
+void on_nble_gap_sm_common_rsp(const struct nble_gap_sm_response *rsp);
+
+/**
+ * Callback for rssi event.
+ */
+typedef void (*rssi_report_t)(const int8_t *rssi_data);
+
+/**
+ * Callback for rssi report response.
+ */
+typedef void (*rssi_report_resp_t)(int status);
+
+struct nble_rssi_report_params {
+	uint16_t conn_handle;
+	/* RSSI operation @ref NBLE_GAP_RSSI_OPS */
+	uint8_t op;
+	/* Channel for RSSI enabling */
+	uint8_t channel;
+	/* minimum RSSI dBm change to report a new RSSI value */
+	uint8_t delta_dBm;
+	/* number of delta_dBm changes before sending a new RSSI report */
+	uint8_t min_count;
+};
+
+void ble_gap_set_rssi_report(struct nble_rssi_report_params *params,
+			      struct bt_conn *conn,
+			      rssi_report_resp_t resp_cb, rssi_report_t evt_cb);
+
+void nble_gap_set_rssi_report_req(const struct nble_rssi_report_params *params,
+			    void *user_data);
+
+void on_nble_gap_set_rssi_report_rsp(const struct nble_response *params);
+
+struct nble_gap_scan_params {
+	uint16_t interval;
+	uint16_t window;
+	uint8_t scan_type;
+	uint8_t use_whitelist;
+};
+
+void nble_gap_start_scan_req(const struct nble_gap_scan_params *params);
+
+void nble_gap_stop_scan_req(void);
+
+void on_nble_gap_scan_start_stop_rsp(const struct nble_response *rsp);
+
+void nble_gap_connect_req(const struct nble_gap_connect_req_params *params,
+		void *user_data);
+
+void on_nble_gap_connect_rsp(const struct nble_response *params);
+
+void nble_gap_cancel_connect_req(void *priv);
+
+void on_nble_gap_cancel_connect_rsp(const struct nble_response *params);
+
+enum BLE_GAP_SET_OPTIONS {
+	BLE_GAP_SET_CH_MAP = 0,
+};
+
+struct nble_gap_channel_map {
+	/* connection on which to change channel map */
+	uint16_t conn_handle;
+	/* 37 bits are used of the 40 bits (LSB) */
+	uint8_t map[5];
+};
+
+
+struct nble_gap_set_option_params {
+	/* Option to set @ref BLE_GAP_SET_OPTIONS */
+	uint8_t op;
+	union {
+		struct nble_gap_channel_map ch_map;
+	};
+};
+
+/*
+ *  Generic request op codes.
+ * This allows to access some non connection related commands like DTM.
+ */
+enum BLE_GAP_GEN_OPS {
+	/* Not used now. */
+	DUMMY_VALUE = 0,
+};
+
+struct nble_gap_gen_cmd_params {
+	/* @ref BLE_GAP_GEN_OPS */
+	uint8_t op_code;
+};
+
+/* Temporary patch: RSSI processing for UAS */
+struct nble_uas_rssi_calibrate {
+	float distance;
+};
+void nble_uas_rssi_calibrate_req(const struct nble_uas_rssi_calibrate *p_struct);
+
+/* Temporary patch: RSSI processing for UAS */
+struct nble_uas_bucket_change {
+	uint8_t distance;
+};
+void on_nble_uas_bucket_change(const struct nble_uas_bucket_change *p_params);
+
+struct nble_version {
+	uint8_t version;
+	uint8_t major;
+	uint8_t minor;
+	uint8_t patch;
+	char version_string[20];
+	uint8_t hash[4];
+};
+
+typedef void (*ble_get_version_cb_t)(const struct nble_version *ver);
+
+struct nble_gap_get_version_param {
+	ble_get_version_cb_t cb;
+};
+
+struct nble_version_response {
+	struct nble_gap_get_version_param params;
+	struct nble_version ver;
+};
+
+void nble_get_version_req(const struct nble_gap_get_version_param *params);
+
+void on_nble_get_version_rsp(const struct nble_version_response *params);
+
+void nble_gap_dtm_init_req(void *user_data);
+
+void on_nble_gap_dtm_init_rsp(void *user_data);
+
+struct nble_gap_tx_power_params {
+	int8_t tx_power;
+};
+
+void nble_gap_tx_power_req(const struct nble_gap_tx_power_params *params);
+
+void on_nble_gap_tx_power_rsp(const struct nble_response *params);
+
+struct nble_gap_connect_evt {
+	uint16_t conn_handle;
+	struct nble_gap_connection_values conn_values;
+	/* 0 if connected as master, otherwise as slave */
+	uint8_t role_slave;
+	/* Address of peer device */
+	bt_addr_le_t peer_bda;
+};
+
+void on_nble_gap_connect_evt(const struct nble_gap_connect_evt *evt);
+
+struct nble_gap_disconnect_evt {
+	uint16_t conn_handle;
+	uint8_t hci_reason;
+};
+
+void on_nble_gap_disconnect_evt(const struct nble_gap_disconnect_evt *evt);
+
+struct nble_gap_conn_update_evt {
+	uint16_t conn_handle;
+	struct nble_gap_connection_values conn_values;
+};
+
+void on_nble_gap_conn_update_evt(const struct nble_gap_conn_update_evt *evt);
+
+struct nble_gap_adv_report_evt {
+	bt_addr_le_t addr;
+	int8_t rssi;
+	uint8_t adv_type;
+};
+
+void on_nble_gap_adv_report_evt(const struct nble_gap_adv_report_evt *evt,
+				const uint8_t *buf, uint8_t len);
+
+struct nble_gap_dir_adv_timeout_evt {
+	uint16_t conn_handle;
+	uint16_t error;
+};
+
+void on_nble_gap_dir_adv_timeout_evt(const struct nble_gap_dir_adv_timeout_evt *p_evt);
+
+#define BLE_GAP_RSSI_EVT_SIZE	32
+struct nble_gap_rssi_evt {
+	uint16_t conn_handle;
+	int8_t rssi_data[BLE_GAP_RSSI_EVT_SIZE];
+};
+
+void on_nble_gap_rssi_evt(const struct nble_gap_rssi_evt *evt);
+
+struct nble_gap_timout_evt {
+	uint16_t conn_handle;
+	/* reason for timeout @ref BLE_SVC_GAP_TIMEOUT_REASON */
+	int reason;
+};
+
+struct nble_gap_sm_passkey_req_evt {
+	uint16_t conn_handle;
+	uint8_t key_type;
+};
+
+void on_nble_gap_sm_passkey_req_evt(const struct nble_gap_sm_passkey_req_evt *evt);
+
+struct nble_gap_sm_passkey_disp_evt {
+	uint16_t conn_handle;
+	uint32_t passkey;
+};
+
+void on_nble_gap_sm_passkey_display_evt(const struct nble_gap_sm_passkey_disp_evt *evt);
+
+struct nble_link_sec {
+	bt_security_t sec_level;
+	uint8_t enc_size;
+};
+
+struct nble_gap_sm_status_evt {
+	uint16_t conn_handle;
+	uint8_t evt_type;
+	int status;
+	struct nble_link_sec enc_link_sec;
+};
+
+void on_nble_gap_sm_status_evt(const struct nble_gap_sm_status_evt *evt);
+
+void on_nble_set_bda_rsp(const struct nble_set_bda_rsp *params);
+
+enum BLE_INFO_REQ_TYPES {
+	BLE_INFO_BONDING = 1, /* Get bonding database related information */
+	BLE_INFO_LAST /* Keep last */
+};
+
+struct ble_gap_bonded_dev_info
+{
+	uint8_t       addr_count;      /* Count of le_addr in array. */
+	uint8_t       irk_count;       /* IRK count */
+	bt_addr_le_t  le_addr[];       /* Bonded device address */
+};
+
+struct ble_get_info_rsp {
+	int status; 		/* Response status */
+	uint8_t info_type; 	/* Requested information type */
+	struct ble_gap_bonded_dev_info info_params;
+};
+
+struct nble_gap_sm_bond_info;
+typedef void (*ble_bond_info_cb_t)(const struct nble_gap_sm_bond_info *info,
+			      const bt_addr_le_t *addr, uint16_t len,
+			      void *user_data);
+
+struct nble_gap_sm_bond_info_param {
+	ble_bond_info_cb_t cb;
+	void *user_data;
+	bool include_bonded_addrs;
+};
+
+void nble_gap_sm_bond_info_req(const struct nble_gap_sm_bond_info_param *params);
+
+struct nble_gap_sm_bond_info {
+	int err;
+	uint8_t addr_count;
+	uint8_t irk_count;
+};
+
+struct nble_gap_sm_bond_info_rsp {
+	ble_bond_info_cb_t cb;
+	void *user_data;
+	struct nble_gap_sm_bond_info info;
+};
+
+void on_nble_gap_sm_bond_info_rsp(const struct nble_gap_sm_bond_info_rsp *rsp,
+		const bt_addr_le_t *peer_addr, uint16_t len);
+
+void ble_gap_get_bonding_info(ble_bond_info_cb_t func, void *user_data,
+			      bool include_bonded_addrs);
+
+void ble_gap_get_version(ble_get_version_cb_t func);
+
+#endif /* GAP_INTERNAL_H_ */
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/gatt_internal.h b/system/libarc32_arduino101/framework/src/services/ble_service/gatt_internal.h
new file mode 100644
index 00000000..25ac4da8
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/gatt_internal.h
@@ -0,0 +1,308 @@
+/*
+ * Copyright (c) 2016 Intel Corporation
+ *
+ * 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.
+ */
+
+#ifndef GATT_INTERNAL_H_
+#define GATT_INTERNAL_H_
+
+#include <stdint.h>
+#include <bluetooth/gatt.h>
+
+/* Forward declarations */
+struct nble_response;
+
+#define BLE_GATT_MTU_SIZE 23
+
+/*
+ * Internal APIs used between host and BLE controller
+ * Typically they are required if gatt.h APIs can not be mapped 1:1 onto controller API
+ */
+
+enum NBLE_GATT_IND_TYPES {
+	NBLE_GATT_IND_TYPE_NONE = 0,
+	NBLE_GATT_IND_TYPE_NOTIFICATION,
+	NBLE_GATT_IND_TYPE_INDICATION,
+};
+
+struct nble_gatt_register_req {
+	/* Base address of the attribute table in the Quark mem space */
+	struct bt_gatt_attr *attr_base;
+	/* Number of of attributes in this service */
+	uint8_t attr_count;
+};
+
+struct nble_gatt_register_rsp {
+	int status;
+	struct bt_gatt_attr *attr_base;
+	/* Number of attributes successfully added */
+	uint8_t attr_count;
+};
+
+enum nble_gatt_wr_flag {
+	NBLE_GATT_WR_FLAG_REPLY	= 1,
+	NBLE_GATT_WR_FLAG_PREP	= 2,
+};
+
+struct nble_gatt_wr_evt {
+	struct bt_gatt_attr *attr;
+	uint16_t conn_handle;
+	uint16_t offset;
+	uint8_t flag;		/* Cf. enum nble_gatt_wr_flag */
+};
+
+struct nble_gatt_wr_exec_evt {
+	uint16_t conn_handle;
+	uint8_t flag;
+};
+
+struct nble_gatt_rd_evt {
+	struct bt_gatt_attr *attr;
+	uint16_t conn_handle;
+	uint16_t offset;
+};
+
+struct nble_gatts_rd_reply_params {
+	uint16_t conn_handle;
+	uint16_t offset;
+	int32_t status;
+};
+
+struct nble_gatts_wr_reply_params {
+	uint16_t conn_handle;
+	int32_t status;
+};
+
+struct nble_gatt_notif_ind_params {
+	const struct bt_gatt_attr *attr;
+	uint16_t offset;
+};
+
+struct nble_gatt_send_notif_params {
+	/* Function to be invoked when buffer is freed */
+	bt_gatt_notify_sent_func_t cback;
+	uint16_t conn_handle;
+	struct nble_gatt_notif_ind_params params;
+};
+
+struct nble_gatt_notif_rsp {
+	bt_gatt_notify_sent_func_t cback;
+	int status;
+	uint16_t conn_handle;
+	struct bt_gatt_attr *attr;
+};
+
+struct nble_gatt_send_ind_params {
+	/* Function to be invoked when buffer is freed */
+	bt_gatt_indicate_func_t cback;
+	uint16_t conn_handle;
+	struct nble_gatt_notif_ind_params params;
+};
+
+struct nble_gatt_ind_rsp {
+	bt_gatt_indicate_func_t cback;
+	int status;
+	uint16_t conn_handle;
+	struct bt_gatt_attr *attr;
+};
+
+struct nble_gatt_handle_range {
+	uint16_t start_handle;
+	uint16_t end_handle;
+};
+
+struct nble_gattc_primary {
+	uint16_t handle;
+	struct nble_gatt_handle_range range;
+	struct bt_uuid_128 uuid;
+};
+
+struct nble_gattc_included {
+	uint16_t handle;
+	struct nble_gatt_handle_range range;
+	struct bt_uuid_128 uuid;
+};
+
+struct nble_gattc_characteristic {
+	uint16_t handle;
+	uint8_t prop;
+	uint16_t value_handle;
+	struct bt_uuid_128 uuid;
+};
+
+struct nble_gattc_descriptor {
+	uint16_t handle;
+	struct bt_uuid_128 uuid;
+};
+
+struct nble_gattc_discover_rsp {
+	int32_t status;
+	void *user_data;
+	uint16_t conn_handle;
+	uint8_t type;
+};
+
+struct nble_gatts_attribute_rsp {
+	int32_t status;
+	struct bt_gatt_attr *attr;
+	void *user_data;
+};
+
+void nble_gatts_rd_reply_req(const struct nble_gatts_rd_reply_params *, uint8_t *, uint16_t);
+
+void nble_gatts_wr_reply_req(const struct nble_gatts_wr_reply_params *p_params);
+
+void nble_gatt_register_req(const struct nble_gatt_register_req *p_param,
+		       uint8_t *p_buf,
+		       uint16_t len);
+
+struct nble_gatt_attr_handles {
+	uint16_t handle;
+};
+
+void on_nble_gatt_register_rsp(const struct nble_gatt_register_rsp *p_param,
+		const struct nble_gatt_attr_handles *p_attrs, uint8_t len);
+
+void on_nble_gatts_write_evt(const struct nble_gatt_wr_evt *p_evt,
+			    const uint8_t *p_buf, uint8_t buflen);
+
+void on_nble_gatts_write_exec_evt(const struct nble_gatt_wr_exec_evt *evt);
+
+void on_nble_gatts_read_evt(const struct nble_gatt_rd_evt *p_evt);
+
+void nble_gatt_send_notif_req(const struct nble_gatt_send_notif_params *p_params,
+			     const uint8_t *p_value, uint16_t length);
+
+void nble_gatt_send_ind_req(const struct nble_gatt_send_ind_params *p_params,
+			   const uint8_t *p_value, uint8_t length);
+
+void on_nble_gatts_send_notif_rsp(const struct nble_gatt_notif_rsp *rsp);
+
+void on_nble_gatts_send_ind_rsp(const struct nble_gatt_ind_rsp *rsp);
+
+#define DISCOVER_FLAGS_UUID_PRESENT 1
+
+struct nble_discover_params {
+	void *user_data;
+	struct bt_uuid_128 uuid;
+	struct nble_gatt_handle_range handle_range;
+	uint16_t conn_handle;
+	uint8_t type;
+	uint8_t flags;
+};
+
+void nble_gattc_discover_req(const struct nble_discover_params *req);
+
+void on_nble_gattc_discover_rsp(const struct nble_gattc_discover_rsp *rsp,
+		const uint8_t *data, uint8_t data_len);
+
+/*
+ * GATT Attribute stream structure.
+ *
+ * This structure is a "compressed" copy of @ref bt_gatt_attr.
+ * UUID pointer and user_data pointer are used as offset into buffer itself.
+ * The offset is from the beginning of the buffer. therefore a value of 0
+ * means that UUID or user_data is not present.
+ */
+struct ble_gatt_attr {
+	/* Attribute permissions */
+	uint16_t perm;
+	/* Attribute variable data size */
+	uint16_t data_size;
+	/* Attribute variable data: always starts with the UUID and data follows */
+	uint8_t data[];
+};
+
+struct ble_gattc_read_params {
+	void *user_data;
+	uint16_t conn_handle;
+	uint16_t handle;
+	uint16_t offset;
+};
+
+struct ble_gattc_read_multiple_params {
+	void *user_data;
+	uint16_t conn_handle;
+};
+
+struct ble_gattc_read_rsp {
+	int status;
+	void *user_data;
+	uint16_t conn_handle;
+	uint16_t handle;
+	uint16_t offset;
+};
+
+/* forward declaration */
+struct bt_gatt_write_params;
+
+typedef void (*bt_att_func_t)(struct bt_conn *conn, uint8_t err,
+		     const struct bt_gatt_write_params *wr_params);
+
+struct bt_gatt_write_params {
+	/* Function invoked upon write response */
+	bt_att_func_t func;
+	/* User specific data */
+	void *user_data[2];
+};
+
+struct ble_gattc_write_params {
+	uint16_t conn_handle;
+	uint16_t handle;
+	uint16_t offset;
+	/* different than 0 if response required */
+	uint8_t with_resp;
+	struct bt_gatt_write_params wr_params;
+};
+
+struct ble_gattc_write_rsp {
+	uint16_t conn_handle;
+	int status;
+	uint16_t handle;
+	uint16_t len;
+	struct bt_gatt_write_params wr_params;
+};
+
+void nble_gattc_read_req(const struct ble_gattc_read_params *params);
+
+void on_nble_gattc_read_rsp(const struct ble_gattc_read_rsp *rsp,
+		uint8_t *data, uint8_t data_len);
+
+void nble_gattc_read_multiple_req(
+		const struct ble_gattc_read_multiple_params *params,
+		const uint16_t *data, uint16_t data_len);
+
+void on_nble_gattc_read_multiple_rsp(const struct ble_gattc_read_rsp *rsp,
+		uint8_t *data, uint8_t data_len);
+
+void nble_gattc_write_req(const struct ble_gattc_write_params *params,
+		const uint8_t *buf, uint8_t buflen);
+
+void on_nble_gattc_write_rsp(const struct ble_gattc_write_rsp *rsp);
+
+void bt_gatt_connected(struct bt_conn *conn);
+void bt_gatt_disconnected(struct bt_conn *conn);
+
+
+struct ble_gattc_value_evt {
+	uint16_t conn_handle;
+	int status;
+	uint16_t handle;
+	uint8_t type;
+};
+
+void on_nble_gattc_value_evt(const struct ble_gattc_value_evt *evt,
+		uint8_t *buf, uint8_t buflen);
+
+#endif /* GATT_INTERNAL_H_ */
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/nble_driver.c b/system/libarc32_arduino101/framework/src/services/ble_service/nble_driver.c
new file mode 100644
index 00000000..4a128f13
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/nble_driver.c
@@ -0,0 +1,354 @@
+/*
+ * Copyright (c) 2015, Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+
+#include "nble_driver.h"
+
+#include "portable.h"
+
+/* ipc/rpc uart interface */
+#include "os/os.h"
+#include "infra/ipc_requests.h"
+#include "infra/port.h"
+#include "infra/log.h"
+#include "drivers/soc_gpio.h"
+#include "drivers/ipc_uart_ns16550.h"
+
+#include "platform.h"
+
+#include "rpc.h"
+
+#include "util/misc.h"
+
+#include "infra/time.h"
+
+#include "infra/ipc_uart.h"
+
+
+
+/*
+ * Macro definition for reset pin
+ * Curie and other board - Everything should be working out of the box
+ */
+#define BLE_SW_CLK_PIN  27
+#define BLE_SWDIO_PIN   6
+#define RESET_PIN       BLE_SWDIO_PIN
+#define QRK_BLE_INT     5
+
+static uint16_t rpc_port_id;
+static list_head_t m_rpc_tx_q;
+
+extern void on_nble_curie_log(char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	log_vprintk(LOG_LEVEL_INFO, LOG_MODULE_BLE, fmt, args);
+	va_end(args);
+}
+
+/*
+ *  When we set val to 1 it will wake up the remote (BLE Core), setting it to 0
+ * will allow remote to sleep.
+ */
+static int nble_wake_assert(bool val)
+{
+    val = true;
+	uint8_t ret = soc_gpio_write(SOC_GPIO_32, QRK_BLE_INT, val);
+
+    //pr_debug(LOG_MODULE_BLE, "BLE wake up: %d", val);
+	if (ret != DRV_RC_OK) {
+		pr_debug(LOG_MODULE_BLE, "Error setting QRK_BLE_INT %d", ret);
+	}
+	return ret;
+}
+
+/**
+ * Function registered to be invoked when the IPC starts or ends
+ * a transmission session.
+ *
+ * This sets the wake state of ble core to either keep it awake at start of a tx
+ * or to allow sleep at the end of a TX.
+ *
+ * @param wake_state false if remote may sleep (end of transmission).
+ * true, wake remote to start a transmission session.
+ * @param ignored not used
+ */
+static void nble_ipc_tx_wake_cb(bool wake_state, void *ignored)
+{
+	nble_wake_assert(wake_state);
+}
+
+static int nble_interface_init(void)
+{
+	DRIVER_API_RC ret = DRV_RC_OK;
+	/* setup IPC UART reference */
+	ipc_uart_ns16550_set_tx_cb(nble_ipc_tx_wake_cb, NULL);
+
+	/* Configure GPIO as output and high by default */
+	gpio_cfg_data_t config;
+	config.gpio_type = GPIO_OUTPUT;
+	ret = soc_gpio_set_config(SOC_GPIO_32, QRK_BLE_INT, &config);
+	if (ret != DRV_RC_OK)
+		return -1;
+
+	list_init(&m_rpc_tx_q);
+
+	ret = nble_wake_assert(1);
+	return ret;
+}
+
+void uart_ipc_disable(void)
+{
+	ipc_uart_ns16550_disable(SYNC_CHANNEL);
+}
+
+static void *m_rpc_channel;
+
+struct rpc_tx_elt {
+	list_t l;
+	uint16_t length;
+	uint8_t data[0];
+};
+
+/**
+ * Try to send a element of the RPC waiting list
+ *
+ * @param l Pointer to the list element to try to send on the UART
+ * @return
+ */
+static int uart_rpc_try_tx(list_t *l)
+{
+	struct rpc_tx_elt *p_elt;
+
+	/* Retrieve the RPC TX element from the list pointer */
+	p_elt = container_of(l, struct rpc_tx_elt, l);
+
+	/* Try to send the element payload */
+	return ipc_uart_ns16550_send_pdu(m_rpc_channel, 
+	                                 p_elt->length, 
+	                                 p_elt->data);
+}
+
+/**
+ * Try to send an RPC TX queue element during the free operation
+ * of the previous message.  This is invoked under interrupt context
+ * and therefore does not require protection.  It is also expected
+ * that the tx operation can not fail.
+ */
+static void uart_rpc_try_tx_on_free(void)
+{
+	list_t *l;
+	int ret;
+
+	/* Get next element in tx q */
+	l = list_get(&m_rpc_tx_q);
+	if (l) {
+		ret = uart_rpc_try_tx(l);
+
+		/* It is not possible to fail when called on free event */
+		assert(ret == IPC_UART_ERROR_OK);
+	}
+}
+
+/**
+ * Try to send an RPC TX queue element after enqueuing an element to the
+ * RPC TX queue.  At this point the state of the UART driver is not known
+ * and there could be a transmission in progress, so the procedure is to
+ * protect from interruption, pick (not get) the first element and only
+ * get it out of the queue if the tx operation was successful.
+ */
+static void uart_rpc_try_tx_on_add(void)
+{
+	list_t *l;
+	int ret;
+
+	int flags = interrupt_lock();
+
+	/* Pick next element in tx q */
+	l = m_rpc_tx_q.head;
+	if (l) {
+		ret = uart_rpc_try_tx(l);
+        
+		/* If it was sent correctly, remove it from the queue */
+		if (ret == IPC_UART_ERROR_OK) {
+			l = list_get(&m_rpc_tx_q);
+		}
+	}
+	
+	interrupt_unlock(flags);
+}
+
+/**
+ * Function handling the events from the UART IPC (irq).
+ *
+ * @param channel Channel on which the event applies
+ * @param request IPC_MSG_TYPE_MESSAGE when a new message was received,
+ * IPC_MSG_TYPE_FREE when the previous message was sent and can be freed.
+ * @param len Length of the data
+ * @param p_data Pointer to the data
+ * @return 0
+ */
+static int uart_ipc_rpc_cback(int channel, int request, int len, void *p_data)
+{
+	switch (request) {
+	case IPC_MSG_TYPE_MESSAGE: {
+#ifdef CONFIG_RPC_IN
+		/* if BLE service is available, handle it in BLE service context */
+		struct ble_rpc_callin *rpc = (void *) message_alloc(
+				sizeof(*rpc), NULL);
+if (NULL == rpc)
+{
+    panic(-1);
+}
+		MESSAGE_ID(&rpc->msg) = 0;
+		MESSAGE_LEN(&rpc->msg) = sizeof(*rpc);
+		MESSAGE_SRC(&rpc->msg) = rpc_port_id;
+		MESSAGE_DST(&rpc->msg) = rpc_port_id;
+		MESSAGE_TYPE(&rpc->msg) = TYPE_INT;
+		rpc->p_data = p_data;
+		rpc->len = len;
+		if (port_send_message(&rpc->msg) != E_OS_OK)
+			panic(-1);
+#endif
+	}
+		break;
+	case IPC_MSG_TYPE_FREE:
+	{
+		/* Free the message */
+    	struct rpc_tx_elt *p_elt;
+
+    	p_elt = container_of(p_data, struct rpc_tx_elt, data);
+		bfree(p_elt);
+        
+        //pr_debug(LOG_MODULE_IPC, "%s:%p", __FUNCTION__, p_elt);
+
+		/* Try to send another message immediately */
+		uart_rpc_try_tx_on_free();
+		break;
+	}
+	default:
+		/* Free the message */
+		bfree(p_data);
+		pr_error(LOG_MODULE_BLE, "Unsupported RPC request");
+		break;
+	}
+	return 0;
+}
+
+uint8_t *rpc_alloc_cb(uint16_t length)
+{
+	struct rpc_tx_elt *p_elt;
+
+    //pr_info(0, "%s", __FUNCTION__);
+
+	p_elt = balloc(length + offsetof(struct rpc_tx_elt, data), NULL);
+    //pr_debug(LOG_MODULE_IPC, "%s:%p", __FUNCTION__, p_elt);
+	assert(p_elt != NULL);
+
+	/* Save the length of the buffer */
+	p_elt->length = length;
+
+	return p_elt->data;
+}
+
+/* called under NON-interrupt context */
+void rpc_transmit_cb(uint8_t *p_buf, uint16_t length)
+{
+	struct rpc_tx_elt *p_elt;
+
+	p_elt = container_of(p_buf, struct rpc_tx_elt, data);
+
+	list_add(&m_rpc_tx_q, &p_elt->l);
+
+	uart_rpc_try_tx_on_add();
+}
+
+/* nble reset is achieved by asserting low the SWDIO pin.
+ * However, the BLE Core chip can be in SWD debug mode, and NRF_POWER->RESET = 0 due to,
+ * other constraints: therefore, this reset might not work everytime, especially after
+ * flashing or debugging.
+ */
+void nble_driver_init(void)
+{
+    uint32_t delay_until;
+    
+    nble_interface_init();
+    /* Setup UART0 for BLE communication, HW flow control required  */
+    SET_PIN_MODE(18, QRK_PMUX_SEL_MODEA); /* UART0_RXD        */
+    SET_PIN_MODE(19, QRK_PMUX_SEL_MODEA); /* UART0_TXD        */
+    SET_PIN_MODE(40, QRK_PMUX_SEL_MODEB); /* UART0_CTS_B      */
+    SET_PIN_MODE(41, QRK_PMUX_SEL_MODEB); /* UART0_RTS_B      */
+    
+	ipc_uart_init(0);
+	
+    //while (1)
+    //{}
+	/* RESET_PIN depends on the board and the local configuration: check top of file */
+	gpio_cfg_data_t pin_cfg = { .gpio_type = GPIO_OUTPUT };
+    
+	soc_gpio_set_config(SOC_GPIO_32, RESET_PIN, &pin_cfg);
+    //soc_gpio_set_config(SOC_GPIO_32_ID, BLE_SW_CLK_PIN, &pin_cfg);
+	/* Reset hold time is 0.2us (normal) or 100us (SWD debug) */
+	soc_gpio_write(SOC_GPIO_32, RESET_PIN, 0);
+	/* Wait for ~1ms */
+	delay_until = get_uptime_32k() + 32768;
+	
+	/* Open the UART channel for RPC while Nordic is in reset */
+	m_rpc_channel = ipc_uart_channel_open(RPC_CHANNEL, uart_ipc_rpc_cback);
+	
+	while (get_uptime_32k() < delay_until);
+	
+	/* De-assert the reset */
+	soc_gpio_write(SOC_GPIO_32, RESET_PIN, 1);
+
+
+	/* Set back GPIO to input to avoid interfering with external debugger */
+	pin_cfg.gpio_type = GPIO_INPUT;
+	soc_gpio_set_config(SOC_GPIO_32, RESET_PIN, &pin_cfg);
+}
+
+
+void nble_driver_configure(T_QUEUE queue, void (*handler)(struct message*, void*))
+{
+	rpc_port_id = port_alloc(queue);
+	assert(rpc_port_id != 0);
+	port_set_handler(rpc_port_id, handler, NULL);
+	pr_debug(LOG_MODULE_BLE, "%s: done port: %d", __func__, rpc_port_id);
+}
+
+
+void on_nble_curie_init(void)
+{
+	nble_driver_init();
+}
+
+
diff --git a/system/libarc32_arduino101/framework/src/services/ble_service/nble_driver.h b/system/libarc32_arduino101/framework/src/services/ble_service/nble_driver.h
new file mode 100644
index 00000000..7ea15b44
--- /dev/null
+++ b/system/libarc32_arduino101/framework/src/services/ble_service/nble_driver.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2015, Intel Corporation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NBLE_DRIVER_H_
+#define NBLE_DRIVER_H_
+
+#include "os/os.h"
+#include "infra/message.h"
+
+struct ble_rpc_callin {
+	struct message msg; /**< Message header, MUST be first element of structure */
+	uint8_t *p_data; /**< RPC buffer, must be freed after deserializing */
+	uint16_t len; /**< length of above buffer */
+};
+
+/**
+ * This resets and initializes the uart/ipc mechanism of nble.
+ *
+ * This this will trigger the call to @ref on_nble_up indicating that rpc
+ * mechanism is up and running.
+ */
+void nble_driver_init(void);
+
+void nble_driver_configure(T_QUEUE queue, void (*handler)(struct message*, void*));
+
+void uart_ipc_disable(void);
+
+#endif /* NBLE_DRIVER_H_ */
diff --git a/variants/arduino_101/libarc32drv_arduino101.a b/variants/arduino_101/libarc32drv_arduino101.a
index ff809d80..db6e5b45 100644
Binary files a/variants/arduino_101/libarc32drv_arduino101.a and b/variants/arduino_101/libarc32drv_arduino101.a differ
diff --git a/variants/arduino_101/linker_scripts/flash.ld b/variants/arduino_101/linker_scripts/flash.ld
index e75a7059..7ba34942 100644
--- a/variants/arduino_101/linker_scripts/flash.ld
+++ b/variants/arduino_101/linker_scripts/flash.ld
@@ -40,7 +40,7 @@ OUTPUT_FORMAT("elf32-littlearc", "elf32-bigarc", "elf32-littlearc")
 MEMORY
     {
     FLASH                 (rx) : ORIGIN = 0x40034000, LENGTH = 152K
-    SRAM                  (wx) : ORIGIN = 0xa800e000, LENGTH = 24K
+    SRAM                  (wx) : ORIGIN = 0xa800a000, LENGTH = 40K
     DCCM                  (wx) : ORIGIN = 0x80000000, LENGTH = 8K
     }
 
@@ -48,11 +48,11 @@ MEMORY
  * See below stack section for __stack_start and __firq_stack_start
  */
 __stack_size = 2048;
-__firq_stack_size = 512;
+__firq_stack_size = 1024;
 
 /* Minimum heap size to allocate
  * Actual heap size might be bigger due to page size alignment */
-__HEAP_SIZE_MIN = 8192;
+__HEAP_SIZE_MIN = 16384;
 /* This should be set to the page size used by the malloc implementation */
 __PAGE_SIZE = 4096;
 
@@ -113,6 +113,7 @@ SECTIONS
 	__data_ram_start = .;
 	*(.data)
 	*(".data.*")
+	. = ALIGN(4);
 	} > SRAM
 
     __data_ram_end = .;
diff --git a/variants/arduino_101/pins_arduino.h b/variants/arduino_101/pins_arduino.h
index e447277c..8be94f07 100644
--- a/variants/arduino_101/pins_arduino.h
+++ b/variants/arduino_101/pins_arduino.h
@@ -25,7 +25,7 @@
 #ifndef Pins_Arduino_h
 #define Pins_Arduino_h
 
-#define NUM_DIGITAL_PINS            29
+#define NUM_DIGITAL_PINS            32
 #define NUM_ANALOG_INPUTS           6
 #define NUM_PWM						4
 #define NUM_UARTS					1
diff --git a/variants/arduino_101/variant.cpp b/variants/arduino_101/variant.cpp
index d9b0bf05..ebcf884c 100644
--- a/variants/arduino_101/variant.cpp
+++ b/variants/arduino_101/variant.cpp
@@ -21,6 +21,12 @@
 
 #include "cfw_platform.h"
 #include "platform.h"
+#include "mailbox.h"
+
+// Add for debug corelib
+#ifdef CONFIGURE_DEBUG_CORELIB_ENABLED
+#include <infra/log.h>
+#endif
 
 
 /*
@@ -82,17 +88,23 @@ PinDescription g_APinDescription[]=
     {  5,   SS_GPIO_8B0,  SS_GPIO,   SS_GPIO_8B0_BASE_ADDR, 13,     GPIO_MUX_MODE, INVALID, INVALID,              13, INPUT_MODE }, // Arduino IO17
     {  6,   SS_GPIO_8B0,  SS_GPIO,   SS_GPIO_8B0_BASE_ADDR, 14,     GPIO_MUX_MODE, INVALID, INVALID,              14, INPUT_MODE }, // Arduino IO18
     {  1,   SS_GPIO_8B0,  SS_GPIO,   SS_GPIO_8B0_BASE_ADDR,  9,     GPIO_MUX_MODE, INVALID, INVALID,               9, INPUT_MODE }, // Arduino IO19
-	  {  0,   SS_GPIO_8B0,  SS_GPIO,   SS_GPIO_8B0_BASE_ADDR,  8,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO20
+    {  0,   SS_GPIO_8B0,  SS_GPIO,   SS_GPIO_8B0_BASE_ADDR,  8,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO20
     { 24,   SOC_GPIO_32,  SOC_GPIO,  SOC_GPIO_BASE_ADDR,    58,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO21
     { 12,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_BASE_ADDR,    46,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO22
     { 13,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_BASE_ADDR,    47,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO23    
     { 14,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_BASE_ADDR,    48,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO24
     { 26,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_BASE_ADDR,    60,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO25
-    {  1,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 1,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO26
-    {  2,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 2,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO27 
-    {  3,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 3,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO28
+    {  0,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 0,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO26 *soft reset button
+    {  1,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 1,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO27
+    {  2,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 2,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO28 
+    {  3,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 3,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO29
+    {  4,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 4,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO30 *imu_int
+    {  5,   SOC_GPIO_32, SOC_GPIO,   SOC_GPIO_AON_BASE_ADDR, 5,     GPIO_MUX_MODE, INVALID, INVALID,         INVALID, INPUT_MODE }, // Arduino IO31 *ble_int
 } ;
 
+uint32_t pwmPeriod[] = {PWM_PERIOD, PWM_PERIOD/2, PWM_PERIOD/2, PWM_PERIOD};
+
+uint8_t pinmuxMode[];
 #ifdef __cplusplus
 }
 #endif
@@ -167,6 +179,8 @@ void variantGpioInit(void)
     for (uint8_t pin = 0; pin < NUM_DIGITAL_PINS; pin++) {
         PinDescription *p = &g_APinDescription[pin];
         SET_PIN_MODE(p->ulSocPin, p->ulPinMode);
+        pinmuxMode[pin] = GPIO_MUX_MODE;
+        pinMode(pin, INPUT);
     }
 }
 
@@ -234,8 +248,21 @@ void initVariant( void )
     variantGpioInit();
     variantPwmInit();
     variantAdcInit();
-
+    
+    //set RTC clock divider to 32768(1 Hz)
+    *SYS_CLK_CTL |= RTC_DIV_1HZ_MASK;
+    *SYS_CLK_CTL &= ~(1 << CCU_RTC_CLK_DIV_EN);
+    *SYS_CLK_CTL |= 1 << CCU_RTC_CLK_DIV_EN;
+    
     cfw_platform_init();
+
+    /* Ready the mailbox for use (but don't initialise HW; x86 does that) */
+    mailbox_init(false);
+    
+    // Add for debug corelib
+    #ifdef CONFIGURE_DEBUG_CORELIB_ENABLED
+    log_init();
+    #endif
 }
 
 #ifdef __cplusplus
diff --git a/variants/arduino_101/variant.h b/variants/arduino_101/variant.h
index ed53a179..2736252f 100644
--- a/variants/arduino_101/variant.h
+++ b/variants/arduino_101/variant.h
@@ -113,6 +113,12 @@ extern "C"{
  * ADC
  */
 
+/*
+ * ONBOARD SPI FLASH
+ */
+#define ONBOARD_FLASH_SPI_PORT  SPI1
+#define ONBOARD_FLASH_CS_PIN    21
+
 /* EAI ADC device registers */
 #define ADC_SET                    (0x80015000)
 #define ADC_DIVSEQSTAT             (0x80015001)
@@ -146,6 +152,14 @@ extern "C"{
 #define ADC_RESOLUTION               12
 #define ADC_CLOCK_GATE             (1 << 31)
 
+/*
+ * Clocking
+ */
+ 
+#define SYS_CLK_CTL         (volatile int*)0xB0800038
+#define CCU_RTC_CLK_DIV_EN  2
+#define RTC_DIV_1HZ_MASK    0x00000078         
+
 #define digitalPinToBitMask(P)     (1 << g_APinDescription[P].ulGPIOId)
 
 //static uint8_t __unused_var_POR;

<!DOCTYPE html PUBLIC '-//W3C//DTD XHTML 1.0 Transitional//EN' 'http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd'>
<html xmlns='http://www.w3.org/1999/xhtml'>
<head>
<title>pFad - Phonifier reborn</title>
<meta http-equiv='Content-Type' content='text/html; charset=utf-8' />
</head>
<body>
<h1>Pfad - The Proxy pFad of &#169; 2024 Garber Painting. All rights reserved.</h1>


<!-- Disclaimer -->
<p>Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.</p>
<br>
<p>Alternative Proxies:</p><p><a href="http://rainy.clevelandohioweatherforecast.com/php-proxy/index.php?q=https://github.com/arduino/ArduinoCore-arc32/compare/arduino:77d9086...arduino:9f2e5e1.diff" target="_blank">Alternative Proxy</a></p><p><a href="http://rainy.clevelandohioweatherforecast.com/pFad/index.php?u=https://github.com/arduino/ArduinoCore-arc32/compare/arduino:77d9086...arduino:9f2e5e1.diff" target="_blank">pFad Proxy</a></p><p><a href="http://rainy.clevelandohioweatherforecast.com/pFad/v3index.php?u=https://github.com/arduino/ArduinoCore-arc32/compare/arduino:77d9086...arduino:9f2e5e1.diff" target="_blank">pFad v3 Proxy</a></p><p><a href="http://rainy.clevelandohioweatherforecast.com/pFad/v4index.php?u=https://github.com/arduino/ArduinoCore-arc32/compare/arduino:77d9086...arduino:9f2e5e1.diff" target="_blank">pFad v4 Proxy</a></p></body>
</html>