My fork of X_NUCLEO_IDB0XA1

Fork of X_NUCLEO_IDB0XA1 by ST

Files at this revision

API Documentation at this revision

Comitter:
Vincent Coubard
Date:
Thu Sep 15 16:59:44 2016 +0100
Parent:
252:0c2cb16a7166
Parent:
305:3d978a7bffc9
Commit message:
Merge sync_with_github into the default branch to makes the online IDE happy.

Sync with 7c82dbe71630c69410de24d80a5a854feaf53729

Changed in this revision

source/bluenrg-hci/hci/hci.c Show diff for this revision Revisions of this file
source/bluenrg-hci/utils/gp_timer.c Show diff for this revision Revisions of this file
source/bluenrg-hci/utils/list.c Show diff for this revision Revisions of this file
source/bluenrg-hci/utils/osal.c Show diff for this revision Revisions of this file
source/platform/clock.c Show diff for this revision Revisions of this file
source/utils/Payload.cpp Show diff for this revision Revisions of this file
source/utils/Utils.cpp Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/clock.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/compiler.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/debug.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/gp_timer.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/hal.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/hal_types.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/hci.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/hci_const.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/link_layer.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/list.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/osal.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/bluenrg-hci/sm.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/utils/Payload.h Show diff for this revision Revisions of this file
x-nucleo-idb0xa1/utils/Utils.h Show diff for this revision Revisions of this file
diff -r 0c2cb16a7166 -r 3a7d9f923493 module.json
--- a/module.json	Mon Jun 27 15:51:20 2016 +0200
+++ b/module.json	Thu Sep 15 16:59:44 2016 +0100
@@ -1,6 +1,6 @@
 {
   "name": "x-nucleo-idb0xa1",
-  "version": "2.0.3",
+  "version": "2.1.0",
   "description": "ST driver for the mbed BLE API.",
   "keywords": [
     "expansion",
@@ -32,6 +32,6 @@
   ],
   "dependencies": {
     "mbed-drivers": ">=0.11.3",
-    "ble": "^2.5.0"
+    "ble": "^2.7.0"
   }
-}
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/BlueNRGDevice.cpp
--- a/source/BlueNRGDevice.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/BlueNRGDevice.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -16,7 +16,7 @@
 
 /**
   ******************************************************************************
-  * @file    BlueNRGDevice.cpp 
+  * @file    BlueNRGDevice.cpp
   * @author  STMicroelectronics
   * @brief   Implementation of BLEDeviceInstanceBase
   ******************************************************************************
@@ -30,27 +30,31 @@
   * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  */ 
-  
+  */
+
 /** @defgroup BlueNRGDevice
  *  @brief BlueNRG BLE_API Device Adaptation
  *  @{
  */
  
-#include "mbed.h"
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif
 #include "BlueNRGDevice.h"
 #include "BlueNRGGap.h"
 #include "BlueNRGGattServer.h"
 
 #include "btle.h"
-#include "Utils.h"
-#include "osal.h"
+#include "ble_utils.h"
+#include "ble_osal.h"
 
-#include "debug.h"
+#include "ble_debug.h"
 #include "stm32_bluenrg_ble.h"
 
 extern "C" {
-    #include "hci.h"
+    #include "ble_hci.h"
     #include "bluenrg_utils.h"
 }
 
@@ -97,14 +101,14 @@
                              PinName sck,
                              PinName cs,
                              PinName rst,
-                             PinName irq) : 
+                             PinName irq) :
 	isInitialized(false), spi_(mosi, miso, sck), nCS_(cs), rst_(rst), irq_(irq)
 {
     // Setup the spi for 8 bit data, low clock polarity,
     // 1-edge phase, with an 8MHz clock rate
     spi_.format(8, 0);
     spi_.frequency(8000000);
-	
+
     // Deselect the BlueNRG chip by keeping its nCS signal high
     nCS_ = 1;
 
@@ -154,7 +158,7 @@
 int BlueNRGDevice::updateFirmware(const uint8_t *fw_image, uint32_t fw_size)
 {
 	int status = program_device(fw_image, fw_size);
-	
+
 	return (status);
 }
 
@@ -181,19 +185,17 @@
         	callback.call(&context);
         	return BLE_ERROR_ALREADY_INITIALIZED;
     	}
-	
-	/* ToDo: Clear memory contents, reset the SD, etc. */
+
 	// Init the BlueNRG/BlueNRG-MS stack
-	// By default, we set the device GAP role to PERIPHERAL
-	btleInit(BlueNRGGap::getInstance().getIsSetAddress(), GAP_PERIPHERAL_ROLE_IDB04A1);
-	
+	btleInit();
+
 	isInitialized = true;
 	BLE::InitializationCompleteCallbackContext context = {
 	        BLE::Instance(instanceID),
 	        BLE_ERROR_NONE
 	};
 	callback.call(&context);
-    
+
 	return BLE_ERROR_NONE;
 }
 
@@ -220,19 +222,17 @@
 }
 
 /*!
-  @brief  Wait for any BLE Event like BLE Connection, Read Request etc.    
+  @brief  Wait for any BLE Event like BLE Connection, Read Request etc.
   @param[in] void
-  @returns    char *      
+  @returns    char *
 */
 void BlueNRGDevice::waitForEvent(void)
 {
 	bool must_return = false;
 
 	do {
-		BlueNRGGap::getInstance().Process();
-		
-		HCI_Process();
-		
+        bluenrgDeviceInstance.processEvents();
+
 		if(must_return) return;
 
 		__WFE(); /* it is recommended that SEVONPEND in the
@@ -241,8 +241,8 @@
 				       that conrol is given back to main loop before next WFE */
 	} while(true);
 
-} 
- 
+}
+
 /*!
     @brief  get GAP version
     @brief Get the BLE stack version information
@@ -259,15 +259,15 @@
 /*!
     @brief  get reference to GAP object
     @param[in] void
-    @returns    Gap&      
+    @returns    Gap&
 */
 /**************************************************************************/
-Gap        &BlueNRGDevice::getGap()        
+Gap        &BlueNRGDevice::getGap()
 {
     return BlueNRGGap::getInstance();
 }
 
-const Gap  &BlueNRGDevice::getGap() const        
+const Gap  &BlueNRGDevice::getGap() const
 {
     return BlueNRGGap::getInstance();
 }
@@ -276,10 +276,10 @@
 /*!
     @brief  get reference to GATT server object
     @param[in] void
-    @returns    GattServer&    
+    @returns    GattServer&
 */
 /**************************************************************************/
-GattServer &BlueNRGDevice::getGattServer() 
+GattServer &BlueNRGDevice::getGattServer()
 {
     return BlueNRGGattServer::getInstance();
 }
@@ -288,7 +288,7 @@
 {
     return BlueNRGGattServer::getInstance();
 }
- 
+
 /**************************************************************************/
 /*!
     @brief  shut down the BLE device
@@ -329,7 +329,7 @@
     return BLE_ERROR_NONE;
 
 }
-																							
+
 /**
  * @brief  Reads from BlueNRG SPI buffer and store data into local buffer.
  * @param  buffer   : Buffer where data from SPI are stored
@@ -342,7 +342,7 @@
   uint8_t len = 0;
   uint8_t char_ff = 0xff;
   volatile uint8_t read_char;
-	
+
 	uint8_t i = 0;
 	volatile uint8_t tmpreg;
 
@@ -351,38 +351,38 @@
 
   /* Select the chip */
   nCS_ = 0;
-	
-  /* Read the header */  
+
+  /* Read the header */
   for (i = 0; i < 5; i++)
-  { 
+  {
 		tmpreg = spi_.write(header_master[i]);
 		header_slave[i] = (uint8_t)(tmpreg);
-  } 
-	
+  }
+
   if (header_slave[0] == 0x02) {
     /* device is ready */
     byte_count = (header_slave[4]<<8)|header_slave[3];
-  
+
     if (byte_count > 0) {
-  
+
       /* avoid to read more data that size of the buffer */
       if (byte_count > buff_size){
         byte_count = buff_size;
       }
-  
+
       for (len = 0; len < byte_count; len++){
         read_char = spi_.write(char_ff);
 				buffer[len] = read_char;
       }
-    }    
+    }
   }
   /* Release CS line to deselect the chip */
   nCS_ = 1;
-	
+
   // Add a small delay to give time to the BlueNRG to set the IRQ pin low
   // to avoid a useless SPI read at the end of the transaction
   for(volatile int i = 0; i < 2; i++)__NOP();
-  
+
 #ifdef PRINT_CSV_FORMAT
   if (len > 0) {
     print_csv_time();
@@ -392,8 +392,8 @@
     PRINT_CSV("\n");
   }
 #endif
-  
-  return len;   
+
+  return len;
 }
 
 /**
@@ -406,11 +406,11 @@
  */
 int32_t BlueNRGDevice::spiWrite(uint8_t* data1,
 				uint8_t* data2, uint8_t Nb_bytes1, uint8_t Nb_bytes2)
-{  
+{
 	int32_t result = 0;
 	uint32_t i;
 	volatile uint8_t tmpreg;
-    
+
   unsigned char header_master[HEADER_SIZE] = {0x0a, 0x00, 0x00, 0x00, 0x00};
   unsigned char header_slave[HEADER_SIZE]  = {0xaa, 0x00, 0x00, 0x00, 0x00};
 
@@ -419,24 +419,24 @@
   /* CS reset */
   nCS_ = 0;
 
-  /* Exchange header */  
+  /* Exchange header */
   for (i = 0; i < 5; i++)
-  { 
+  {
 		tmpreg = spi_.write(header_master[i]);
 		header_slave[i] = tmpreg;
-  } 
-	
+  }
+
   if (header_slave[0] == 0x02) {
     /* SPI is ready */
     if (header_slave[1] >= (Nb_bytes1+Nb_bytes2)) {
-  
+
       /*  Buffer is big enough */
 			for (i = 0; i < Nb_bytes1; i++) {
 				spi_.write(*(data1 + i));
       }
       for (i = 0; i < Nb_bytes2; i++) {
 				spi_.write(*(data2 + i));
-      }			
+      }
     } else {
       /* Buffer is too small */
       result = -2;
@@ -445,13 +445,13 @@
     /* SPI is not ready */
     result = -1;
   }
-    
+
   /* Release CS line */
   //HAL_GPIO_WritePin(BNRG_SPI_CS_PORT, BNRG_SPI_CS_PIN, GPIO_PIN_SET);
   nCS_ = 1;
-			
+
   enable_irq();
-    
+
   return result;
 }
 
@@ -469,3 +469,7 @@
 {
     irq_.enable_irq();
 }
+
+void BlueNRGDevice::processEvents() {
+    btle_handler();
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/BlueNRGDiscoveredCharacteristic.cpp
--- a/source/BlueNRGDiscoveredCharacteristic.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/BlueNRGDiscoveredCharacteristic.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -62,3 +62,7 @@
     props._indicate        = propsIn.indicate();
     props._authSignedWrite = propsIn.authSignedWrite();
 }
+
+ void BlueNRGDiscoveredCharacteristic::setLastHandle(GattAttribute::Handle_t  lastHandleIn) {
+     lastHandle = lastHandleIn;
+ }
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/BlueNRGGap.cpp
--- a/source/BlueNRGGap.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/BlueNRGGap.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -17,7 +17,7 @@
 
 /**
   ******************************************************************************
-  * @file    BlueNRGGap.cpp 
+  * @file    BlueNRGGap.cpp
   * @author  STMicroelectronics
   * @brief   Implementation of BLE_API Gap Class
   ******************************************************************************
@@ -39,20 +39,20 @@
  */
 
 #include "BlueNRGDevice.h"
-#include "mbed.h"
-#include "Payload.h"
-#include "Utils.h"
-#include "debug.h"
-
-//Local Variables
-//const char *local_name = NULL;
-//uint8_t local_name_length = 0;
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif
+#include "ble_payload.h"
+#include "ble_utils.h"
+#include "ble_debug.h"
 
 /*
  * Utility to process GAP specific events (e.g., Advertising timeout)
  */
 void BlueNRGGap::Process(void)
-{    
+{
     if(AdvToFlag) {
         stopAdvertising();
     }
@@ -61,7 +61,7 @@
 
 /**************************************************************************/
 /*!
-    @brief  Sets the advertising parameters and payload for the device. 
+    @brief  Sets the advertising parameters and payload for the device.
             Note: Some data types give error when their adv data is updated using aci_gap_update_adv_data() API
 
     @params[in] advData
@@ -95,7 +95,7 @@
 */
 /**************************************************************************/
 ble_error_t BlueNRGGap::setAdvertisingData(const GapAdvertisingData &advData, const GapAdvertisingData &scanResponse)
-{ 
+{
     PRINTF("BlueNRGGap::setAdvertisingData\n\r");
     /* Make sure we don't exceed the advertising payload length */
     if (advData.getPayloadLen() > GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
@@ -104,99 +104,28 @@
     }
 
     /* Make sure we have a payload! */
-    if (advData.getPayloadLen() == 0) {
-        PRINTF("advData.getPayloadLen() == 0\n\r");
-        //return BLE_ERROR_PARAM_OUT_OF_RANGE;
-        local_name_length = 0;
-        txPowLevSet = 0;
-        servUuidlength = 0;
-        AdvLen = 0;
-    } else {
+    if (advData.getPayloadLen() != 0) {
         PayloadPtr loadPtr(advData.getPayload(), advData.getPayloadLen());
 
-        for(uint8_t index=0; index<loadPtr.getPayloadUnitCount(); index++) {                  
+        /* Align the GAP Service Appearance Char value coherently
+           This setting is duplicate (see below GapAdvertisingData::APPEARANCE)
+           since BLE API has an overloaded function for appearance
+        */
+        STORE_LE_16(deviceAppearance, advData.getAppearance());
+        setAppearance((GapAdvertisingData::Appearance)(deviceAppearance[1]<<8|deviceAppearance[0]));
+
+
+        for(uint8_t index=0; index<loadPtr.getPayloadUnitCount(); index++) {
             loadPtr.getUnitAtIndex(index);
 
             PRINTF("adData[%d].length=%d\n\r", index,(uint8_t)(*loadPtr.getUnitAtIndex(index).getLenPtr()));
-            PRINTF("adData[%d].AdType=0x%x\n\r", index,(uint8_t)(*loadPtr.getUnitAtIndex(index).getAdTypePtr()));                  
-            
+            PRINTF("adData[%d].AdType=0x%x\n\r", index,(uint8_t)(*loadPtr.getUnitAtIndex(index).getAdTypePtr()));
+
             switch(*loadPtr.getUnitAtIndex(index).getAdTypePtr()) {
-            case GapAdvertisingData::FLAGS:                              /* ref *Flags */                     
-                {
-                PRINTF("Advertising type: FLAGS\n\r");
-                //Check if Flags are OK. BlueNRG only supports LE Mode.
-                uint8_t *flags = loadPtr.getUnitAtIndex(index).getDataPtr();
-                if((*flags & GapAdvertisingData::BREDR_NOT_SUPPORTED) != GapAdvertisingData::BREDR_NOT_SUPPORTED) {
-                    PRINTF("BlueNRG does not support BR/EDR Mode");
-                    return BLE_ERROR_PARAM_OUT_OF_RANGE;
-                }
-                
-                break;
-                }
-            case GapAdvertisingData::INCOMPLETE_LIST_16BIT_SERVICE_IDS:  /**< Incomplete list of 16-bit Service IDs */
-            case GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS:    /**< Complete list of 16-bit Service IDs */
-            case GapAdvertisingData::INCOMPLETE_LIST_128BIT_SERVICE_IDS: /**< Incomplete list of 128-bit Service IDs */
-            case GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS:   /**< Complete list of 128-bit Service IDs */
+            /**< TX Power Level (in dBm) */
+            case GapAdvertisingData::TX_POWER_LEVEL:
                 {
-                PRINTF("Advertising type: INCOMPLETE_LIST SERVICE_IDS/COMPLETE_LIST SERVICE_IDS\n\r");
-                
-                uint8_t buffSize = *loadPtr.getUnitAtIndex(index).getLenPtr()-1;
-                // The total lenght should include the Data Type Value
-                if(buffSize>UUID_BUFFER_SIZE-1) {
-                    return BLE_ERROR_INVALID_PARAM;
-                }
-                
-                servUuidlength = buffSize+1; // +1 to include the Data Type Value
-                servUuidData[0] = (uint8_t)(*loadPtr.getUnitAtIndex(index).getAdTypePtr()); //Data Type Value
-                
-                PRINTF("servUuidlength=%d servUuidData[0]=%d buffSize=%d\n\r", servUuidlength, servUuidData[0], buffSize);
-                // Save the Service UUID list just after the Data Type Value field
-                memcpy(servUuidData+1, loadPtr.getUnitAtIndex(index).getDataPtr(), buffSize);
-#ifdef DEBUG
-                for(unsigned i=0; i<servUuidlength; i++) {
-                    PRINTF("servUuidData[%d] = 0x%x\n\r", i, servUuidData[i]);
-                }
-                
-                for(unsigned i=0; i<buffSize; i++) {
-                    PRINTF("loadPtr.getUnitAtIndex(index).getDataPtr()[%d] = 0x%x\n\r",
-                            i, loadPtr.getUnitAtIndex(index).getDataPtr()[i]);
-                }
-#endif /* DEBUG */
-                break;
-                }
-            case GapAdvertisingData::INCOMPLETE_LIST_32BIT_SERVICE_IDS:  /**< Incomplete list of 32-bit Service IDs (not relevant for Bluetooth 4.0) */
-                {
-                PRINTF("Advertising type: INCOMPLETE_LIST_32BIT_SERVICE_IDS\n\r");
-                return BLE_ERROR_NOT_IMPLEMENTED;
-                }
-            case GapAdvertisingData::COMPLETE_LIST_32BIT_SERVICE_IDS:    /**< Complete list of 32-bit Service IDs (not relevant for Bluetooth 4.0) */
-                {
-                PRINTF("Advertising type: COMPLETE_LIST_32BIT_SERVICE_IDS\n\r");
-                return BLE_ERROR_NOT_IMPLEMENTED;
-                }
-            case GapAdvertisingData::SHORTENED_LOCAL_NAME:               /**< Shortened Local Name */
-                {
-                break;
-                }
-            case GapAdvertisingData::COMPLETE_LOCAL_NAME:                /**< Complete Local Name */
-                {
-                PRINTF("Advertising type: COMPLETE_LOCAL_NAME\n\r");
-                loadPtr.getUnitAtIndex(index).printDataAsString();
-                loadPtr.getUnitAtIndex(index).printDataAsHex();
-                local_name_length = *loadPtr.getUnitAtIndex(index).getLenPtr()-1;
-                // The total length should include the Data Type Value
-                if(local_name_length>ADV_DATA_MAX_SIZE-1) {
-                    return BLE_ERROR_INVALID_PARAM;
-                }
-                local_name[0] = (uint8_t)(*loadPtr.getUnitAtIndex(index).getAdTypePtr()); //Data Type Value
-                memcpy(local_name+1, (uint8_t*)loadPtr.getUnitAtIndex(index).getDataPtr(), local_name_length-1);
-                PRINTF("Advertising type: COMPLETE_LOCAL_NAME local_name=%s local_name_length=%d\n\r", local_name+1, local_name_length);
-
-                break;
-                }
-            case GapAdvertisingData::TX_POWER_LEVEL:                     /**< TX Power Level (in dBm) */
-                {
-                PRINTF("Advertising type: TX_POWER_LEVEL\n\r");     
+                PRINTF("Advertising type: TX_POWER_LEVEL\n\r");
                 int8_t enHighPower = 0;
                 int8_t paLevel = 0;
 
@@ -208,114 +137,55 @@
 #endif
                 if(ret == BLE_STATUS_SUCCESS) {
                   aci_hal_set_tx_power_level(enHighPower, paLevel);
-                  txPowLevSet = 1;
                 }
                 break;
                 }
-            case GapAdvertisingData::DEVICE_ID:                          /**< Device ID */
-                {
-                break;
-                }
-            case GapAdvertisingData::SLAVE_CONNECTION_INTERVAL_RANGE:    /**< Slave :Connection Interval Range */
-                {
-                PRINTF("Advertising type: SLAVE_CONNECTION_INTERVAL_RANGE\n\r");
-                uint8_t *ptr = loadPtr.getUnitAtIndex(index).getDataPtr();
-                slaveConnIntervMin = ptr[0]|ptr[1]<<8;
-                slaveConnIntervMax = ptr[2]|ptr[3]<<8;
-
-                break;
-                }
-            case GapAdvertisingData::SERVICE_DATA:                       /**< Service Data */
+            /**< Appearance */
+            case GapAdvertisingData::APPEARANCE:
                 {
-                PRINTF("Advertising type: SERVICE_DATA\n\r");
-                uint8_t buffSize = *loadPtr.getUnitAtIndex(index).getLenPtr()-1;
-                PRINTF("Advertising type: SERVICE_DATA (buffSize=%d)\n\r", buffSize);
-                // the total ADV DATA LEN should include two more bytes: the buffer size byte; and the Service Data Type Value byte
-                if(buffSize>ADV_DATA_MAX_SIZE-2) {
-                    return BLE_ERROR_PARAM_OUT_OF_RANGE;
-                }
-#ifdef DEBUG
-                for(int i=0; i<buffSize+1; i++) {
-                    PRINTF("Advertising type: SERVICE_DATA loadPtr.getUnitAtIndex(index).getDataPtr()[%d] = 0x%x\n\r",
-                            i, loadPtr.getUnitAtIndex(index).getDataPtr()[i]);
-                }
-#endif
-                // the total ADV DATA LEN should include two more bytes: the buffer size byte; and the Service Data Type Value byte
-                AdvData[AdvLen++] = buffSize+1; // the fisrt byte is the data buffer size (type+data)
-                AdvData[AdvLen++] = AD_TYPE_SERVICE_DATA;
-                memcpy(&AdvData[AdvLen], loadPtr.getUnitAtIndex(index).getDataPtr(), buffSize);
-                AdvLen += buffSize;
+                PRINTF("Advertising type: APPEARANCE\n\r");
+
+                GapAdvertisingData::Appearance appearanceP;
+                memcpy(deviceAppearance, loadPtr.getUnitAtIndex(index).getDataPtr(), 2);
+
+                PRINTF("input: deviceAppearance= 0x%x 0x%x\n\r", deviceAppearance[1], deviceAppearance[0]);
+
+                appearanceP = (GapAdvertisingData::Appearance)(deviceAppearance[1]<<8|deviceAppearance[0]);
+                /* Align the GAP Service Appearance Char value coherently */
+                setAppearance(appearanceP);
                 break;
                 }
 
-            case GapAdvertisingData::ADVERTISING_INTERVAL:               /**< Advertising Interval */
-                {
-                printf("Advertising type: ADVERTISING_INTERVAL\n\r");
-                uint8_t buffSize = *loadPtr.getUnitAtIndex(index).getLenPtr()-1;
-                AdvData[AdvLen++] = buffSize+1; // the fisrt byte is the data buffer size (type+data)
-                AdvData[AdvLen++] = AD_TYPE_ADVERTISING_INTERVAL;
-                memcpy(&AdvData[AdvLen], loadPtr.getUnitAtIndex(index).getDataPtr(), buffSize);
-                AdvLen += buffSize;
-                break;
-                }
-            case GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA:        /**< Manufacturer Specific Data */                             
-                {
-                PRINTF("Advertising type: MANUFACTURER_SPECIFIC_DATA\n\r");
-                uint8_t buffSize = *loadPtr.getUnitAtIndex(index).getLenPtr()-1;
-                PRINTF("Advertising type: MANUFACTURER_SPECIFIC_DATA (buffSize=%d)\n\r", buffSize);
-                // the total ADV DATA LEN should include two more bytes:
-		// the buffer size byte;
-		// and the Manufacturer Specific Data Type Value byte
-                if(buffSize>ADV_DATA_MAX_SIZE-2) {
-                    return BLE_ERROR_PARAM_OUT_OF_RANGE;
-                }
-#ifdef DBEUG
-                for(int i=0; i<buffSize+1; i++) {
-                    PRINTF("Advertising type: MANUFACTURER_SPECIFIC_DATA loadPtr.getUnitAtIndex(index).getDataPtr()[%d] = 0x%x\n\r",
-				i, loadPtr.getUnitAtIndex(index).getDataPtr()[i]);
-                }
-#endif
-                // the total ADV DATA LEN should include two more bytes: the buffer size byte; and the Manufacturer Specific Data Type Value byte
-                AdvData[AdvLen++] = buffSize+1; // the fisrt byte is the data buffer size (type+data)
-                AdvData[AdvLen++] = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
-                memcpy(&AdvData[AdvLen], loadPtr.getUnitAtIndex(index).getDataPtr(), buffSize);
-                AdvLen += buffSize;
-                break;
-                }
             } // end switch
 
         } //end for
 
-        //Set the SCAN_RSP Payload
-        if(scanResponse.getPayloadLen() > 0) {
-          scan_response_payload = scanResponse.getPayload();
-          scan_rsp_length = scanResponse.getPayloadLen();
+    }
+
+    // update the advertising data in the shield if advertising is running
+    if (state.advertising == 1) {
+        tBleStatus ret = hci_le_set_scan_resp_data(scanResponse.getPayloadLen(), scanResponse.getPayload());
+
+        if(BLE_STATUS_SUCCESS != ret) {
+            PRINTF(" error while setting scan response data (ret=0x%x)\n", ret);
+            switch (ret) {
+                case BLE_STATUS_TIMEOUT:
+                    return BLE_STACK_BUSY;
+                default:
+                    return BLE_ERROR_UNSPECIFIED;
+            }
         }
 
-        /* Align the GAP Service Appearance Char value coherently */
-        STORE_LE_16(deviceAppearance, advData.getAppearance());
-        setAppearance((GapAdvertisingData::Appearance)(deviceAppearance[1]<<8|deviceAppearance[0]));
-
-        // Update the ADV data if we are already in ADV mode
-        if(AdvLen > 0 && state.advertising == 1) {
- 
-            tBleStatus ret = aci_gap_update_adv_data(AdvLen, AdvData);
-            if(BLE_STATUS_SUCCESS!=ret) {
-                PRINTF("error occurred while adding adv data (ret=0x%x)\n", ret);
-                switch (ret) {
-                  case BLE_STATUS_TIMEOUT:
-                    return BLE_STACK_BUSY;
-                  case ERR_INVALID_HCI_CMD_PARAMS:
-                  case BLE_STATUS_INVALID_PARAMS:
-                    return BLE_ERROR_INVALID_PARAM;
-                  case BLE_STATUS_FAILED:
-                    return BLE_ERROR_PARAM_OUT_OF_RANGE;
-                  default:
-                    return BLE_ERROR_UNSPECIFIED;
-                }
-            }
+        ret = hci_le_set_advertising_data(advData.getPayloadLen(), advData.getPayload());
+        if (ret) {
+            PRINTF("error while setting the payload\r\n");
+            return BLE_ERROR_UNSPECIFIED;
         }
     }
+
+    _advData = advData;
+    _scanResponse = scanResponse;
+
     return BLE_ERROR_NONE;
 }
 
@@ -333,40 +203,40 @@
 static void advTimeoutCB(void)
 {
     Gap::GapState_t state;
-    
+
     state = BlueNRGGap::getInstance().getState();
     if (state.advertising == 1) {
-        
+
         BlueNRGGap::getInstance().stopAdvertising();
-        
+
     }
 }
 #else
 static void advTimeoutCB(void)
 {
     Gap::GapState_t state;
-    
+
     state = BlueNRGGap::getInstance().getState();
     if (state.advertising == 1) {
-        
+
         BlueNRGGap::getInstance().setAdvToFlag();
-        
+
         Timeout t = BlueNRGGap::getInstance().getAdvTimeout();
         t.detach(); /* disable the callback from the timeout */
 
     }
 }
 #endif /* AST_FOR_MBED_OS */
-    
+
 /**************************************************************************/
 /*!
     @brief  Starts the BLE HW, initialising any services that were
             added before this function was called.
-    
+
     @param[in]  params
                 Basic advertising details, including the advertising
                 delay, timeout and how the device should be advertised
-                
+
     @note   All services must be added before calling this function!
 
     @returns    ble_error_t
@@ -385,7 +255,7 @@
 ble_error_t BlueNRGGap::startAdvertising(const GapAdvertisingParams &params)
 {
     tBleStatus ret;
-    ble_error_t rc;
+    int err;
 
     /* Make sure we support the advertising type */
     if (params.getAdvertisingType() == GapAdvertisingParams::ADV_CONNECTABLE_DIRECTED) {
@@ -454,8 +324,8 @@
         params.getAdvertisingType() == GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED) {
 
         /* set scan response data */
-        PRINTF(" setting scan response data (scan_rsp_length=%u)\n", scan_rsp_length);
-        ret = hci_le_set_scan_resp_data(scan_rsp_length, scan_response_payload);
+        PRINTF(" setting scan response data (_scanResponseLen=%u)\n", _scanResponse.getPayloadLen());
+        ret = hci_le_set_scan_resp_data(_scanResponse.getPayloadLen(), _scanResponse.getPayload());
 
         if(BLE_STATUS_SUCCESS!=ret) {
             PRINTF(" error while setting scan response data (ret=0x%x)\n", ret);
@@ -470,54 +340,42 @@
         hci_le_set_scan_resp_data(0, NULL);
     }
 
-    //advInterval = params.getIntervalInADVUnits();
     setAdvParameters();
     PRINTF("advInterval=%d advType=%d\n\r", advInterval, params.getAdvertisingType());
 
-    /* Setting discoverable mode */
-    ret = aci_gap_set_discoverable(params.getAdvertisingType(), // AdvType
-                                   advInterval,                 // AdvIntervMin
-                                   advInterval,                 // AdvIntervMax
-                                   addr_type,                   // OwnAddrType
-                                   advFilterPolicy,             // AdvFilterPolicy
-                                   local_name_length,           // LocalNameLen
-                                   (const char*)local_name,     // LocalName
-                                   servUuidlength,              // ServiceUUIDLen
-                                   servUuidData,                // ServiceUUIDList
-                                   slaveConnIntervMin,          // SlaveConnIntervMin
-                                   slaveConnIntervMax);         // SlaveConnIntervMax
+    err = hci_le_set_advertising_data(_advData.getPayloadLen(), _advData.getPayload());
 
-    
-    PRINTF("!!!setting discoverable (servUuidlength=0x%x)\n\r", servUuidlength);
-    if(BLE_STATUS_SUCCESS!=ret) {
-       PRINTF("error occurred while setting discoverable (ret=0x%x)\n\r", ret);
-       switch (ret) {
-         case BLE_STATUS_INVALID_PARAMS:
-         case ERR_INVALID_HCI_CMD_PARAMS:
-           return BLE_ERROR_INVALID_PARAM;
-         case ERR_COMMAND_DISALLOWED:
-           return BLE_ERROR_OPERATION_NOT_PERMITTED;
-         case ERR_UNSUPPORTED_FEATURE:
-           return BLE_ERROR_NOT_IMPLEMENTED;
-         case BLE_STATUS_TIMEOUT:
-           return BLE_STACK_BUSY;
-         default:
-           return BLE_ERROR_UNSPECIFIED;
-       }
+    if (err) {
+        PRINTF("error while setting the payload\r\n");
+        return BLE_ERROR_UNSPECIFIED;
     }
 
-    // Since AD_TYPE_TX_POWER_LEVEL has not been set by application, we delete it
-    if(!txPowLevSet) {
-      PRINTF("Deleting TX POW LEV\n");
-      aci_gap_delete_ad_type(AD_TYPE_TX_POWER_LEVEL);
-      txPowLevSet = 0;
+    tBDAddr dummy_addr = { 0 };
+    uint16_t advIntervalMin = advInterval == GapAdvertisingParams::GAP_ADV_PARAMS_INTERVAL_MAX ? advInterval - 1 : advInterval;
+    uint16_t advIntervalMax = advIntervalMin + 1;
+
+    err = hci_le_set_advertising_parameters(
+        advIntervalMin,
+        advIntervalMax,
+        params.getAdvertisingType(),
+    	addr_type,
+        0x00,
+        dummy_addr,
+        /* all channels */ 7,
+    	advFilterPolicy
+    );
+
+    if (err) {
+        PRINTF("impossible to set advertising parameters\n\r");
+        PRINTF("advInterval min: %u, advInterval max: %u\n\r", advInterval, advInterval + 1);
+        PRINTF("advType: %u, advFilterPolicy: %u\n\r", params.getAdvertisingType(), advFilterPolicy);
+        return BLE_ERROR_INVALID_PARAM;
     }
 
-    // Stop Advertising if an error occurs while updating ADV data
-    rc = updateAdvertisingData();
-    if(rc != BLE_ERROR_NONE) {
-      aci_gap_set_non_discoverable();
-      return rc;
+    err = hci_le_set_advertise_enable(0x01);
+    if (err) {
+        PRINTF("impossible to start advertising\n\r");
+        return BLE_ERROR_UNSPECIFIED;
     }
 
     state.advertising = 1;
@@ -533,76 +391,9 @@
     }
 
     return BLE_ERROR_NONE;
+
 }
 
-ble_error_t BlueNRGGap::updateAdvertisingData(void)
-{
-    tBleStatus ret;
-
-    // Before updating the ADV data, delete COMPLETE_LOCAL_NAME field
-    if(AdvLen > 0) {
-      if(local_name_length > 0) {
-        ret = aci_gap_delete_ad_type(AD_TYPE_COMPLETE_LOCAL_NAME);
-        if (BLE_STATUS_SUCCESS!=ret){
-          PRINTF("aci_gap_delete_ad_type failed return=%d\n", ret);
-          switch (ret) {
-            case BLE_STATUS_TIMEOUT:
-              return BLE_STACK_BUSY;
-            case ERR_COMMAND_DISALLOWED:
-              return BLE_ERROR_OPERATION_NOT_PERMITTED;
-            case ERR_INVALID_HCI_CMD_PARAMS:
-              return BLE_ERROR_INVALID_PARAM;
-            default:
-              return BLE_ERROR_UNSPECIFIED;
-          }
-        }
-      }
-
-      // ...and TX_POWER_LEVEL field to make the needed room in ADV payload
-      if(txPowLevSet) {
-        ret = aci_gap_delete_ad_type(AD_TYPE_TX_POWER_LEVEL);
-        if (BLE_STATUS_SUCCESS!=ret){
-          PRINTF("aci_gap_delete_ad_type failed return=%d\n", ret);
-          switch (ret) {
-            case BLE_STATUS_TIMEOUT:
-              return BLE_STACK_BUSY;
-            case ERR_COMMAND_DISALLOWED:
-              return BLE_ERROR_OPERATION_NOT_PERMITTED;
-            case ERR_INVALID_HCI_CMD_PARAMS:
-              return BLE_ERROR_INVALID_PARAM;
-            default:
-              return BLE_ERROR_UNSPECIFIED;
-          }
-        }
-      }
-
-      ret = aci_gap_update_adv_data(AdvLen, AdvData);
-      if(BLE_STATUS_SUCCESS!=ret) {
-          PRINTF("error occurred while adding adv data (ret=0x%x)\n\r", ret);
-          switch (ret) {
-            case BLE_STATUS_TIMEOUT:
-              return BLE_STACK_BUSY;
-            case ERR_INVALID_HCI_CMD_PARAMS:
-            case BLE_STATUS_INVALID_PARAMS:
-              return BLE_ERROR_INVALID_PARAM;
-            case BLE_STATUS_FAILED:
-              return BLE_ERROR_PARAM_OUT_OF_RANGE;
-            default:
-              return BLE_ERROR_UNSPECIFIED;
-          }
-      }
-
-    } // AdvLen>0
-
-    if(deviceAppearance != 0) {
-      uint8_t appearance[] = {3, AD_TYPE_APPEARANCE, deviceAppearance[0], deviceAppearance[1]};
-      // just ignore error code while setting appearance
-      aci_gap_update_adv_data(4, appearance);
-    }
-
-    return BLE_ERROR_NONE;
-
-}
 
 /**************************************************************************/
 /*!
@@ -622,28 +413,19 @@
 /**************************************************************************/
 ble_error_t BlueNRGGap::stopAdvertising(void)
 {
-    tBleStatus ret;
 
     if(state.advertising == 1) {
-        //Set non-discoverable to stop advertising
-        ret = aci_gap_set_non_discoverable();
-        
-        if (BLE_STATUS_SUCCESS!=ret){
-            PRINTF("Error in stopping advertisement (ret=0x%x)!!\n\r", ret) ;
-            switch (ret) {
-              case ERR_COMMAND_DISALLOWED:
-                return BLE_ERROR_OPERATION_NOT_PERMITTED;
-              case BLE_STATUS_TIMEOUT:
-                return BLE_STACK_BUSY;
-              default:
-                return BLE_ERROR_UNSPECIFIED;
-            }
+
+        int err = hci_le_set_advertise_enable(0);
+        if (err) {
+            return BLE_ERROR_OPERATION_NOT_PERMITTED;
         }
+
         PRINTF("Advertisement stopped!!\n\r") ;
         //Set GapState_t::advertising state
         state.advertising = 0;
     }
-    
+
     return BLE_ERROR_NONE;
 }
 
@@ -653,7 +435,7 @@
 
     @param[in]  reason
                 Disconnection Reason
-                
+
     @returns    ble_error_t
 
     @retval     BLE_ERROR_NONE
@@ -683,7 +465,7 @@
             return BLE_ERROR_UNSPECIFIED;
         }
     }
-    
+
     return BLE_ERROR_NONE;
 }
 
@@ -693,7 +475,7 @@
 
     @param[in]  reason
                 Disconnection Reason
-                
+
     @returns    ble_error_t
 
     @retval     BLE_ERROR_NONE
@@ -714,10 +496,10 @@
 /**************************************************************************/
 /*!
     @brief  Sets the 16-bit connection handle
-    
+
     @param[in]  conn_handle
                 Connection Handle which is set in the Gap Instance
-                
+
     @returns    void
 */
 /**************************************************************************/
@@ -729,9 +511,9 @@
 /**************************************************************************/
 /*!
     @brief  Gets the 16-bit connection handle
-    
+
     @param[in]  void
-                
+
     @returns    uint16_t
                 Connection Handle of the Gap Instance
 */
@@ -748,10 +530,10 @@
 
     @param[in]  type
                 Type of Address
-    
+
     @param[in]  address[6]
                 Value of the Address to be set
-                
+
     @returns    ble_error_t
 
     @section EXAMPLE
@@ -763,26 +545,47 @@
 /**************************************************************************/
 ble_error_t BlueNRGGap::setAddress(AddressType_t type, const BLEProtocol::AddressBytes_t address)
 {
-    tBleStatus ret;
-
     if (type > BLEProtocol::AddressType::RANDOM_PRIVATE_NON_RESOLVABLE) {
         return BLE_ERROR_PARAM_OUT_OF_RANGE;
     }
-    
-    addr_type = type;
 
-    // If Address Type is other than PUBLIC, the given Address is ignored
-    if(addr_type == BLEProtocol::AddressType::PUBLIC){
-        ret = aci_hal_write_config_data(CONFIG_DATA_PUBADDR_OFFSET,
-                                        CONFIG_DATA_PUBADDR_LEN,
-                                        address);
+    if(type == BLEProtocol::AddressType::PUBLIC){
+        tBleStatus ret = aci_hal_write_config_data(
+            CONFIG_DATA_PUBADDR_OFFSET,
+            CONFIG_DATA_PUBADDR_LEN,
+            address
+        );
         if(ret != BLE_STATUS_SUCCESS) {
             return BLE_ERROR_OPERATION_NOT_PERMITTED;
         }
+    } else if  (type == BLEProtocol::AddressType::RANDOM_STATIC) {
+        // ensure that the random static address is well formed
+        if ((address[5] & 0xC0) != 0xC0) {
+            return BLE_ERROR_PARAM_OUT_OF_RANGE;
+        }
+
+        // thanks to const correctness of the API ...
+        tBDAddr random_address = { 0 };
+        memcpy(random_address, address, sizeof(random_address));
+        int err = hci_le_set_random_address(random_address);
+        if (err) {
+            return BLE_ERROR_OPERATION_NOT_PERMITTED;
+        }
+
+        // It is not possible to get the bluetooth address when it is set
+        // store it locally in class data member
+        memcpy(bdaddr, address, sizeof(bdaddr));
     } else {
-        return BLE_ERROR_OPERATION_NOT_PERMITTED;
+        // FIXME random addresses are not supported yet
+        // BLEProtocol::AddressType::RANDOM_PRIVATE_NON_RESOLVABLE
+        // BLEProtocol::AddressType::RANDOM_PRIVATE_RESOLVABLE
+        return BLE_ERROR_NOT_IMPLEMENTED;
     }
-    
+
+    // if we're here then the address was correctly set
+    // commit it inside the addr_type
+    addr_type = type;
+    isSetAddress = true;
     return BLE_ERROR_NONE;
 }
 
@@ -790,7 +593,7 @@
 /*!
     @brief      Returns boolean if the address of the device has been set
                 or not
-                
+
     @returns    bool
 
     @section EXAMPLE
@@ -800,9 +603,9 @@
     @endcode
 */
 /**************************************************************************/
-bool BlueNRGGap::getIsSetAddress() 
+bool BlueNRGGap::getIsSetAddress()
 {
-    return isSetAddress;   
+    return isSetAddress;
 }
 
 /**************************************************************************/
@@ -818,24 +621,36 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::getAddress(AddressType_t *typeP, Address_t address) 
+ble_error_t BlueNRGGap::getAddress(BLEProtocol::AddressType_t *typeP, BLEProtocol::AddressBytes_t address)
 {
     uint8_t bdaddr[BDADDR_SIZE];
     uint8_t data_len_out;
 
-    if(typeP != NULL) {
-        *typeP = addr_type;
+    // precondition, check that pointers in input are valid
+    if (typeP == NULL || address == NULL) {
+        return BLE_ERROR_INVALID_PARAM;
     }
 
-    tBleStatus ret = aci_hal_read_config_data(CONFIG_DATA_RANDOM_ADDRESS_IDB05A1, BDADDR_SIZE, &data_len_out, bdaddr);
-    if(ret != BLE_STATUS_SUCCESS) {
-        return BLE_ERROR_UNSPECIFIED;
+    if (addr_type == BLEProtocol::AddressType::PUBLIC) {
+        tBleStatus ret = aci_hal_read_config_data(CONFIG_DATA_PUBADDR_OFFSET, BDADDR_SIZE, &data_len_out, bdaddr);
+        if(ret != BLE_STATUS_SUCCESS || data_len_out != BDADDR_SIZE) {
+            return BLE_ERROR_UNSPECIFIED;
+        }
+    } else if (addr_type == BLEProtocol::AddressType::RANDOM_STATIC) {
+        // FIXME hci_read_bd_addr and
+        // aci_hal_read_config_data CONFIG_DATA_RANDOM_ADDRESS_IDB05A1
+        // does not work, use the address stored in class data member
+        memcpy(bdaddr, this->bdaddr, sizeof(bdaddr));
+    } else {
+        // FIXME: should be implemented with privacy features
+        // BLEProtocol::AddressType::RANDOM_PRIVATE_NON_RESOLVABLE
+        // BLEProtocol::AddressType::RANDOM_PRIVATE_RESOLVABLE
+        return BLE_ERROR_NOT_IMPLEMENTED;
     }
 
-    if(address != NULL) {
-        memcpy(address, bdaddr, BDADDR_SIZE);
-    }
-        
+    *typeP = addr_type;
+    memcpy(address, bdaddr, BDADDR_SIZE);
+
     return BLE_ERROR_NONE;
 }
 
@@ -852,12 +667,39 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::getPreferredConnectionParams(ConnectionParams_t *params) 
+ble_error_t BlueNRGGap::getPreferredConnectionParams(ConnectionParams_t *params)
 {
-    /* avoid compiler warnings about unused variables */
-    (void)params;
+    static const size_t parameter_size = 2;
+
+    if (!g_preferred_connection_parameters_char_handle) {
+        return BLE_ERROR_OPERATION_NOT_PERMITTED;
+    }
+
+    // Peripheral preferred connection parameters are an array of 4 uint16_t
+    uint8_t parameters_packed[parameter_size * 4];
+    uint16_t bytes_read = 0;
 
-    return BLE_ERROR_NOT_IMPLEMENTED;
+    tBleStatus err = aci_gatt_read_handle_value(
+        g_preferred_connection_parameters_char_handle + BlueNRGGattServer::CHAR_VALUE_HANDLE,
+        sizeof(parameters_packed),
+        &bytes_read,
+        parameters_packed
+    );
+
+    PRINTF("getPreferredConnectionParams err=0x%02x (bytes_read=%u)\n\r", err, bytes_read);
+
+    // check that the read succeed and the result have the expected length
+    if (err || bytes_read != sizeof(parameters_packed)) {
+        return BLE_ERROR_UNSPECIFIED;
+    }
+
+    // memcpy field by field
+    memcpy(&params->minConnectionInterval, parameters_packed, parameter_size);
+    memcpy(&params->maxConnectionInterval, &parameters_packed[parameter_size], parameter_size);
+    memcpy(&params->slaveLatency, &parameters_packed[2 * parameter_size], parameter_size);
+    memcpy(&params->connectionSupervisionTimeout, &parameters_packed[3 * parameter_size], parameter_size);
+
+    return BLE_ERROR_NONE;
 }
 
 
@@ -874,12 +716,63 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::setPreferredConnectionParams(const ConnectionParams_t *params) 
+ble_error_t BlueNRGGap::setPreferredConnectionParams(const ConnectionParams_t *params)
 {
-    /* avoid compiler warnings about unused variables */
-    (void)params;
+    static const size_t parameter_size = 2;
+    uint8_t parameters_packed[parameter_size * 4];
+
+    // ensure that parameters are correct
+    // see BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part C]
+    // section 12.3 PERIPHERAL PREFERRED CONNECTION PARAMETERS CHARACTERISTIC
+    if (((0x0006 > params->minConnectionInterval) || (params->minConnectionInterval > 0x0C80)) &&
+        params->minConnectionInterval != 0xFFFF) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
+
+    if (((params->minConnectionInterval > params->maxConnectionInterval) || (params->maxConnectionInterval > 0x0C80)) &&
+        params->maxConnectionInterval != 0xFFFF) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
+
+    if (params->slaveLatency > 0x01F3) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
+
+    if (((0x000A > params->connectionSupervisionTimeout) || (params->connectionSupervisionTimeout > 0x0C80)) &&
+        params->connectionSupervisionTimeout != 0xFFFF) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
 
-    return BLE_ERROR_NOT_IMPLEMENTED;
+    // copy the parameters inside the byte array
+    memcpy(parameters_packed, &params->minConnectionInterval, parameter_size);
+    memcpy(&parameters_packed[parameter_size], &params->maxConnectionInterval, parameter_size);
+    memcpy(&parameters_packed[2 * parameter_size], &params->slaveLatency, parameter_size);
+    memcpy(&parameters_packed[3 * parameter_size], &params->connectionSupervisionTimeout, parameter_size);
+
+    tBleStatus err = aci_gatt_update_char_value(
+        g_gap_service_handle,
+        g_preferred_connection_parameters_char_handle,
+        /* offset */ 0,
+        sizeof(parameters_packed),
+        parameters_packed
+    );
+
+    if (err) {
+        PRINTF("setPreferredConnectionParams failed (err=0x%x)!!\n\r", err) ;
+        switch (err) {
+          case BLE_STATUS_INVALID_HANDLE:
+          case BLE_STATUS_INVALID_PARAMETER:
+            return BLE_ERROR_INVALID_PARAM;
+          case BLE_STATUS_INSUFFICIENT_RESOURCES:
+            return BLE_ERROR_NO_MEM;
+          case BLE_STATUS_TIMEOUT:
+            return BLE_STACK_BUSY;
+          default:
+            return BLE_ERROR_UNSPECIFIED;
+        }
+    }
+
+    return BLE_ERROR_NONE;
 }
 
 /**************************************************************************/
@@ -897,16 +790,43 @@
 /**************************************************************************/
 ble_error_t BlueNRGGap::updateConnectionParams(Handle_t handle, const ConnectionParams_t *params)
 {
-    /* avoid compiler warnings about unused variables */
-    (void) handle;
-    (void)params;
+    tBleStatus ret = BLE_STATUS_SUCCESS;
+
+    if(gapRole == Gap::CENTRAL) {
+        ret = aci_gap_start_connection_update(handle,
+                                              params->minConnectionInterval,
+                                              params->maxConnectionInterval,
+                                              params->slaveLatency,
+                                              params->connectionSupervisionTimeout,
+                                              CONN_L1, CONN_L2);
+    } else {
+        ret = aci_l2cap_connection_parameter_update_request(handle,
+                                                            params->minConnectionInterval,
+                                                            params->maxConnectionInterval,
+                                                            params->slaveLatency,
+                                                            params->connectionSupervisionTimeout);
+    }
+
+    if (BLE_STATUS_SUCCESS != ret){
+        PRINTF("updateConnectionParams failed (ret=0x%x)!!\n\r", ret) ;
+        switch (ret) {
+          case ERR_INVALID_HCI_CMD_PARAMS:
+          case BLE_STATUS_INVALID_PARAMETER:
+            return BLE_ERROR_INVALID_PARAM;
+          case ERR_COMMAND_DISALLOWED:
+          case BLE_STATUS_NOT_ALLOWED:
+            return BLE_ERROR_OPERATION_NOT_PERMITTED;
+          default:
+            return BLE_ERROR_UNSPECIFIED;
+        }
+    }
 
     return BLE_ERROR_NONE;
 }
 
 /**************************************************************************/
 /*!
-    @brief  Sets the Device Name Characteristic 
+    @brief  Sets the Device Name Characteristic
 
     @param[in]  deviceName
                 pointer to device name to be set
@@ -923,11 +843,11 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::setDeviceName(const uint8_t *deviceName) 
+ble_error_t BlueNRGGap::setDeviceName(const uint8_t *deviceName)
 {
     tBleStatus ret;
-    uint8_t nameLen = 0;     
-    
+    uint8_t nameLen = 0;
+
     nameLen = strlen((const char*)deviceName);
     PRINTF("DeviceName Size=%d\n\r", nameLen);
 
@@ -957,13 +877,13 @@
 
 /**************************************************************************/
 /*!
-    @brief  Gets the Device Name Characteristic 
+    @brief  Gets the Device Name Characteristic
 
     @param[in]  deviceName
-                pointer to device name                 
+                pointer to device name
 
     @param[in]  lengthP
-                pointer to device name length                
+                pointer to device name length
 
     @returns    ble_error_t
 
@@ -995,10 +915,10 @@
 
 /**************************************************************************/
 /*!
-    @brief  Sets the Device Appearance Characteristic 
+    @brief  Sets the Device Appearance Characteristic
 
     @param[in]  appearance
-                device appearance      
+                device appearance
 
     @returns    ble_error_t
 
@@ -1017,9 +937,9 @@
     tBleStatus ret;
     uint8_t deviceAppearance[2];
 
-    STORE_LE_16(deviceAppearance, appearance);                 
-    PRINTF("input: incoming = %d deviceAppearance= 0x%x 0x%x\n\r", appearance, deviceAppearance[1], deviceAppearance[0]);
-    
+    STORE_LE_16(deviceAppearance, appearance);
+    PRINTF("setAppearance= 0x%x 0x%x\n\r", deviceAppearance[1], deviceAppearance[0]);
+
     ret = aci_gatt_update_char_value(g_gap_service_handle,
                                      g_appearance_char_handle,
                                      0, 2, (uint8_t *)deviceAppearance);
@@ -1046,7 +966,7 @@
     @brief  Gets the Device Appearance Characteristic
 
     @param[in]  appearance
-                pointer to device appearance value      
+                pointer to device appearance value
 
     @returns    ble_error_t
 
@@ -1135,7 +1055,7 @@
       default:
         type = GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED;
       }
-    
+
       PRINTF("data_length=%d adv peerAddr[%02x %02x %02x %02x %02x %02x] \r\n",
              *data_length, addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
       if(!_connecting) {
@@ -1144,7 +1064,7 @@
       PRINTF("!!!After processAdvertisementReport\n\r");
     }
     break;
-    
+
   case DISCOVERY_COMPLETE:
     // The discovery is complete. If this is due to a stop scanning (i.e., the device
     // we are interested in has been found) and a connection has been requested
@@ -1173,7 +1093,7 @@
 
 ble_error_t BlueNRGGap::startRadioScan(const GapScanningParams &scanningParams)
 {
-  
+
   tBleStatus ret = BLE_STATUS_SUCCESS;
 
   // Stop ADV before scanning
@@ -1230,13 +1150,13 @@
 
   PRINTF("stopScan\n\r");
   ret = aci_gap_terminate_gap_procedure(GAP_OBSERVATION_PROC);
-  
+
   if (ret != BLE_STATUS_SUCCESS) {
     PRINTF("GAP Terminate Gap Procedure failed(ret=0x%x)\n", ret);
-    return BLE_ERROR_UNSPECIFIED; 
+    return BLE_ERROR_UNSPECIFIED;
   } else {
     PRINTF("Discovery Procedure Terminated\n");
-    return BLE_ERROR_NONE; 
+    return BLE_ERROR_NONE;
   }
 }
 
@@ -1250,7 +1170,7 @@
 ble_error_t BlueNRGGap::setTxPower(int8_t txPower)
 {
     tBleStatus ret;
-    
+
     int8_t enHighPower = 0;
     int8_t paLevel = 0;
 
@@ -1259,7 +1179,7 @@
         return BLE_ERROR_PARAM_OUT_OF_RANGE;
     }
 
-    PRINTF("enHighPower=%d, paLevel=%d\n\r", enHighPower, paLevel);                    
+    PRINTF("enHighPower=%d, paLevel=%d\n\r", enHighPower, paLevel);
     ret = aci_hal_set_tx_power_level(enHighPower, paLevel);
     if(ret!=BLE_STATUS_SUCCESS) {
       return BLE_ERROR_PARAM_OUT_OF_RANGE;
@@ -1272,7 +1192,7 @@
 /*!
     @brief  get permitted Tx power values
     @param[in] values pointer to pointer to permitted power values
-    @param[in] num number of values   
+    @param[in] num number of values
 */
 /**************************************************************************/
 void BlueNRGGap::getPermittedTxPowerValues(const int8_t **valueArrayPP, size_t *countP) {
@@ -1298,6 +1218,8 @@
   if(state.connected == 1) {
     advIntMS = (conn_min_interval*1.25)-GUARD_INT;
     advInterval = _advParams.MSEC_TO_ADVERTISEMENT_DURATION_UNITS(advIntMS);
+
+    PRINTF("conn_min_interval is equal to %u\r\n", conn_min_interval);
   } else {
     advInterval = _advParams.getIntervalInADVUnits();
   }
@@ -1365,8 +1287,8 @@
   setConnectionParameters();
 
   /*
-    Scan_Interval, Scan_Window, Peer_Address_Type, Peer_Address, Own_Address_Type, Conn_Interval_Min, 
-    Conn_Interval_Max, Conn_Latency, Supervision_Timeout, Conn_Len_Min, Conn_Len_Max    
+    Scan_Interval, Scan_Window, Peer_Address_Type, Peer_Address, Own_Address_Type, Conn_Interval_Min,
+    Conn_Interval_Max, Conn_Latency, Supervision_Timeout, Conn_Len_Min, Conn_Len_Max
   */
   ret = aci_gap_create_connection(scanInterval,
 				  scanWindow,
@@ -1377,7 +1299,7 @@
 				  SUPERV_TIMEOUT, CONN_L1, CONN_L1);
 
   //_connecting = false;
-  
+
   if (ret != BLE_STATUS_SUCCESS) {
     PRINTF("Error while starting connection (ret=0x%02X).\n\r", ret);
     return BLE_ERROR_UNSPECIFIED;
@@ -1395,7 +1317,12 @@
 {
   /* avoid compiler warnings about unused variables */
   (void)connectionParams;
-  (void)scanParams;
+
+  setScanParams(scanParams->getInterval(),
+                scanParams->getWindow(),
+                scanParams->getTimeout(),
+                scanParams->getActiveScanning()
+               );
 
   // Save the peer address
   for(int i=0; i<BDADDR_SIZE; i++) {
@@ -1411,7 +1338,7 @@
     PRINTF("Calling createConnection from connect()\n\r");
     return createConnection();
   }
-  
+
   return BLE_ERROR_NONE;
 }
 
@@ -1502,11 +1429,6 @@
     /* Clear derived class members */
     m_connectionHandle = BLE_CONN_HANDLE_INVALID;
 
-    memset(deviceAppearance, 0, sizeof(deviceAppearance));
-    memset(local_name, 0, LOCAL_NAME_MAX_SIZE);
-    memset(local_name, 0, UUID_BUFFER_SIZE);
-    memset(AdvData, 0, ADV_DATA_MAX_SIZE);
-
     /* Set the whitelist policy filter modes to IGNORE_WHITELIST */
     advertisingPolicyMode = Gap::ADV_POLICY_IGNORE_WHITELIST;
     scanningPolicyMode    = Gap::SCAN_POLICY_IGNORE_WHITELIST;
@@ -1514,3 +1436,12 @@
     return BLE_ERROR_NONE;
 }
 
+void BlueNRGGap::setConnectionInterval(uint16_t interval) {
+    conn_min_interval = interval;
+    conn_max_interval = interval;
+}
+
+void BlueNRGGap::setGapRole(Role_t role)
+{
+    gapRole = role;
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/BlueNRGGattClient.cpp
--- a/source/BlueNRGGattClient.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/BlueNRGGattClient.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -15,7 +15,7 @@
 */
 /**
   ******************************************************************************
-  * @file    BlueNRGGattServer.cpp 
+  * @file    BlueNRGGattServer.cpp
   * @author  STMicroelectronics
   * @brief   Implementation of BlueNRG BLE_API GattServer Class
   ******************************************************************************
@@ -29,17 +29,22 @@
   * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  */ 
- 
+  */
+
 /** @defgroup BlueNRGGATTClient
  *  @brief BlueNRG BLE_API GattClient Adaptation
  *  @{
  */
- 
+
 #include "BlueNRGGattClient.h"
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif 
 #include "BlueNRGGap.h"
-#include "Utils.h"
-#include "debug.h"
+#include "ble_utils.h"
+#include "ble_debug.h"
 
 static uint8_t props_mask[] = {
   0x01,
@@ -58,10 +63,10 @@
     _currentState = GATT_IDLE;
     return;
   }
-    
+
   // Service Discovery complete
 /*
-  if(_currentState != GATT_IDLE && 
+  if(_currentState != GATT_IDLE &&
      _currentState != GATT_DISCOVERY_TERMINATED &&
      _currentState != GATT_WRITE_CHAR &&
      _currentState != GATT_READ_CHAR) {
@@ -71,6 +76,13 @@
   }
 
   if(_currentState == GATT_CHAR_DESC_DISCOVERY) {
+      if(charDescTerminationCallback != NULL) {
+         CharacteristicDescriptorDiscovery::TerminationCallbackParams_t params = {
+                                   _characteristic,
+                                   BLE_ERROR_NONE
+         };
+         charDescTerminationCallback(&params);
+       }
     _currentState = GATT_IDLE;
   }
 
@@ -85,7 +97,7 @@
     _currentState = GATT_IDLE;
   }
 }
-                                                
+
 void BlueNRGGattClient::primaryServicesCB(Gap::Handle_t connectionHandle,
                                           uint8_t event_data_length,
                                           uint8_t attribute_data_length,
@@ -106,13 +118,13 @@
 
     // UUID Type
     if (attribute_data_length == 6) {
-      
+
       PRINTF("UUID_TYPE_16\n\r");
       uuid = attribute_data_list[offset+5]<<8|attribute_data_list[offset+4];
       PRINTF("S UUID-%X attrs[%u %u]\r\n", uuid.getShortUUID(), startHandle, endHandle);
-      
+
     } else {
-      
+
       PRINTF("UUID_TYPE_128\n\r");
       uuid.setupLong(attribute_data_list+offset+4, UUID::LSB);
 
@@ -124,17 +136,12 @@
       }
 #endif
       PRINTF(" attrs[%u %u]\r\n", startHandle, endHandle);
-      
+
     }
-    
+
     PRINTF("Setup serviceIndex = %d\n\r", _numServices);
     discoveredService[_numServices].setup(uuid, startHandle, endHandle);
-    
-    if(serviceDiscoveryCallback) {
-      if(_matchingServiceUUID == BLE_UUID_UNKNOWN || _matchingServiceUUID == discoveredService[_numServices].getUUID()) {
-        serviceDiscoveryCallback(&discoveredService[_numServices]);
-      }
-    }
+
     _numServices++;
 
     offset += attribute_data_length;
@@ -180,12 +187,9 @@
     }
 
     discoveredService[i].setup(uuid, startHandle, endHandle);
-    
-    if(serviceDiscoveryCallback) {
-      serviceDiscoveryCallback(&discoveredService[_numServices]);
-    }
+
     _numServices++;
-    
+
     offset += 4;
   }
 }
@@ -224,10 +228,10 @@
       PRINTF("\r\n");
 #endif
     }
-    
+
     // Properties
     DiscoveredCharacteristic::Properties_t p;
-    
+
     p._broadcast = (props_mask[0] & handle_value_pair[offset+2]);
     p._read = (props_mask[1] & handle_value_pair[offset+2])>>1;
     p._writeWoResp = (props_mask[2] & handle_value_pair[offset+2])>>2;
@@ -262,9 +266,14 @@
                                     valueHandle,
                                     lastHandle);
 
-    if(characteristicDiscoveryCallback) {
-      characteristicDiscoveryCallback(&discoveredChar[_numChars]);
+    if (_numChars != 0) {
+        discoveredChar[_numChars - 1].setLastHandle(declHandle - 1);
+
+        if(characteristicDiscoveryCallback) {
+          characteristicDiscoveryCallback(&discoveredChar[_numChars - 1]);
+        }
     }
+
     _numChars++;
 
     offset += handle_value_pair_length;
@@ -279,9 +288,9 @@
   // Charac Properties(1), Charac Value Handle(2), Charac UUID(2/16)
   GattAttribute::Handle_t declHandle, valueHandle, lastHandle;
   UUID uuid;
-  
+
   PRINTF("serviceCharByUUIDCB\n\r");
-  
+
   // UUID Type
   if (event_data_length == 7) {
     PRINTF("Char UUID_TYPE_16\n\r");
@@ -302,7 +311,7 @@
 
   // Properties
   DiscoveredCharacteristic::Properties_t p;
-  
+
   p._broadcast = (props_mask[0] & attr_value[0]);
   p._read = (props_mask[1] & attr_value[0])>>1;
   p._writeWoResp = (props_mask[2] & attr_value[0])>>2;
@@ -335,7 +344,7 @@
                                   declHandle,
                                   valueHandle,
                                   lastHandle);
-  
+
   if(characteristicDiscoveryCallback) {
     characteristicDiscoveryCallback(&discoveredChar[_numChars]);
   }
@@ -345,24 +354,37 @@
 ble_error_t BlueNRGGattClient::findServiceChars(Gap::Handle_t connectionHandle)
 {
   PRINTF("findServiceChars\n\r");
-  
+
   tBleStatus ret;
   uint8_t uuid_type = UUID_TYPE_16;
   uint8_t short_uuid[2];
   uint8_t *uuid = NULL;
-  
+
   DiscoveredService *service;
-  
+
+  // complete the discovery of the last characteristic of the previous service.
+  // Its last handle wasn't known before this point
+  // update the handle and call the characteristic discovery callback.
+  if (_servIndex != 0 && _numChars != 0) {
+      discoveredChar[_numChars - 1].setLastHandle(discoveredService[_servIndex - 1].getEndHandle());
+
+      if(characteristicDiscoveryCallback) {
+        characteristicDiscoveryCallback(&discoveredChar[_numChars - 1]);
+      }
+  }
+
+  _numChars = 0;
+
   // We finished chars discovery for all services
   if(_servIndex >= _numServices) {
     PRINTF("!!!We finished chars discovery for all services!!!\n\r");
     //_currentState = GATT_CHARS_DISCOVERY_COMPLETE;
-    
+
     terminateServiceDiscovery();
-    
+
     return BLE_ERROR_NONE;
   }
-    
+
   service = &discoveredService[_servIndex];
   /*
   if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
@@ -376,55 +398,59 @@
     PRINTF("\r\n");
   }
   */
-  
+
+  if(serviceDiscoveryCallback) {
+    serviceDiscoveryCallback(service);
+  }
+
   PRINTF("findServiceChars (_servIndex=%d)\n\r", _servIndex);
   //ret = aci_gatt_disc_all_charac_of_serv(connectionHandle, service->getStartHandle(), service->getEndHandle());
 
-  if(_matchingCharacteristicUUIDIn == BLE_UUID_UNKNOWN) {
-    PRINTF("findServiceChars (BLE_UUID_UNKNOWN)\n\r");
-    ret = aci_gatt_disc_all_charac_of_serv(connectionHandle, service->getStartHandle(), service->getEndHandle());
-  } else {
-    
-    uint8_t type = _matchingCharacteristicUUIDIn.shortOrLong();
-    
-    if(type == UUID::UUID_TYPE_SHORT) {
-      STORE_LE_16(short_uuid, _matchingCharacteristicUUIDIn.getShortUUID());
-      
-      uuid_type = UUID_TYPE_16;
-      uuid = short_uuid;
+    if(_matchingCharacteristicUUIDIn == BLE_UUID_UNKNOWN) {
+        PRINTF("findServiceChars (BLE_UUID_UNKNOWN)\n\r");
+        ret = aci_gatt_disc_all_charac_of_serv(connectionHandle, service->getStartHandle(), service->getEndHandle());
+    } else {
+
+        uint8_t type = _matchingCharacteristicUUIDIn.shortOrLong();
+
+        if(type == UUID::UUID_TYPE_SHORT) {
+            STORE_LE_16(short_uuid, _matchingCharacteristicUUIDIn.getShortUUID());
+
+            uuid_type = UUID_TYPE_16;
+            uuid = short_uuid;
 #ifdef DEBUG
-      PRINTF("findServiceChars C UUID-");
-      for(unsigned i = 0; i < 2; i++) {
-        PRINTF("%02X", short_uuid[i]);
-      }
-      PRINTF("\n\r");
+            PRINTF("findServiceChars C UUID-");
+            for(unsigned i = 0; i < 2; i++) {
+                PRINTF("%02X", short_uuid[i]);
+            }
+            PRINTF("\n\r");
 #endif
-    } else if(type==UUID::UUID_TYPE_LONG) {
-      
-      uuid_type = UUID_TYPE_128;
-      uuid = (unsigned char*)_matchingCharacteristicUUIDIn.getBaseUUID();
+        } else if(type==UUID::UUID_TYPE_LONG) {
+
+            uuid_type = UUID_TYPE_128;
+            uuid = (unsigned char*)_matchingCharacteristicUUIDIn.getBaseUUID();
 #ifdef DEBUG
-      PRINTF("(findServiceChars) C UUID-");
-      for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
-        PRINTF("%02X", uuid[i]);
-      }
-      PRINTF("\r\n");
+            PRINTF("(findServiceChars) C UUID-");
+            for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
+                PRINTF("%02X", uuid[i]);
+            }
+            PRINTF("\r\n");
 #endif
+        }
+
+        ret = aci_gatt_disc_charac_by_uuid(connectionHandle,
+                                           service->getStartHandle(),
+                                           service->getEndHandle(),
+                                           uuid_type,
+                                           uuid);
     }
 
-    ret = aci_gatt_disc_charac_by_uuid(connectionHandle,
-                                       service->getStartHandle(),
-                                       service->getEndHandle(),
-                                       uuid_type,
-                                       uuid);
-  }
-  
   if(ret == BLE_STATUS_SUCCESS) {
     _servIndex++;
   }
-  
+
   PRINTF("findServiceChars ret=%d\n\r", ret);
-  
+
   return BLE_ERROR_NONE;
 }
 
@@ -435,7 +461,7 @@
                                                       const UUID                                 &matchingCharacteristicUUIDIn)
 {
   PRINTF("launchServiceDiscovery\n\r");
-  
+
   tBleStatus ret;
   uint8_t uuid_type = UUID_TYPE_16;
   uint8_t short_uuid[2];
@@ -465,17 +491,17 @@
   for(j = 0; j < BLE_TOTAL_DISCOVERED_SERVICES; j++) {
     discoveredService[j].setup(BLE_UUID_UNKNOWN, GattAttribute::INVALID_HANDLE, GattAttribute::INVALID_HANDLE);
   }
-  
+
   if(matchingServiceUUID == BLE_UUID_UNKNOWN) {
-    
+
     // Wildcard: search for all services
     ret = aci_gatt_disc_all_prim_services((uint16_t)connectionHandle);
-    
+
   } else {
-  
+
     uint8_t type = matchingServiceUUID.shortOrLong();
     //PRINTF("AddService(): Type:%d\n\r", type);
-    
+
     if(type == UUID::UUID_TYPE_SHORT) {
       STORE_LE_16(short_uuid, matchingServiceUUID.getShortUUID());
 #ifdef DEBUG
@@ -485,10 +511,10 @@
       }
       PRINTF("\n\r");
 #endif
-      
+
       uuid_type = UUID_TYPE_16;
       uuid = short_uuid;
-      
+
     } else if(type==UUID::UUID_TYPE_LONG) {
 
       uuid_type = UUID_TYPE_128;
@@ -502,18 +528,18 @@
       PRINTF("\n\r");
 #endif
     }
-    
+
     // search for specific service by UUID
     ret = aci_gatt_disc_prim_service_by_uuid((uint16_t)connectionHandle, uuid_type, uuid);
     //ret = aci_gatt_disc_all_prim_services((uint16_t)connectionHandle);
   }
-    
+
   if(ret == BLE_STATUS_SUCCESS) {
     _currentState = GATT_SERVICE_DISCOVERY;
   }
-  
+
   PRINTF("launchServiceDiscovery ret=%d\n\r", ret);
-  
+
   return BLE_ERROR_NONE;
 }
 
@@ -557,7 +583,7 @@
          _currentState == GATT_WRITE_CHAR ) {
     return false;
   }
-  
+
   return true;
 */
 }
@@ -565,7 +591,7 @@
 void BlueNRGGattClient::terminateServiceDiscovery(void)
 {
   _currentState = GATT_IDLE;//GATT_DISCOVERY_TERMINATED;
-  
+
   if (terminationCallback) {
     terminationCallback(_connectionHandle);
   }
@@ -589,18 +615,18 @@
   (void)offset;
 
   tBleStatus ret;
-  
+
   BlueNRGGattClient *gattc = const_cast<BlueNRGGattClient*>(this);
-  
-  // Save the attribute_handle not provided by evt_att_read_resp    
+
+  // Save the attribute_handle not provided by evt_att_read_resp
   gattc->readCBParams.handle = attributeHandle;
-  
+
   // FIXME: We need to wait for a while before starting a read
   // due to BlueNRG process queue handling
   Clock_Wait(100);
 
   ret = aci_gatt_read_charac_val(connHandle, attributeHandle);
-  
+
   if(ret == BLE_STATUS_SUCCESS) {
     gattc->_currentState = GATT_READ_CHAR;
     return BLE_ERROR_NONE;
@@ -638,7 +664,7 @@
   (void)event_data_length;
 
   writeCBParams.connHandle = connHandle;
-  
+
   BlueNRGGattClient::getInstance().processWriteResponse(&writeCBParams);
 }
 
@@ -652,9 +678,9 @@
   (void)cmd;
 
   tBleStatus ret;
-  
+
   BlueNRGGattClient *gattc = const_cast<BlueNRGGattClient*>(this);
-  
+
   // We save the write response params (used by the callback) because
   // when the aci_gatt_write_charac_value() is used the only event received is the EVT_BLUE_GATT_PROCEDURE_COMPLETE
   gattc->writeCBParams.connHandle = connHandle;
@@ -663,10 +689,10 @@
   gattc->writeCBParams.offset = 0;
   gattc->writeCBParams.len = length;
   gattc->writeCBParams.data = value;
-  
+
   ret = aci_gatt_write_charac_value(connHandle, attributeHandle, length, const_cast<uint8_t *>(value));
   //ret = aci_gatt_write_charac_reliable(connHandle, attributeHandle, 0, length, const_cast<uint8_t *>(value));
-  
+
   if (ret == BLE_STATUS_SUCCESS) {
     gattc->_currentState = GATT_WRITE_CHAR;
     return BLE_ERROR_NONE;
@@ -693,21 +719,25 @@
     handle_uuid_length = 18; //Handle + UUID_128
 
   numCharacDesc = (event_data_length - 1) / handle_uuid_length;
-  
+
   offset = 0;
 
+  PRINTF("\r\ncharacteristic descriptor discovered: data length %u, format %u\r\n",
+    event_data_length, format);
+
+
   for (i=0; i<numCharacDesc; i++) {
-    attHandle = handle_uuid_pair[offset];
+    memcpy(&attHandle, handle_uuid_pair + offset, sizeof(attHandle));
 
     // UUID Type
     if (handle_uuid_length == 4) {
-      
+
       PRINTF("UUID_TYPE_16\n\r");
       uuid = handle_uuid_pair[offset+3]<<8|handle_uuid_pair[offset+2];
       PRINTF("D UUID-%X attHandle=%u\r\n", uuid.getShortUUID(), attHandle);
-      
+
     } else {
-      
+
       PRINTF("UUID_TYPE_128\n\r");
       uuid.setupLong(handle_uuid_pair+offset+2, UUID::LSB);
 #ifdef DEBUG
@@ -736,15 +766,6 @@
 
     offset += handle_uuid_length;
   }
-
-  if(charDescTerminationCallback != NULL) {
-     CharacteristicDescriptorDiscovery::TerminationCallbackParams_t params = {
-                               _characteristic,
-                               BLE_ERROR_NONE
-     };
-     charDescTerminationCallback(&params);
-   }
-
 }
 
 ble_error_t BlueNRGGattClient::discoverCharacteristicDescriptors(
@@ -765,7 +786,7 @@
   GattAttribute::Handle_t valueHandle = characteristic.getValueHandle();
   GattAttribute::Handle_t lastHandle = characteristic.getLastHandle();
 
-  PRINTF("Starting aci_gatt_disc_all_charac_descriptors...\n\r");
+  PRINTF("Starting aci_gatt_disc_all_charac_descriptors... [%u : %u]\n\r", valueHandle, lastHandle);
   ret = aci_gatt_disc_all_charac_descriptors(connHandle, valueHandle, lastHandle);
 
   if (ret == BLE_STATUS_SUCCESS) {
@@ -811,5 +832,4 @@
   memset(discoveredChar, 0, sizeof(discoveredChar));
 
   return BLE_ERROR_NONE;
-}
-
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/BlueNRGGattServer.cpp
--- a/source/BlueNRGGattServer.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/BlueNRGGattServer.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -15,7 +15,7 @@
 */
 /**
   ******************************************************************************
-  * @file    BlueNRGGattServer.cpp 
+  * @file    BlueNRGGattServer.cpp
   * @author  STMicroelectronics
   * @brief   Implementation of BlueNRG BLE_API GattServer Class
   ******************************************************************************
@@ -30,17 +30,21 @@
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
   */
- 
+
 /** @defgroup BlueNRGGATTSERVER
  *  @brief BlueNRG BLE_API GattServer Adaptation
  *  @{
  */
- 
+
 #include "BlueNRGGattServer.h"
-#include "mbed.h"
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif 
 #include "BlueNRGGap.h"
-#include "Utils.h"
-#include "debug.h"
+#include "ble_utils.h"
+#include "ble_debug.h"
 
 /**************************************************************************/
 /*!
@@ -48,7 +52,7 @@
 
     @params[in] service
                 Pointer to instance of the Gatt Server to add
-                
+
     @returns    ble_error_t
 
     @retval     BLE_ERROR_NONE
@@ -66,7 +70,7 @@
     /* ToDo: Make sure we don't overflow the array, etc. */
     /* ToDo: Make sure this service UUID doesn't already exist (?) */
     /* ToDo: Basic validation */
-    
+
     tBleStatus ret;
     uint8_t type;
     uint16_t short_uuid;
@@ -75,26 +79,33 @@
     uint8_t char_base_uuid[16];
     const uint8_t *base_uuid;
     const uint8_t *base_char_uuid;
-    
-    uint8_t charsCount = 0;
-    uint8_t maxAttrRecords = 0;
+
+    uint8_t charsCount = service.getCharacteristicCount();
+    const uint8_t available_characteristics = BLE_TOTAL_CHARACTERISTICS - characteristicCount;
+
+    // check that there is enough characteristics left in the
+    // characteristic array.
+    if (charsCount > available_characteristics) {
+        PRINTF("charCount = %u and characteristicCount = %u\r\n", charsCount, available_characteristics);
+        return BLE_ERROR_NO_MEM;
+    }
+
+    const uint16_t maxAttrRecords = computeAttributesRecord(service);
 
     type = (service.getUUID()).shortOrLong();
     PRINTF("AddService(): Type:%d\n\r", type);
-    
+
     /* Add the service to the BlueNRG */
     short_uuid = (service.getUUID()).getShortUUID();
     STORE_LE_16(primary_short_uuid, short_uuid);
-    
+
     if(type==UUID::UUID_TYPE_LONG) {
-        base_uuid = (service.getUUID()).getBaseUUID();   
-        
+        base_uuid = (service.getUUID()).getBaseUUID();
+
         COPY_UUID_128(primary_base_uuid, base_uuid[15],base_uuid[14],primary_short_uuid[1],primary_short_uuid[0],base_uuid[11],base_uuid[10],base_uuid[9],base_uuid[8],base_uuid[7],base_uuid[6],base_uuid[5],base_uuid[4],base_uuid[3],base_uuid[2],base_uuid[1],base_uuid[0]);
     }
 
-    charsCount = service.getCharacteristicCount();
-    //1(service record)+2records*char+1record*char_desc
-    maxAttrRecords = 1+3*charsCount;
+    ret = BLE_STATUS_SUCCESS;
 
     if(type==UUID::UUID_TYPE_SHORT) {
         ret = aci_gatt_add_serv(UUID_TYPE_16,
@@ -102,9 +113,9 @@
                                 PRIMARY_SERVICE,
                                 maxAttrRecords/*7*/,
                                 &servHandle);
-        PRINTF("aci_gatt_add_serv UUID_TYPE_LONG ret=%d\n\r", ret);
-    }
-    else if(type==UUID::UUID_TYPE_LONG) {
+        PRINTF("aci_gatt_add_serv UUID_TYPE_SHORT ret=%d\n\r", ret);
+
+    } else if(type==UUID::UUID_TYPE_LONG) {
         ret = aci_gatt_add_serv(UUID_TYPE_128,
                                 primary_base_uuid,
                                 PRIMARY_SERVICE,
@@ -112,12 +123,31 @@
                                 &servHandle);
         PRINTF("aci_gatt_add_serv UUID_TYPE_LONG ret=%d\n\r", ret);
     }
-    
+
+    switch (ret) {
+        case BLE_STATUS_SUCCESS:
+            break;
+
+        case BLE_STATUS_INVALID_PARAMETER:
+            return BLE_ERROR_INVALID_PARAM;
+
+        case BLE_STATUS_OUT_OF_HANDLE:
+        case BLE_STATUS_INSUFFICIENT_RESOURCES:
+        case ERR_UNSPECIFIED_ERROR:
+            return BLE_ERROR_NO_MEM;
+
+        case BLE_STATUS_ERROR:
+        default:
+            return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    }
+
+
+
     service.setHandle(servHandle);
     //serviceHandleVector.push_back(servHandle);
     PRINTF("added servHandle handle =%u\n\r", servHandle);
     uint16_t bleCharacteristic;
-    
+
     //iterate to include all characteristics
     for (uint8_t i = 0; i < charsCount; i++) {
         GattCharacteristic *p_char = service.getCharacteristic(i);
@@ -138,7 +168,7 @@
 #endif
             COPY_UUID_128(char_base_uuid,base_char_uuid[15],base_char_uuid[14],int_8_uuid[1],int_8_uuid[0],base_char_uuid[11],base_char_uuid[10],base_char_uuid[9],base_char_uuid[8],base_char_uuid[7],base_char_uuid[6],base_char_uuid[5],base_char_uuid[4],base_char_uuid[3],base_char_uuid[2],base_char_uuid[1],base_char_uuid[0]);
         }
-        
+
         PRINTF("Char Properties 0x%x\n\r", p_char->getProperties());
         /*
         * Gatt_Evt_Mask -> HardCoded (0)
@@ -151,13 +181,13 @@
                     (GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE|
                         GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE))) {
             PRINTF("Setting up Gatt GATT_NOTIFY_ATTRIBUTE_WRITE Mask\n\r");
-            Gatt_Evt_Mask = Gatt_Evt_Mask | GATT_NOTIFY_ATTRIBUTE_WRITE;
+            Gatt_Evt_Mask = Gatt_Evt_Mask | GATT_NOTIFY_ATTRIBUTE_WRITE  | GATT_NOTIFY_WRITE_REQ_AND_WAIT_FOR_APPL_RESP;
         }
         if((p_char->getProperties() &
                     (GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ|
                         GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY| GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE))) {
             PRINTF("Setting up Gatt GATT_NOTIFY_READ_REQ_AND_WAIT_FOR_APPL_RESP Mask\n\r");
-            Gatt_Evt_Mask = Gatt_Evt_Mask | GATT_NOTIFY_READ_REQ_AND_WAIT_FOR_APPL_RESP; 
+            Gatt_Evt_Mask = Gatt_Evt_Mask | GATT_NOTIFY_READ_REQ_AND_WAIT_FOR_APPL_RESP;
         }    //This will support also GATT_SERVER_ATTR_READ_WRITE since it will be covered by previous if() check.
 
         if(type==UUID::UUID_TYPE_SHORT) {
@@ -171,7 +201,7 @@
                                      16 /*Encryption_Key_Size*/,
                                      1 /*isVariable*/,
                                      &bleCharacteristic);
-            
+
             PRINTF("aci_gatt_add_char UUID_TYPE_16 props=%d MaxLength=%d ret=%d\n\r",
                     p_char->getProperties(), p_char->getValueAttribute().getMaxLength(), ret);
 
@@ -186,36 +216,85 @@
                                      16 /*Encryption_Key_Size*/,
                                      1 /*isVariable*/,
                                      &bleCharacteristic);
-            
+
             PRINTF("aci_gatt_add_char UUID_TYPE_128 props=%d MaxLength=%d ret=%d\n\r",
                     p_char->getProperties(), p_char->getValueAttribute().getMaxLength(), ret);
         }
-        
+
+        switch (ret) {
+            case BLE_STATUS_SUCCESS:
+                break;
+
+            case ERR_UNSPECIFIED_ERROR:
+            case BLE_STATUS_INSUFFICIENT_RESOURCES:
+            case BLE_STATUS_OUT_OF_HANDLE:
+                // TODO remove characteristics and the service previously added.
+                // remove service in the stack by using: Aci_Gatt_Del_Service
+                // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                // update service counter
+                // destroy registered characteristic and updat echaracteristic counter
+                return BLE_ERROR_NO_MEM;
+
+            case BLE_STATUS_INVALID_HANDLE:
+            case BLE_STATUS_INVALID_PARAMETER:
+            case BLE_STATUS_CHARAC_ALREADY_EXISTS:
+            // TODO remove characteristics and the service previously added.
+            // remove service in the stack by using: Aci_Gatt_Del_Service
+            // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+            // update service counter
+            // destroy registered characteristic and updat echaracteristic counter
+                return BLE_ERROR_INVALID_PARAM;
+
+            case BLE_STATUS_ERROR:
+            default:
+            // TODO remove characteristics and the service previously added.
+            // remove service in the stack by using: Aci_Gatt_Del_Service
+            // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+            // update service counter
+            // destroy registered characteristic and updat echaracteristic counter
+                return BLE_ERROR_INTERNAL_STACK_FAILURE;
+        }
+
         bleCharHandleMap.insert(std::pair<uint16_t, uint16_t>(bleCharacteristic, servHandle));
-        
+
         p_characteristics[characteristicCount++] = p_char;
         /* Set the characteristic value handle */
         p_char->getValueAttribute().setHandle(bleCharacteristic+BlueNRGGattServer::CHAR_VALUE_HANDLE);
         PRINTF("added bleCharacteristic (value handle =%u)\n\r", p_char->getValueAttribute().getHandle());
 
         if ((p_char->getValueAttribute().getValuePtr() != NULL) && (p_char->getValueAttribute().getLength() > 0)) {
-            write(p_char->getValueAttribute().getHandle(),
+            ble_error_t err = write(p_char->getValueAttribute().getHandle(),
                   p_char->getValueAttribute().getValuePtr(),
                   p_char->getValueAttribute().getLength(), false /* localOnly */);
+            if (err) {
+                PRINTF("ERROR HERE !!!!\r\n");
+                return err;
+            }
         }
 
         // add descriptors now
         uint16_t descHandle = 0;
         PRINTF("p_char->getDescriptorCount()=%d\n\r", p_char->getDescriptorCount());
-        
+
         for(uint8_t descIndex=0; descIndex<p_char->getDescriptorCount(); descIndex++) {
             GattAttribute *descriptor = p_char->getDescriptor(descIndex);
-            uint16_t shortUUID = descriptor->getUUID().getShortUUID();
-            const uint8_t uuidArray[] = {(uint8_t)((shortUUID>>8)&0xFF), (uint8_t)((shortUUID&0xFF))};
+            uint8_t desc_uuid[16] = { 0 };
+
+
+            uint8_t desc_uuid_type = CHAR_DESC_TYPE_16_BIT;
+            STORE_LE_16(desc_uuid, descriptor->getUUID().getShortUUID());
+
+            if((descriptor->getUUID()).shortOrLong() == UUID::UUID_TYPE_LONG) {
+                desc_uuid_type = CHAR_DESC_TYPE_128_BIT;
+                const uint8_t* base_desc_uuid  = descriptor->getUUID().getBaseUUID();
+
+                COPY_UUID_128(desc_uuid, base_desc_uuid[15], base_desc_uuid[14],base_desc_uuid[13],base_desc_uuid[12], base_desc_uuid[11], base_desc_uuid[10], base_desc_uuid[9], base_desc_uuid[8], base_desc_uuid[7], base_desc_uuid[6], base_desc_uuid[5], base_desc_uuid[4], base_desc_uuid[3], base_desc_uuid[2], base_desc_uuid[1], base_desc_uuid[0]);
+            }
+
             ret = aci_gatt_add_char_desc(service.getHandle(),
                                          bleCharacteristic,
-                                         CHAR_DESC_TYPE_16_BIT,
-                                         uuidArray,
+                                         desc_uuid_type,
+                                         desc_uuid,
                                          descriptor->getMaxLength(),
                                          descriptor->getLength(),
                                          descriptor->getValuePtr(),
@@ -226,19 +305,52 @@
                                          CHAR_ATTRIBUTE_LEN_IS_FIXED,
                                          &descHandle);
             PRINTF("Adding Descriptor descriptor handle=%d ret=%d\n\r", descHandle, ret);
-            if(ret==(tBleStatus)0) {
-                PRINTF("Descriptor added successfully, descriptor handle=%d\n\r", descHandle);
-                descriptor->setHandle(descHandle);
+
+            switch (ret) {
+                case BLE_STATUS_SUCCESS:
+                    PRINTF("Descriptor added successfully, descriptor handle=%d\n\r", descHandle);
+                    descriptor->setHandle(descHandle);
+                    break;
+
+                case ERR_UNSPECIFIED_ERROR:
+                case BLE_STATUS_INSUFFICIENT_RESOURCES:
+                case BLE_STATUS_OUT_OF_HANDLE:
+                    // TODO remove characteristics and the service previously added.
+                    // remove service in the stack by using: Aci_Gatt_Del_Service
+                    // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                    // update service counter
+                    // destroy registered characteristic and updat echaracteristic counter
+                    return BLE_ERROR_NO_MEM;
+
+                case BLE_STATUS_INVALID_HANDLE:
+                case BLE_STATUS_INVALID_PARAMETER:
+                // TODO remove characteristics and the service previously added.
+                // remove service in the stack by using: Aci_Gatt_Del_Service
+                // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                // update service counter
+                // destroy registered characteristic and updat echaracteristic counter
+                    return BLE_ERROR_INVALID_PARAM;
+
+                case BLE_STATUS_INVALID_OPERATION:
+                    return BLE_ERROR_OPERATION_NOT_PERMITTED;
+
+                case BLE_STATUS_ERROR:
+                default:
+                // TODO remove characteristics and the service previously added.
+                // remove service in the stack by using: Aci_Gatt_Del_Service
+                // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                // update service counter
+                // destroy registered characteristic and updat echaracteristic counter
+                    return BLE_ERROR_INTERNAL_STACK_FAILURE;
             }
         }
+    }
 
-    }    
-    
     serviceCount++;
-    
-    //FIXME: There is no GattService pointer array in GattServer. 
+
+    //FIXME: There is no GattService pointer array in GattServer.
     //        There should be one? (Only the user is aware of GattServices!) Report to forum.
-    
+
     return BLE_ERROR_NONE;
 }
 
@@ -269,7 +381,7 @@
 ble_error_t BlueNRGGattServer::read(GattAttribute::Handle_t attributeHandle, uint8_t buffer[], uint16_t *lengthP)
 {
   tBleStatus ret;
-  uint16_t charHandle = attributeHandle-BlueNRGGattServer::CHAR_VALUE_HANDLE;
+  uint16_t charHandle = attributeHandle;
 
   ret = aci_gatt_read_handle_value(charHandle, *lengthP, lengthP, buffer);
 
@@ -335,38 +447,91 @@
 
   return BLE_ERROR_NONE;
 }
-    
+
 ble_error_t BlueNRGGattServer::write(GattAttribute::Handle_t attributeHandle, const uint8_t buffer[], uint16_t len, bool localOnly)
 {
     /* avoid compiler warnings about unused variables */
     (void)localOnly;
 
-    tBleStatus ret;
-
-    uint16_t charHandle = attributeHandle-BlueNRGGattServer::CHAR_VALUE_HANDLE;
-
-    PRINTF("updating bleCharacteristic valueHandle=%u,\
-            corresponding serviceHandle=%u len=%d\n\r",
-            attributeHandle, bleCharHandleMap.find(charHandle)->second, len);
-
-    /*
-     * If notifications (or indications) are enabled on that characteristic, a notification (or indication)
-     * will be sent to the client after sending this command to the BlueNRG.
-     */
-    ret = aci_gatt_update_char_value(bleCharHandleMap.find(charHandle)->second, charHandle, 0, len, buffer);
-
-    if (ret != BLE_STATUS_SUCCESS){
-      PRINTF("Error while updating characteristic (ret=0x%x).\n\r", ret);
-      switch (ret) {
-        case BLE_STATUS_INVALID_HANDLE:
-        case BLE_STATUS_INVALID_PARAMETER:
-          return BLE_ERROR_INVALID_PARAM;
-        default:
-          return BLE_STACK_BUSY;
-      }
+    // check that the len of the data to write are compatible with the characteristic
+    GattCharacteristic* characteristic = getCharacteristicFromHandle(attributeHandle);
+    if (!characteristic) {
+        PRINTF("characteristic not found\r\n");
+        return BLE_ERROR_INVALID_PARAM;
     }
 
-    return BLE_ERROR_NONE;
+    // if the attribute handle is the attribute handle of the characteristic value then
+    // write the value
+    if (attributeHandle == characteristic->getValueHandle()) {
+        // assert the len in input is correct for this characteristic
+        const GattAttribute& value_attribute = characteristic->getValueAttribute();
+
+        // reject write if the lenght exceed the maximum lenght of this attribute
+        if (value_attribute.getMaxLength() < len) {
+            PRINTF("invalid variable length: %u, max length is: %u\r\n", len, value_attribute.getMaxLength());
+            return BLE_ERROR_INVALID_PARAM;
+        }
+
+        // reject write if the attribute size is fixed and the lenght in input is different than the
+        // length of the attribute.
+        if (value_attribute.hasVariableLength() == false && value_attribute.getMaxLength() != len) {
+            PRINTF("invalid fixed length: %u, len should be %u\r\n", len, value_attribute.getMaxLength());
+            return BLE_ERROR_INVALID_PARAM;
+        }
+
+        tBleStatus ret;
+
+        uint16_t charHandle = characteristic->getValueHandle() - BlueNRGGattServer::CHAR_VALUE_HANDLE;
+
+        PRINTF("updating bleCharacteristic valueHandle=%u,\
+                corresponding serviceHandle=%u len=%d\n\r",
+                attributeHandle, bleCharHandleMap.find(charHandle)->second, len);
+
+        /*
+         * If notifications (or indications) are enabled on that characteristic, a notification (or indication)
+         * will be sent to the client after sending this command to the BlueNRG.
+         */
+        ret = aci_gatt_update_char_value(bleCharHandleMap.find(charHandle)->second, charHandle, 0, len, buffer);
+
+        if (ret != BLE_STATUS_SUCCESS){
+          PRINTF("Error while updating characteristic (ret=0x%x).\n\r", ret);
+          switch (ret) {
+            case BLE_STATUS_INVALID_HANDLE:
+            case BLE_STATUS_INVALID_PARAMETER:
+              return BLE_ERROR_INVALID_PARAM;
+            default:
+              return BLE_STACK_BUSY;
+          }
+        }
+
+        return BLE_ERROR_NONE;
+    } else {
+        // write this handle has a descriptor handle
+        uint16_t charHandle = characteristic->getValueHandle() - BlueNRGGattServer::CHAR_VALUE_HANDLE;
+        uint16_t service_handle = bleCharHandleMap.find(charHandle)->second;
+
+        tBleStatus ret = aci_gatt_set_desc_value(
+            service_handle,
+            charHandle,
+            attributeHandle,
+            0,
+        	len,
+        	buffer
+        );
+
+        if (ret != BLE_STATUS_SUCCESS){
+          PRINTF("Error while updating characteristic descriptor (ret=0x%x).\n\r", ret);
+          switch (ret) {
+            case BLE_STATUS_INVALID_HANDLE:
+            case BLE_STATUS_INVALID_PARAMETER:
+              return BLE_ERROR_INVALID_PARAM;
+            default:
+              return BLE_STACK_BUSY;
+          }
+        }
+
+        return BLE_ERROR_NONE;
+    }
 }
 
 /**************************************************************************/
@@ -391,22 +556,58 @@
 ble_error_t BlueNRGGattServer::Read_Request_CB(uint16_t attributeHandle)
 {
     uint16_t gapConnectionHandle = BlueNRGGap::getInstance().getConnectionHandle();
-    
+
     GattReadCallbackParams readParams;
     readParams.handle = attributeHandle;
 
     //PRINTF("readParams.handle = %d\n\r", readParams.handle);
     HCIDataReadEvent(&readParams);
-    
+
     //EXIT:
     if(gapConnectionHandle != 0){
         //PRINTF("Calling aci_gatt_allow_read\n\r");
         aci_gatt_allow_read(gapConnectionHandle);
     }
-    
+
     return BLE_ERROR_NONE;
 }
 
+// ask if the write request should be accepted of rejected
+// return 0 in case of success or an ATT error response in
+// case of faillure
+uint8_t BlueNRGGattServer::Write_Request_CB(
+    uint16_t connection_handle, uint16_t attr_handle, uint8_t data_length,
+    const uint8_t* data) {
+
+    GattCharacteristic* characteristic = getCharacteristicFromHandle(attr_handle);
+    if(!characteristic) {
+        return AUTH_CALLBACK_REPLY_ATTERR_INVALID_HANDLE & 0xFF;
+    }
+
+    // check if the data length is in range
+    if (characteristic->getValueAttribute().getMaxLength() < data_length) {
+        return AUTH_CALLBACK_REPLY_ATTERR_INVALID_ATT_VAL_LENGTH & 0xFF;
+    }
+
+    // if the length of the characteristic value is fixed
+    // then the data in input should be of that length
+    if (characteristic->getValueAttribute().hasVariableLength() == false &&
+        characteristic->getValueAttribute().getMaxLength() != data_length) {
+        return AUTH_CALLBACK_REPLY_ATTERR_INVALID_ATT_VAL_LENGTH & 0xFF;
+    }
+
+    GattWriteAuthCallbackParams params = {
+        connection_handle,
+        attr_handle,
+        /* offset */ 0,
+        data_length,
+        data,
+        /* authorizationReply */ AUTH_CALLBACK_REPLY_ATTERR_WRITE_NOT_PERMITTED
+    };
+
+    return characteristic->authorizeWrite(&params) & 0xFF;
+}
+
 /**************************************************************************/
 /*!
     @brief  Returns the GattCharacteristic according to the handle provided
@@ -444,7 +645,7 @@
                 p_char = p_characteristics[i];
                 PRINTF("Found Characteristic Properties 0x%x (handle=%d)\n\r",p_char->getProperties(), handle);
                 break;
-            }            
+            }
         }
         else {
             handle_1 = p_characteristics[i+1]->getValueAttribute().getHandle()-BlueNRGGattServer::CHAR_VALUE_HANDLE;
@@ -464,7 +665,7 @@
 void BlueNRGGattServer::HCIDataWrittenEvent(const GattWriteCallbackParams *params) {
     this->handleDataWrittenEvent(params);
 }
-    
+
 void BlueNRGGattServer::HCIDataReadEvent(const GattReadCallbackParams *params) {
     PRINTF("Called HCIDataReadEvent\n\r");
     this->handleDataReadEvent(params);
@@ -478,10 +679,10 @@
     this->handleDataSentEvent(count);
 }
 
-    
+
 ble_error_t BlueNRGGattServer::initializeGATTDatabase(void)   {
-    // <TODO>    
-    return (ble_error_t)0;       
+    // <TODO>
+    return (ble_error_t)0;
 }
 
 /**************************************************************************/
@@ -509,3 +710,44 @@
 
     return BLE_ERROR_NONE;
 }
+
+
+/// compute the number of attributes needed by this service.
+uint16_t BlueNRGGattServer::computeAttributesRecord(GattService& service) {
+    uint16_t attribute_records = 1;
+
+    for (uint8_t characteristic_index = 0; characteristic_index < service.getCharacteristicCount(); ++characteristic_index) {
+        // add two attributes, one for the characteristic declaration
+        // and the other for the characteristic value.
+        attribute_records += 2;
+
+        const GattCharacteristic* characteristic = service.getCharacteristic(characteristic_index);
+        const uint8_t properties = characteristic->getProperties();
+        // if notify or indicate are present, two attributes are
+        // needed
+        if ((properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY) ||
+            (properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE)) {
+            attribute_records += 2;
+        }
+
+        // if broadcast is set, two attributes are needed
+        if (properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_BROADCAST) {
+            attribute_records += 2;
+        }
+
+        // if extended properties flag is set, two attributes are needed
+        if (properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_EXTENDED_PROPERTIES) {
+            attribute_records += 2;
+        }
+
+        attribute_records += characteristic->getDescriptorCount();
+    }
+
+    // for some reason, if there is just a service, this value should
+    // be equal to 5
+    if (attribute_records == 1) {
+        attribute_records = 5;
+    }
+
+    return attribute_records;
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/ble_hci.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/bluenrg-hci/hci/ble_hci.c	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,1227 @@
+/**
+  ******************************************************************************
+  * @file    ble_hci.c 
+  * @author  AMS/HESA Application Team
+  * @brief   Function for managing HCI interface.
+  ******************************************************************************
+  *
+  *
+  * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+  * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+  * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
+  * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+  * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+  * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+  *
+  * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
+  */ 
+
+#include "ble_hal_types.h"
+#include "ble_osal.h"
+#include "ble_status.h"
+#include "ble_hal.h"
+#include "ble_hci_const.h"
+#include "ble_gp_timer.h"
+#include "ble_debug.h"
+
+#include "stm32_bluenrg_ble.h"
+
+#if BLE_CONFIG_DBG_ENABLE
+#undef PRINTF
+#endif
+
+#define HCI_LOG_ON 0
+
+#define HCI_READ_PACKET_NUM_MAX 		 (0x40)
+
+#define MIN(a,b)            ((a) < (b) )? (a) : (b)
+#define MAX(a,b)            ((a) > (b) )? (a) : (b)
+
+tListNode hciReadPktPool;
+tListNode hciReadPktRxQueue;
+
+// betzw - DEBUG:
+//#define POOL_CNT
+#ifdef POOL_CNT
+#include <stdio.h>
+static unsigned int nr_hciReadPktPool;
+static unsigned int lowest_nr_hciReadPktPool;
+#endif // POOL_CNT
+
+/* pool of hci read packets */
+static tHciDataPacket   hciReadPacketBuffer[HCI_READ_PACKET_NUM_MAX];
+
+static volatile uint8_t readPacketListFull=FALSE;
+
+static volatile uint8_t hci_timer_id;
+static volatile uint8_t hci_timeout;
+
+void hci_timeout_callback(void)
+{
+  hci_timeout = 1;
+  return;
+}
+
+void HCI_Init(void)
+{
+  uint8_t index;
+  
+  Disable_SPI_IRQ();
+
+#ifdef POOL_CNT
+  nr_hciReadPktPool = 0;
+#endif // POOL_CNT
+
+  /* Initialize list heads of ready and free hci data packet queues */
+  list_init_head (&hciReadPktPool);
+  list_init_head (&hciReadPktRxQueue);
+  
+  /* Initialize the queue of free hci data packets */
+  for (index = 0; index < HCI_READ_PACKET_NUM_MAX; index++)
+  {
+    list_insert_tail(&hciReadPktPool, (tListNode *)&hciReadPacketBuffer[index]);
+#ifdef POOL_CNT
+    nr_hciReadPktPool++;
+#endif // POOL_CNT
+  }
+
+#ifdef POOL_CNT
+  lowest_nr_hciReadPktPool = nr_hciReadPktPool;
+#endif // POOL_CNT
+
+  Enable_SPI_IRQ();
+}
+
+#define HCI_PCK_TYPE_OFFSET                 0
+#define  EVENT_PARAMETER_TOT_LEN_OFFSET     2
+
+/**
+ * Verify if HCI packet is correctly formatted..
+ *
+ * @param[in] hciReadPacket    The packet that is received from HCI interface.
+ * @return 0 if HCI packet is as expected
+ */
+int HCI_verify(const tHciDataPacket * hciReadPacket)
+{
+  const uint8_t *hci_pckt = hciReadPacket->dataBuff;
+  
+  if(hci_pckt[HCI_PCK_TYPE_OFFSET] != HCI_EVENT_PKT)
+    return 1;  /* Incorrect type. */
+  
+  if(hci_pckt[EVENT_PARAMETER_TOT_LEN_OFFSET] != hciReadPacket->data_len - (1+HCI_EVENT_HDR_SIZE))
+    return 2; /* Wrong length (packet truncated or too long). */
+  
+  return 0;      
+}
+
+void HCI_Process(void)
+{
+  uint8_t data_len;
+  uint8_t buffer[HCI_READ_PACKET_SIZE];
+  tHciDataPacket * hciReadPacket = NULL;
+  
+#ifdef POOL_CNT
+  printf("betzw(%s, %d): nr_hciReadPktPool = %u (lowest = %u)\r\n", __func__, __LINE__, 
+	 nr_hciReadPktPool, lowest_nr_hciReadPktPool);
+#endif // POOL_CNT
+
+  Disable_SPI_IRQ();
+  uint8_t list_empty = list_is_empty(&hciReadPktRxQueue);        
+  /* process any pending events read */
+  while(list_empty == FALSE)
+  {
+    list_remove_head (&hciReadPktRxQueue, (tListNode **)&hciReadPacket);
+    Enable_SPI_IRQ();
+    HCI_Event_CB(hciReadPacket->dataBuff);
+    Disable_SPI_IRQ();
+    list_insert_tail(&hciReadPktPool, (tListNode *)hciReadPacket);
+#ifdef POOL_CNT
+    nr_hciReadPktPool++;
+#endif
+    list_empty = list_is_empty(&hciReadPktRxQueue);
+  }
+  if (readPacketListFull) {
+    while(BlueNRG_DataPresent()) {
+      data_len = BlueNRG_SPI_Read_All(buffer, HCI_READ_PACKET_SIZE);
+      if(data_len > 0)
+        HCI_Event_CB(buffer);
+    }
+    readPacketListFull = FALSE;
+  }
+  
+  Enable_SPI_IRQ();    
+}
+
+BOOL HCI_Queue_Empty(void)
+{
+  return list_is_empty(&hciReadPktRxQueue);
+}
+
+void HCI_Isr(void)
+{
+  tHciDataPacket * hciReadPacket = NULL;
+  uint8_t data_len;
+  
+  Clear_SPI_EXTI_Flag();
+  while(BlueNRG_DataPresent()){        
+    if (list_is_empty (&hciReadPktPool) == FALSE){
+      
+      /* enqueueing a packet for read */
+      list_remove_head (&hciReadPktPool, (tListNode **)&hciReadPacket);
+#ifdef POOL_CNT
+      nr_hciReadPktPool--;
+      if(nr_hciReadPktPool < lowest_nr_hciReadPktPool)
+	      lowest_nr_hciReadPktPool = nr_hciReadPktPool;
+#endif
+
+      data_len = BlueNRG_SPI_Read_All(hciReadPacket->dataBuff, HCI_READ_PACKET_SIZE);
+      if(data_len > 0){                    
+        hciReadPacket->data_len = data_len;
+        if(HCI_verify(hciReadPacket) == 0) {
+          list_insert_tail(&hciReadPktRxQueue, (tListNode *)hciReadPacket);
+	      signalEventsToProcess();
+        } else {
+          list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket);
+#ifdef POOL_CNT
+	  nr_hciReadPktPool++;
+#endif
+	}
+      }
+      else {
+        // Insert the packet back into the pool.
+        list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket);
+#ifdef POOL_CNT
+	nr_hciReadPktPool++;
+#endif
+      }
+    }
+    else{
+      // HCI Read Packet Pool is empty, wait for a free packet.
+      signalEventsToProcess();
+      readPacketListFull = TRUE;
+      Clear_SPI_EXTI_Flag();
+      return;
+    }
+    Clear_SPI_EXTI_Flag();
+  }
+}
+
+void hci_write(const void* data1, const void* data2, uint8_t n_bytes1, uint8_t n_bytes2){
+#if  HCI_LOG_ON
+  PRINTF("HCI <- ");
+  for(int i=0; i < n_bytes1; i++)
+    PRINTF("%02X ", *((uint8_t*)data1 + i));
+  for(int i=0; i < n_bytes2; i++)
+    PRINTF("%02X ", *((uint8_t*)data2 + i));
+  PRINTF("\n");    
+#endif
+  
+  Hal_Write_Serial(data1, data2, n_bytes1, n_bytes2);
+}
+
+void hci_send_cmd(uint16_t ogf, uint16_t ocf, uint8_t plen, void *param)
+{
+  hci_command_hdr hc;
+  
+  hc.opcode = htobs(cmd_opcode_pack(ogf, ocf));
+  hc.plen= plen;
+  
+  uint8_t header[HCI_HDR_SIZE + HCI_COMMAND_HDR_SIZE];
+  header[0] = HCI_COMMAND_PKT;
+  Osal_MemCpy(header+1, &hc, sizeof(hc));
+  
+  hci_write(header, param, sizeof(header), plen);
+}
+
+static void move_list(tListNode * dest_list, tListNode * src_list)
+{
+  pListNode tmp_node;
+  
+  while(!list_is_empty(src_list)){
+    list_remove_head(src_list, &tmp_node);
+    list_insert_tail(dest_list, tmp_node);
+  }
+}
+
+int hci_send_req(struct hci_request *r, BOOL async)
+{
+  uint8_t *ptr;
+  uint16_t opcode = htobs(cmd_opcode_pack(r->ogf, r->ocf));
+  hci_event_pckt *event_pckt;
+  hci_uart_pckt *hci_hdr;
+  int to = DEFAULT_TIMEOUT;
+  struct timer t;
+  tHciDataPacket * hciReadPacket = NULL;
+  tListNode hciTempQueue;
+  
+  list_init_head(&hciTempQueue);
+  
+  hci_send_cmd(r->ogf, r->ocf, r->clen, r->cparam);
+  
+  if(async){
+    goto done;
+  }
+  
+  /* Minimum timeout is 1. */
+  if(to == 0)
+    to = 1;
+  
+  Timer_Set(&t, to);
+  
+  while(1) {
+    evt_cmd_complete *cc;
+    evt_cmd_status *cs;
+    evt_le_meta_event *me;
+    int len;
+      
+#if ENABLE_MICRO_SLEEP    
+    while(1){
+      ATOMIC_SECTION_BEGIN();
+      if(Timer_Expired(&t)){
+        ATOMIC_SECTION_END();
+        goto failed;
+      }
+      if(!HCI_Queue_Empty()){
+        ATOMIC_SECTION_END();
+        break;
+      }
+      Enter_Sleep_Mode();
+      ATOMIC_SECTION_END();
+    }
+#else
+    while(1){
+      if(Timer_Expired(&t)){
+        goto failed;
+      }
+      if(!HCI_Queue_Empty()){
+        break;
+      }
+    }
+#endif
+    
+    /* Extract packet from HCI event queue. */
+    Disable_SPI_IRQ();
+    list_remove_head(&hciReadPktRxQueue, (tListNode **)&hciReadPacket);    
+    
+    hci_hdr = (void *)hciReadPacket->dataBuff;
+    if(hci_hdr->type != HCI_EVENT_PKT){
+      list_insert_tail(&hciTempQueue, (tListNode *)hciReadPacket); // See comment below
+      Enable_SPI_IRQ();
+      continue;
+    }
+    
+    event_pckt = (void *) (hci_hdr->data);
+    
+    ptr = hciReadPacket->dataBuff + (1 + HCI_EVENT_HDR_SIZE);
+    len = hciReadPacket->data_len - (1 + HCI_EVENT_HDR_SIZE);
+    
+    switch (event_pckt->evt) {
+      
+    case EVT_CMD_STATUS:
+      cs = (void *) ptr;
+      
+      if (cs->opcode != opcode)
+        goto failed;
+      
+      if (r->event != EVT_CMD_STATUS) {
+        if (cs->status) {
+          goto failed;
+        }
+        break;
+      }
+      
+      r->rlen = MIN(len, r->rlen);
+      Osal_MemCpy(r->rparam, ptr, r->rlen);
+      goto done;
+      
+    case EVT_CMD_COMPLETE:
+      cc = (void *) ptr;
+      
+      if (cc->opcode != opcode)
+        goto failed;
+      
+      ptr += EVT_CMD_COMPLETE_SIZE;
+      len -= EVT_CMD_COMPLETE_SIZE;
+      
+      r->rlen = MIN(len, r->rlen);
+      Osal_MemCpy(r->rparam, ptr, r->rlen);
+      goto done;
+      
+    case EVT_LE_META_EVENT:
+      me = (void *) ptr;
+      
+      if (me->subevent != r->event)
+        break;
+      
+      len -= 1;
+      r->rlen = MIN(len, r->rlen);
+      Osal_MemCpy(r->rparam, me->data, r->rlen);
+      goto done;
+      
+    case EVT_HARDWARE_ERROR:            
+      goto failed;
+      
+    default:      
+      break;
+    }
+    
+    /* In the meantime there could be other events from the controller.
+       In this case, insert the packet in a different queue. These packets will be
+       inserted back in the main queue just before exiting from send_req().
+    */
+    if(hciReadPacket != NULL) {
+	    list_insert_tail(&hciTempQueue, (tListNode *)hciReadPacket);
+	    hciReadPacket = NULL;
+    }
+    /* Be sure there is at list one packet in the pool to process the expected event. */
+    if(list_is_empty(&hciReadPktPool)){ // betzw: this is a kind of steeling (should never happen?!?)
+      pListNode tmp_node;      
+      list_remove_head(&hciReadPktRxQueue, &tmp_node);
+      list_insert_tail(&hciReadPktPool, tmp_node);     
+#ifdef POOL_CNT
+      nr_hciReadPktPool++;
+#endif
+    }
+    
+    Enable_SPI_IRQ();
+    
+  }
+  
+failed: 
+  // Insert the packet back into the pool.
+  if(hciReadPacket != NULL) {
+	  list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket); 
+#ifdef POOL_CNT
+	  nr_hciReadPktPool++;
+#endif
+	  hciReadPacket = NULL;
+  }
+  move_list(&hciReadPktRxQueue, &hciTempQueue);  
+  Enable_SPI_IRQ();
+  return -1;
+  
+done:
+  // Insert the packet back into the pool.
+  if(hciReadPacket != NULL) {
+	  list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket); 
+#ifdef POOL_CNT
+	  nr_hciReadPktPool++;
+#endif
+	  hciReadPacket = NULL;
+  }
+  move_list(&hciReadPktRxQueue, &hciTempQueue);
+  
+  Enable_SPI_IRQ();
+  return 0;
+}
+
+int hci_reset()
+{
+  struct hci_request rq;
+  uint8_t status;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_HOST_CTL;
+  rq.ocf = OCF_RESET;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;  
+}
+
+int hci_disconnect(uint16_t	handle, uint8_t reason)
+{
+  struct hci_request rq;
+  disconnect_cp cp;
+  uint8_t status;
+  
+  cp.handle = handle;
+  cp.reason = reason;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LINK_CTL;
+  rq.ocf = OCF_DISCONNECT;
+  rq.cparam = &cp;
+  rq.clen = DISCONNECT_CP_SIZE;
+  rq.event = EVT_CMD_STATUS;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;  
+}
+
+int hci_le_read_local_version(uint8_t *hci_version, uint16_t *hci_revision, uint8_t *lmp_pal_version, 
+                              uint16_t *manufacturer_name, uint16_t *lmp_pal_subversion)
+{
+  struct hci_request rq;
+  read_local_version_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_INFO_PARAM;
+  rq.ocf = OCF_READ_LOCAL_VERSION;
+  rq.cparam = NULL;
+  rq.clen = 0;
+  rq.rparam = &resp;
+  rq.rlen = READ_LOCAL_VERSION_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  
+  *hci_version = resp.hci_version;
+  *hci_revision =  btohs(resp.hci_revision);
+  *lmp_pal_version = resp.lmp_pal_version;
+  *manufacturer_name = btohs(resp.manufacturer_name);
+  *lmp_pal_subversion = btohs(resp.lmp_pal_subversion);
+  
+  return 0;
+}
+
+int hci_le_read_buffer_size(uint16_t *pkt_len, uint8_t *max_pkt)
+{
+  struct hci_request rq;
+  le_read_buffer_size_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_READ_BUFFER_SIZE;
+  rq.cparam = NULL;
+  rq.clen = 0;
+  rq.rparam = &resp;
+  rq.rlen = LE_READ_BUFFER_SIZE_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  *pkt_len = resp.pkt_len;
+  *max_pkt = resp.max_pkt;
+  
+  return 0;
+}
+
+int hci_le_set_advertising_parameters(uint16_t min_interval, uint16_t max_interval, uint8_t advtype,
+                                      uint8_t own_bdaddr_type, uint8_t direct_bdaddr_type, const tBDAddr direct_bdaddr, uint8_t chan_map,
+                                      uint8_t filter)
+{
+  struct hci_request rq;
+  le_set_adv_parameters_cp adv_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&adv_cp, 0, sizeof(adv_cp));
+  adv_cp.min_interval = min_interval;
+  adv_cp.max_interval = max_interval;
+  adv_cp.advtype = advtype;
+  adv_cp.own_bdaddr_type = own_bdaddr_type;
+  adv_cp.direct_bdaddr_type = direct_bdaddr_type;
+  if(direct_bdaddr != NULL)
+    Osal_MemCpy(adv_cp.direct_bdaddr,direct_bdaddr,sizeof(adv_cp.direct_bdaddr));
+  adv_cp.chan_map = chan_map;
+  adv_cp.filter = filter;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_SET_ADV_PARAMETERS;
+  rq.cparam = &adv_cp;
+  rq.clen = LE_SET_ADV_PARAMETERS_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_le_set_advertising_data(uint8_t length, const uint8_t data[])
+{
+  struct hci_request rq;
+  le_set_adv_data_cp adv_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&adv_cp, 0, sizeof(adv_cp));
+  adv_cp.length = length;
+  Osal_MemCpy(adv_cp.data, data, MIN(31,length));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_SET_ADV_DATA;
+  rq.cparam = &adv_cp;
+  rq.clen = LE_SET_ADV_DATA_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_le_set_advertise_enable(uint8_t enable)
+{
+  struct hci_request rq;
+  le_set_advertise_enable_cp adv_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&adv_cp, 0, sizeof(adv_cp));
+  adv_cp.enable = enable?1:0;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_SET_ADVERTISE_ENABLE;
+  rq.cparam = &adv_cp;
+  rq.clen = LE_SET_ADVERTISE_ENABLE_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_le_set_scan_parameters(uint8_t	type, uint16_t interval,
+                               uint16_t window, uint8_t own_bdaddr_type,
+                               uint8_t	filter)
+{
+  struct hci_request rq;
+  le_set_scan_parameters_cp scan_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&scan_cp, 0, sizeof(scan_cp));
+  scan_cp.type = type;
+  scan_cp.interval = interval;
+  scan_cp.window = window;
+  scan_cp.own_bdaddr_type = own_bdaddr_type;
+  scan_cp.filter = filter;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_SET_SCAN_PARAMETERS;
+  rq.cparam = &scan_cp;
+  rq.clen = LE_SET_SCAN_PARAMETERS_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_le_set_scan_enable(uint8_t enable, uint8_t filter_dup)
+{
+  struct hci_request rq;
+  le_set_scan_enable_cp scan_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&scan_cp, 0, sizeof(scan_cp));
+  scan_cp.enable = enable?1:0;
+  scan_cp.filter_dup = filter_dup;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_SET_SCAN_ENABLE;
+  rq.cparam = &scan_cp;
+  rq.clen = LE_SET_SCAN_ENABLE_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_le_rand(uint8_t random_number[8])
+{
+  struct hci_request rq;
+  le_rand_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_RAND;
+  rq.cparam = NULL;
+  rq.clen = 0;
+  rq.rparam = &resp;
+  rq.rlen = LE_RAND_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  Osal_MemCpy(random_number, resp.random, 8);
+  
+  return 0;
+}
+
+int hci_le_set_scan_resp_data(uint8_t length, const uint8_t data[])
+{
+  struct hci_request rq;
+  le_set_scan_response_data_cp scan_resp_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&scan_resp_cp, 0, sizeof(scan_resp_cp));
+  scan_resp_cp.length = length;
+  Osal_MemCpy(scan_resp_cp.data, data, MIN(31,length));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_SET_SCAN_RESPONSE_DATA;
+  rq.cparam = &scan_resp_cp;
+  rq.clen = LE_SET_SCAN_RESPONSE_DATA_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_le_read_advertising_channel_tx_power(int8_t *tx_power_level)
+{
+  struct hci_request rq;
+  le_read_adv_channel_tx_power_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_READ_ADV_CHANNEL_TX_POWER;
+  rq.cparam = NULL;
+  rq.clen = 0;
+  rq.rparam = &resp;
+  rq.rlen = LE_RAND_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  *tx_power_level = resp.level;
+  
+  return 0;
+}
+
+int hci_le_set_random_address(tBDAddr bdaddr)
+{
+  struct hci_request rq;
+  le_set_random_address_cp set_rand_addr_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&set_rand_addr_cp, 0, sizeof(set_rand_addr_cp));
+  Osal_MemCpy(set_rand_addr_cp.bdaddr, bdaddr, sizeof(tBDAddr));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_SET_RANDOM_ADDRESS;
+  rq.cparam = &set_rand_addr_cp;
+  rq.clen = LE_SET_RANDOM_ADDRESS_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_read_bd_addr(tBDAddr bdaddr)
+{
+  struct hci_request rq;
+  read_bd_addr_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_INFO_PARAM;
+  rq.ocf = OCF_READ_BD_ADDR;
+  rq.cparam = NULL;
+  rq.clen = 0;
+  rq.rparam = &resp;
+  rq.rlen = READ_BD_ADDR_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  Osal_MemCpy(bdaddr, resp.bdaddr, sizeof(tBDAddr));
+  
+  return 0;
+}
+
+int hci_le_create_connection(uint16_t interval,	uint16_t window, uint8_t initiator_filter, uint8_t peer_bdaddr_type,
+                             const tBDAddr peer_bdaddr,	uint8_t	own_bdaddr_type, uint16_t min_interval,	uint16_t max_interval,
+                             uint16_t latency,	uint16_t supervision_timeout, uint16_t min_ce_length, uint16_t max_ce_length)
+{
+  struct hci_request rq;
+  le_create_connection_cp create_cp;
+  uint8_t status;
+  
+  Osal_MemSet(&create_cp, 0, sizeof(create_cp));
+  create_cp.interval = interval;
+  create_cp.window =  window;
+  create_cp.initiator_filter = initiator_filter;
+  create_cp.peer_bdaddr_type = peer_bdaddr_type;
+  Osal_MemCpy(create_cp.peer_bdaddr, peer_bdaddr, sizeof(tBDAddr));
+  create_cp.own_bdaddr_type = own_bdaddr_type;
+  create_cp.min_interval=min_interval;
+  create_cp.max_interval=max_interval;
+  create_cp.latency = latency;
+  create_cp.supervision_timeout=supervision_timeout;
+  create_cp.min_ce_length=min_ce_length;
+  create_cp.max_ce_length=max_ce_length;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_CREATE_CONN;
+  rq.cparam = &create_cp;
+  rq.clen = LE_CREATE_CONN_CP_SIZE;
+  rq.event = EVT_CMD_STATUS;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return status;
+}
+
+int hci_le_create_connection_cancel(void)
+{
+  struct hci_request rq;
+  uint8_t status;
+
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_CREATE_CONN_CANCEL;
+  rq.rparam = &status;
+  rq.rlen = 1;
+
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+
+  return status;
+}
+
+int hci_le_encrypt(uint8_t key[16], uint8_t plaintextData[16], uint8_t encryptedData[16])
+{
+  struct hci_request rq;
+  le_encrypt_cp params;
+  le_encrypt_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemCpy(params.key, key, 16);
+  Osal_MemCpy(params.plaintext, plaintextData, 16);
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_ENCRYPT;
+  rq.cparam = &params;
+  rq.clen = LE_ENCRYPT_CP_SIZE;
+  rq.rparam = &resp;
+  rq.rlen = LE_ENCRYPT_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  Osal_MemCpy(encryptedData, resp.encdata, 16);
+  
+  return 0;
+}
+
+int hci_le_ltk_request_reply(uint8_t key[16])
+{
+  struct hci_request rq;
+  le_ltk_reply_cp params;
+  le_ltk_reply_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  params.handle = 1;
+  Osal_MemCpy(params.key, key, 16);
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_LTK_REPLY;
+  rq.cparam = &params;
+  rq.clen = LE_LTK_REPLY_CP_SIZE;
+  rq.rparam = &resp;
+  rq.rlen = LE_LTK_REPLY_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return resp.status;
+}
+
+int hci_le_ltk_request_neg_reply()
+{
+  struct hci_request rq;
+  le_ltk_neg_reply_cp params;
+  le_ltk_neg_reply_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  params.handle = 1;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_LTK_NEG_REPLY;
+  rq.cparam = &params;
+  rq.clen = LE_LTK_NEG_REPLY_CP_SIZE;
+  rq.rparam = &resp;
+  rq.rlen = LE_LTK_NEG_REPLY_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0)
+    return BLE_STATUS_TIMEOUT;
+  
+  return resp.status;
+}
+
+int hci_le_read_white_list_size(uint8_t *size)
+{
+  struct hci_request rq;
+  le_read_white_list_size_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_READ_WHITE_LIST_SIZE;
+  rq.rparam = &resp;
+  rq.rlen = LE_READ_WHITE_LIST_SIZE_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  *size = resp.size;
+  
+  return 0;
+}
+
+int hci_le_clear_white_list()
+{
+  struct hci_request rq;
+  uint8_t status;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_CLEAR_WHITE_LIST;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  return status;
+}
+
+int hci_le_add_device_to_white_list(uint8_t	bdaddr_type, tBDAddr bdaddr)
+{
+  struct hci_request rq;
+  le_add_device_to_white_list_cp params;
+  uint8_t status;
+  
+  params.bdaddr_type = bdaddr_type;
+  Osal_MemCpy(params.bdaddr, bdaddr, 6);
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_ADD_DEVICE_TO_WHITE_LIST;
+  rq.cparam = &params;
+  rq.clen = LE_ADD_DEVICE_TO_WHITE_LIST_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  return status;
+}
+
+int hci_le_remove_device_from_white_list(uint8_t bdaddr_type, tBDAddr bdaddr)
+{
+  struct hci_request rq;
+  le_remove_device_from_white_list_cp params;
+  uint8_t status;
+  
+  params.bdaddr_type = bdaddr_type;
+  Osal_MemCpy(params.bdaddr, bdaddr, 6);
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST;
+  rq.cparam = &params;
+  rq.clen = LE_REMOVE_DEVICE_FROM_WHITE_LIST_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  return status;
+}
+
+int hci_read_transmit_power_level(uint16_t *conn_handle, uint8_t type, int8_t * tx_level)
+{
+  struct hci_request rq;
+  read_transmit_power_level_cp params;
+  read_transmit_power_level_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  params.handle = *conn_handle;
+  params.type = type;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_HOST_CTL;
+  rq.ocf = OCF_READ_TRANSMIT_POWER_LEVEL;
+  rq.cparam = &params;
+  rq.clen = READ_TRANSMIT_POWER_LEVEL_CP_SIZE;
+  rq.rparam = &resp;
+  rq.rlen = READ_TRANSMIT_POWER_LEVEL_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  *conn_handle = resp.handle;
+  *tx_level = resp.level;
+  
+  return 0;
+}
+
+int hci_read_rssi(uint16_t *conn_handle, int8_t * rssi)
+{
+  struct hci_request rq;
+  read_rssi_cp params;
+  read_rssi_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  params.handle = *conn_handle;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_STATUS_PARAM;
+  rq.ocf = OCF_READ_RSSI;
+  rq.cparam = &params;
+  rq.clen = READ_RSSI_CP_SIZE;
+  rq.rparam = &resp;
+  rq.rlen = READ_RSSI_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  *conn_handle = resp.handle;
+  *rssi = resp.rssi;
+  
+  return 0;
+}
+
+int hci_le_read_local_supported_features(uint8_t *features)
+{
+  struct hci_request rq;
+  le_read_local_supported_features_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_READ_LOCAL_SUPPORTED_FEATURES;
+  rq.rparam = &resp;
+  rq.rlen = LE_READ_LOCAL_SUPPORTED_FEATURES_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  Osal_MemCpy(features, resp.features, sizeof(resp.features));
+  
+  return 0;
+}
+
+int hci_le_read_channel_map(uint16_t conn_handle, uint8_t ch_map[5])
+{
+  struct hci_request rq;
+  le_read_channel_map_cp params;
+  le_read_channel_map_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  params.handle = conn_handle;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_READ_CHANNEL_MAP;
+  rq.cparam = &params;
+  rq.clen = LE_READ_CHANNEL_MAP_CP_SIZE;
+  rq.rparam = &resp;
+  rq.rlen = LE_READ_CHANNEL_MAP_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  Osal_MemCpy(ch_map, resp.map, 5);
+  
+  return 0;
+}
+
+int hci_le_read_supported_states(uint8_t states[8])
+{
+  struct hci_request rq;
+  le_read_supported_states_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_READ_SUPPORTED_STATES;
+  rq.rparam = &resp;
+  rq.rlen = LE_READ_SUPPORTED_STATES_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  Osal_MemCpy(states, resp.states, 8);
+  
+  return 0;
+}
+
+int hci_le_receiver_test(uint8_t frequency)
+{
+  struct hci_request rq;
+  le_receiver_test_cp params;
+  uint8_t status;
+  
+  params.frequency = frequency;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_RECEIVER_TEST;
+  rq.cparam = &params;
+  rq.clen = LE_RECEIVER_TEST_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  return status;
+}
+
+int hci_le_transmitter_test(uint8_t frequency, uint8_t length, uint8_t payload)
+{
+  struct hci_request rq;
+  le_transmitter_test_cp params;
+  uint8_t status;
+  
+  params.frequency = frequency;
+  params.length = length;
+  params.payload = payload;
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_TRANSMITTER_TEST;
+  rq.cparam = &params;
+  rq.clen = LE_TRANSMITTER_TEST_CP_SIZE;
+  rq.rparam = &status;
+  rq.rlen = 1;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  return status;
+}
+
+int hci_le_test_end(uint16_t *num_pkts)
+{
+  struct hci_request rq;
+  le_test_end_rp resp;
+  
+  Osal_MemSet(&resp, 0, sizeof(resp));
+  
+  Osal_MemSet(&rq, 0, sizeof(rq));
+  rq.ogf = OGF_LE_CTL;
+  rq.ocf = OCF_LE_TEST_END;
+  rq.rparam = &resp;
+  rq.rlen = LE_TEST_END_RP_SIZE;
+  
+  if (hci_send_req(&rq, FALSE) < 0){
+    return BLE_STATUS_TIMEOUT;
+  }
+  
+  if (resp.status) {
+    return resp.status;
+  }
+  
+  *num_pkts = resp.num_pkts;
+  
+  return 0;
+}
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/controller/bluenrg_gap_aci.c
--- a/source/bluenrg-hci/hci/controller/bluenrg_gap_aci.c	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/bluenrg-hci/hci/controller/bluenrg_gap_aci.c	Thu Sep 15 16:59:44 2016 +0100
@@ -13,12 +13,12 @@
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
 
-#include "hal_types.h"
-#include "osal.h"
+#include "ble_hal_types.h"
+#include "ble_osal.h"
 #include "ble_status.h"
-#include "hal.h"
-#include "osal.h"
-#include "hci_const.h"
+#include "ble_hal.h"
+#include "ble_osal.h"
+#include "ble_hci_const.h"
 #include "bluenrg_aci_const.h"
 #include "bluenrg_gap_aci.h"
 #include "bluenrg_gatt_server.h"
@@ -1304,4 +1304,4 @@
     Osal_MemCpy(device_list, rp.dev_list, MIN(device_list_size,rp.num_addr*7));
   
   return 0;
-}
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/controller/bluenrg_gatt_aci.c
--- a/source/bluenrg-hci/hci/controller/bluenrg_gatt_aci.c	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/bluenrg-hci/hci/controller/bluenrg_gatt_aci.c	Thu Sep 15 16:59:44 2016 +0100
@@ -13,12 +13,11 @@
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
 
-#include "hal_types.h"
-#include "osal.h"
+#include "ble_hal_types.h"
+#include "ble_osal.h"
 #include "ble_status.h"
-#include "hal.h"
-#include "osal.h"
-#include "hci_const.h"
+#include "ble_hal.h"
+#include "ble_hci_const.h"
 #include "bluenrg_aci_const.h"
 #include "bluenrg_gatt_aci.h"
 #include "bluenrg_gatt_server.h"
@@ -1469,4 +1468,4 @@
   Osal_MemCpy(data, rp.value, MIN(data_len, *data_len_out_p));
 
   return 0; 
-}
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/controller/bluenrg_hal_aci.c
--- a/source/bluenrg-hci/hci/controller/bluenrg_hal_aci.c	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/bluenrg-hci/hci/controller/bluenrg_hal_aci.c	Thu Sep 15 16:59:44 2016 +0100
@@ -13,12 +13,12 @@
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
 
-#include "hal_types.h"
-#include "osal.h"
+#include "ble_hal_types.h"
+#include "ble_osal.h"
 #include "ble_status.h"
-#include "hal.h"
-#include "osal.h"
-#include "hci_const.h"
+#include "ble_hal.h"
+#include "ble_osal.h"
+#include "ble_hci_const.h"
 #include "bluenrg_aci_const.h"
 #include "bluenrg_hal_aci.h"
 #include "bluenrg_gatt_server.h"
@@ -180,4 +180,3 @@
 }
 
 
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/controller/bluenrg_l2cap_aci.c
--- a/source/bluenrg-hci/hci/controller/bluenrg_l2cap_aci.c	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/bluenrg-hci/hci/controller/bluenrg_l2cap_aci.c	Thu Sep 15 16:59:44 2016 +0100
@@ -13,12 +13,12 @@
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
 
-#include "hal_types.h"
-#include "osal.h"
+#include "ble_hal_types.h"
+#include "ble_osal.h"
 #include "ble_status.h"
-#include "hal.h"
-#include "osal.h"
-#include "hci_const.h"
+#include "ble_hal.h"
+#include "ble_osal.h"
+#include "ble_hci_const.h"
 #include "bluenrg_aci_const.h"
 #include "bluenrg_hal_aci.h"
 #include "bluenrg_gap.h"
@@ -115,4 +115,4 @@
     return BLE_STATUS_TIMEOUT;
 
   return status;
-}
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/controller/bluenrg_updater_aci.c
--- a/source/bluenrg-hci/hci/controller/bluenrg_updater_aci.c	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/bluenrg-hci/hci/controller/bluenrg_updater_aci.c	Thu Sep 15 16:59:44 2016 +0100
@@ -13,12 +13,12 @@
 * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *******************************************************************************/
 
-#include "hal_types.h"
-#include "osal.h"
+#include "ble_hal_types.h"
+#include "ble_osal.h"
 #include "ble_status.h"
-#include "hal.h"
-#include "osal.h"
-#include "hci_const.h"
+#include "ble_hal.h"
+#include "ble_osal.h"
+#include "ble_hci_const.h"
 #include "bluenrg_aci_const.h"
 #include "bluenrg_updater_aci.h"
 
@@ -267,4 +267,3 @@
 
 
 
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/controller/bluenrg_utils.c
--- a/source/bluenrg-hci/hci/controller/bluenrg_utils.c	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/bluenrg-hci/hci/controller/bluenrg_utils.c	Thu Sep 15 16:59:44 2016 +0100
@@ -1,11 +1,11 @@
 
-#include "hal.h"
-#include "hal_types.h"
+#include "ble_hal.h"
+#include "ble_hal_types.h"
 #include "ble_status.h"
 #include "bluenrg_aci.h"
 #include "bluenrg_utils.h"
-#include "hci.h"
-#include "osal.h"
+#include "ble_hci.h"
+#include "ble_osal.h"
 #include "string.h"
 #include "stm32_bluenrg_ble.h"
 
@@ -427,4 +427,4 @@
     return 0;
 
   return 1;
-}
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/hci/hci.c
--- a/source/bluenrg-hci/hci/hci.c	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1232 +0,0 @@
-/**
-  ******************************************************************************
-  * @file    hci.c 
-  * @author  AMS/HESA Application Team
-  * @brief   Function for managing HCI interface.
-  ******************************************************************************
-  *
-  *
-  * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-  * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
-  * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
-  * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
-  * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
-  * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-  *
-  * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  */ 
-
-#include "hal_types.h"
-#include "osal.h"
-#include "ble_status.h"
-#include "hal.h"
-#include "hci_const.h"
-#include "gp_timer.h"
-#include "debug.h"
-
-#include "stm32_bluenrg_ble.h"
-
-#if BLE_CONFIG_DBG_ENABLE
-#undef PRINTF
-#endif
-
-#define HCI_LOG_ON 0
-
-#define HCI_READ_PACKET_NUM_MAX 		 (0x40)
-
-#define MIN(a,b)            ((a) < (b) )? (a) : (b)
-#define MAX(a,b)            ((a) > (b) )? (a) : (b)
-
-tListNode hciReadPktPool;
-tListNode hciReadPktRxQueue;
-
-// betzw - DEBUG:
-//#define POOL_CNT
-#ifdef POOL_CNT
-#include <stdio.h>
-static unsigned int nr_hciReadPktPool;
-static unsigned int lowest_nr_hciReadPktPool;
-#endif // POOL_CNT
-
-/* pool of hci read packets */
-static tHciDataPacket   hciReadPacketBuffer[HCI_READ_PACKET_NUM_MAX];
-
-static volatile uint8_t readPacketListFull=FALSE;
-
-static volatile uint8_t hci_timer_id;
-static volatile uint8_t hci_timeout;
-
-void hci_timeout_callback(void)
-{
-  hci_timeout = 1;
-  return;
-}
-
-void HCI_Init(void)
-{
-  uint8_t index;
-  
-  Disable_SPI_IRQ();
-
-#ifdef POOL_CNT
-  nr_hciReadPktPool = 0;
-#endif // POOL_CNT
-
-  /* Initialize list heads of ready and free hci data packet queues */
-  list_init_head (&hciReadPktPool);
-  list_init_head (&hciReadPktRxQueue);
-  
-  /* Initialize the queue of free hci data packets */
-  for (index = 0; index < HCI_READ_PACKET_NUM_MAX; index++)
-  {
-    list_insert_tail(&hciReadPktPool, (tListNode *)&hciReadPacketBuffer[index]);
-#ifdef POOL_CNT
-    nr_hciReadPktPool++;
-#endif // POOL_CNT
-  }
-
-#ifdef POOL_CNT
-  lowest_nr_hciReadPktPool = nr_hciReadPktPool;
-#endif // POOL_CNT
-
-  Enable_SPI_IRQ();
-}
-
-#define HCI_PCK_TYPE_OFFSET                 0
-#define  EVENT_PARAMETER_TOT_LEN_OFFSET     2
-
-/**
- * Verify if HCI packet is correctly formatted..
- *
- * @param[in] hciReadPacket    The packet that is received from HCI interface.
- * @return 0 if HCI packet is as expected
- */
-int HCI_verify(const tHciDataPacket * hciReadPacket)
-{
-  const uint8_t *hci_pckt = hciReadPacket->dataBuff;
-  
-  if(hci_pckt[HCI_PCK_TYPE_OFFSET] != HCI_EVENT_PKT)
-    return 1;  /* Incorrect type. */
-  
-  if(hci_pckt[EVENT_PARAMETER_TOT_LEN_OFFSET] != hciReadPacket->data_len - (1+HCI_EVENT_HDR_SIZE))
-    return 2; /* Wrong length (packet truncated or too long). */
-  
-  return 0;      
-}
-
-void HCI_Process(void)
-{
-  uint8_t data_len;
-  uint8_t buffer[HCI_READ_PACKET_SIZE];
-  tHciDataPacket * hciReadPacket = NULL;
-  
-#ifdef POOL_CNT
-  printf("betzw(%s, %d): nr_hciReadPktPool = %u (lowest = %u)\r\n", __func__, __LINE__, 
-	 nr_hciReadPktPool, lowest_nr_hciReadPktPool);
-#endif // POOL_CNT
-
-  Disable_SPI_IRQ();
-  uint8_t list_empty = list_is_empty(&hciReadPktRxQueue);        
-  /* process any pending events read */
-  while(list_empty == FALSE)
-  {
-    list_remove_head (&hciReadPktRxQueue, (tListNode **)&hciReadPacket);
-    Enable_SPI_IRQ();
-    HCI_Event_CB(hciReadPacket->dataBuff);
-    Disable_SPI_IRQ();
-    list_insert_tail(&hciReadPktPool, (tListNode *)hciReadPacket);
-#ifdef POOL_CNT
-    nr_hciReadPktPool++;
-#endif
-    list_empty = list_is_empty(&hciReadPktRxQueue);
-  }
-  if (readPacketListFull) {
-    while(BlueNRG_DataPresent()) {
-      data_len = BlueNRG_SPI_Read_All(buffer, HCI_READ_PACKET_SIZE);
-      if(data_len > 0)
-        HCI_Event_CB(buffer);
-    }
-    readPacketListFull = FALSE;
-  }
-  
-  Enable_SPI_IRQ();    
-}
-
-BOOL HCI_Queue_Empty(void)
-{
-  return list_is_empty(&hciReadPktRxQueue);
-}
-
-void HCI_Isr(void)
-{
-  tHciDataPacket * hciReadPacket = NULL;
-  uint8_t data_len;
-  
-  Clear_SPI_EXTI_Flag();
-  while(BlueNRG_DataPresent()){        
-    if (list_is_empty (&hciReadPktPool) == FALSE){
-      
-      /* enqueueing a packet for read */
-      list_remove_head (&hciReadPktPool, (tListNode **)&hciReadPacket);
-#ifdef POOL_CNT
-      nr_hciReadPktPool--;
-      if(nr_hciReadPktPool < lowest_nr_hciReadPktPool)
-	      lowest_nr_hciReadPktPool = nr_hciReadPktPool;
-#endif
-
-      data_len = BlueNRG_SPI_Read_All(hciReadPacket->dataBuff, HCI_READ_PACKET_SIZE);
-      if(data_len > 0){                    
-        hciReadPacket->data_len = data_len;
-        if(HCI_verify(hciReadPacket) == 0) {
-          list_insert_tail(&hciReadPktRxQueue, (tListNode *)hciReadPacket);
-#ifdef AST_FOR_MBED_OS
-	  Call_BTLE_Handler();
-#endif
-        } else {
-          list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket);
-#ifdef POOL_CNT
-	  nr_hciReadPktPool++;
-#endif
-	}
-      }
-      else {
-        // Insert the packet back into the pool.
-        list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket);
-#ifdef POOL_CNT
-	nr_hciReadPktPool++;
-#endif
-      }
-    }
-    else{
-      // HCI Read Packet Pool is empty, wait for a free packet.
-#ifdef AST_FOR_MBED_OS
-      Call_BTLE_Handler();
-#endif
-      readPacketListFull = TRUE;
-      Clear_SPI_EXTI_Flag();
-      return;
-    }
-    Clear_SPI_EXTI_Flag();
-  }
-}
-
-void hci_write(const void* data1, const void* data2, uint8_t n_bytes1, uint8_t n_bytes2){
-#if  HCI_LOG_ON
-  PRINTF("HCI <- ");
-  for(int i=0; i < n_bytes1; i++)
-    PRINTF("%02X ", *((uint8_t*)data1 + i));
-  for(int i=0; i < n_bytes2; i++)
-    PRINTF("%02X ", *((uint8_t*)data2 + i));
-  PRINTF("\n");    
-#endif
-  
-  Hal_Write_Serial(data1, data2, n_bytes1, n_bytes2);
-}
-
-void hci_send_cmd(uint16_t ogf, uint16_t ocf, uint8_t plen, void *param)
-{
-  hci_command_hdr hc;
-  
-  hc.opcode = htobs(cmd_opcode_pack(ogf, ocf));
-  hc.plen= plen;
-  
-  uint8_t header[HCI_HDR_SIZE + HCI_COMMAND_HDR_SIZE];
-  header[0] = HCI_COMMAND_PKT;
-  Osal_MemCpy(header+1, &hc, sizeof(hc));
-  
-  hci_write(header, param, sizeof(header), plen);
-}
-
-static void move_list(tListNode * dest_list, tListNode * src_list)
-{
-  pListNode tmp_node;
-  
-  while(!list_is_empty(src_list)){
-    list_remove_head(src_list, &tmp_node);
-    list_insert_tail(dest_list, tmp_node);
-  }
-}
-
-int hci_send_req(struct hci_request *r, BOOL async)
-{
-  uint8_t *ptr;
-  uint16_t opcode = htobs(cmd_opcode_pack(r->ogf, r->ocf));
-  hci_event_pckt *event_pckt;
-  hci_uart_pckt *hci_hdr;
-  int to = DEFAULT_TIMEOUT;
-  struct timer t;
-  tHciDataPacket * hciReadPacket = NULL;
-  tListNode hciTempQueue;
-  
-  list_init_head(&hciTempQueue);
-  
-  hci_send_cmd(r->ogf, r->ocf, r->clen, r->cparam);
-  
-  if(async){
-    goto done;
-  }
-  
-  /* Minimum timeout is 1. */
-  if(to == 0)
-    to = 1;
-  
-  Timer_Set(&t, to);
-  
-  while(1) {
-    evt_cmd_complete *cc;
-    evt_cmd_status *cs;
-    evt_le_meta_event *me;
-    int len;
-      
-#if ENABLE_MICRO_SLEEP    
-    while(1){
-      ATOMIC_SECTION_BEGIN();
-      if(Timer_Expired(&t)){
-        ATOMIC_SECTION_END();
-        goto failed;
-      }
-      if(!HCI_Queue_Empty()){
-        ATOMIC_SECTION_END();
-        break;
-      }
-      Enter_Sleep_Mode();
-      ATOMIC_SECTION_END();
-    }
-#else
-    while(1){
-      if(Timer_Expired(&t)){
-        goto failed;
-      }
-      if(!HCI_Queue_Empty()){
-        break;
-      }
-    }
-#endif
-    
-    /* Extract packet from HCI event queue. */
-    Disable_SPI_IRQ();
-    list_remove_head(&hciReadPktRxQueue, (tListNode **)&hciReadPacket);    
-    
-    hci_hdr = (void *)hciReadPacket->dataBuff;
-    if(hci_hdr->type != HCI_EVENT_PKT){
-      list_insert_tail(&hciTempQueue, (tListNode *)hciReadPacket); // See comment below
-      Enable_SPI_IRQ();
-      continue;
-    }
-    
-    event_pckt = (void *) (hci_hdr->data);
-    
-    ptr = hciReadPacket->dataBuff + (1 + HCI_EVENT_HDR_SIZE);
-    len = hciReadPacket->data_len - (1 + HCI_EVENT_HDR_SIZE);
-    
-    switch (event_pckt->evt) {
-      
-    case EVT_CMD_STATUS:
-      cs = (void *) ptr;
-      
-      if (cs->opcode != opcode)
-        goto failed;
-      
-      if (r->event != EVT_CMD_STATUS) {
-        if (cs->status) {
-          goto failed;
-        }
-        break;
-      }
-      
-      r->rlen = MIN(len, r->rlen);
-      Osal_MemCpy(r->rparam, ptr, r->rlen);
-      goto done;
-      
-    case EVT_CMD_COMPLETE:
-      cc = (void *) ptr;
-      
-      if (cc->opcode != opcode)
-        goto failed;
-      
-      ptr += EVT_CMD_COMPLETE_SIZE;
-      len -= EVT_CMD_COMPLETE_SIZE;
-      
-      r->rlen = MIN(len, r->rlen);
-      Osal_MemCpy(r->rparam, ptr, r->rlen);
-      goto done;
-      
-    case EVT_LE_META_EVENT:
-      me = (void *) ptr;
-      
-      if (me->subevent != r->event)
-        break;
-      
-      len -= 1;
-      r->rlen = MIN(len, r->rlen);
-      Osal_MemCpy(r->rparam, me->data, r->rlen);
-      goto done;
-      
-    case EVT_HARDWARE_ERROR:            
-      goto failed;
-      
-    default:      
-      break;
-    }
-    
-    /* In the meantime there could be other events from the controller.
-       In this case, insert the packet in a different queue. These packets will be
-       inserted back in the main queue just before exiting from send_req().
-    */
-    if(hciReadPacket != NULL) {
-	    list_insert_tail(&hciTempQueue, (tListNode *)hciReadPacket);
-	    hciReadPacket = NULL;
-    }
-    /* Be sure there is at list one packet in the pool to process the expected event. */
-    if(list_is_empty(&hciReadPktPool)){ // betzw: this is a kind of steeling (should never happen?!?)
-      pListNode tmp_node;      
-      list_remove_head(&hciReadPktRxQueue, &tmp_node);
-      list_insert_tail(&hciReadPktPool, tmp_node);     
-#ifdef POOL_CNT
-      nr_hciReadPktPool++;
-#endif
-    }
-    
-    Enable_SPI_IRQ();
-    
-  }
-  
-failed: 
-  // Insert the packet back into the pool.
-  if(hciReadPacket != NULL) {
-	  list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket); 
-#ifdef POOL_CNT
-	  nr_hciReadPktPool++;
-#endif
-	  hciReadPacket = NULL;
-  }
-  move_list(&hciReadPktRxQueue, &hciTempQueue);  
-  Enable_SPI_IRQ();
-  return -1;
-  
-done:
-  // Insert the packet back into the pool.
-  if(hciReadPacket != NULL) {
-	  list_insert_head(&hciReadPktPool, (tListNode *)hciReadPacket); 
-#ifdef POOL_CNT
-	  nr_hciReadPktPool++;
-#endif
-	  hciReadPacket = NULL;
-  }
-  move_list(&hciReadPktRxQueue, &hciTempQueue);
-  
-  Enable_SPI_IRQ();
-  return 0;
-}
-
-int hci_reset()
-{
-  struct hci_request rq;
-  uint8_t status;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_HOST_CTL;
-  rq.ocf = OCF_RESET;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;  
-}
-
-int hci_disconnect(uint16_t	handle, uint8_t reason)
-{
-  struct hci_request rq;
-  disconnect_cp cp;
-  uint8_t status;
-  
-  cp.handle = handle;
-  cp.reason = reason;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LINK_CTL;
-  rq.ocf = OCF_DISCONNECT;
-  rq.cparam = &cp;
-  rq.clen = DISCONNECT_CP_SIZE;
-  rq.event = EVT_CMD_STATUS;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;  
-}
-
-int hci_le_read_local_version(uint8_t *hci_version, uint16_t *hci_revision, uint8_t *lmp_pal_version, 
-                              uint16_t *manufacturer_name, uint16_t *lmp_pal_subversion)
-{
-  struct hci_request rq;
-  read_local_version_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_INFO_PARAM;
-  rq.ocf = OCF_READ_LOCAL_VERSION;
-  rq.cparam = NULL;
-  rq.clen = 0;
-  rq.rparam = &resp;
-  rq.rlen = READ_LOCAL_VERSION_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  
-  *hci_version = resp.hci_version;
-  *hci_revision =  btohs(resp.hci_revision);
-  *lmp_pal_version = resp.lmp_pal_version;
-  *manufacturer_name = btohs(resp.manufacturer_name);
-  *lmp_pal_subversion = btohs(resp.lmp_pal_subversion);
-  
-  return 0;
-}
-
-int hci_le_read_buffer_size(uint16_t *pkt_len, uint8_t *max_pkt)
-{
-  struct hci_request rq;
-  le_read_buffer_size_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_READ_BUFFER_SIZE;
-  rq.cparam = NULL;
-  rq.clen = 0;
-  rq.rparam = &resp;
-  rq.rlen = LE_READ_BUFFER_SIZE_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  *pkt_len = resp.pkt_len;
-  *max_pkt = resp.max_pkt;
-  
-  return 0;
-}
-
-int hci_le_set_advertising_parameters(uint16_t min_interval, uint16_t max_interval, uint8_t advtype,
-                                      uint8_t own_bdaddr_type, uint8_t direct_bdaddr_type, const tBDAddr direct_bdaddr, uint8_t chan_map,
-                                      uint8_t filter)
-{
-  struct hci_request rq;
-  le_set_adv_parameters_cp adv_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&adv_cp, 0, sizeof(adv_cp));
-  adv_cp.min_interval = min_interval;
-  adv_cp.max_interval = max_interval;
-  adv_cp.advtype = advtype;
-  adv_cp.own_bdaddr_type = own_bdaddr_type;
-  adv_cp.direct_bdaddr_type = direct_bdaddr_type;
-  if(direct_bdaddr != NULL)
-    Osal_MemCpy(adv_cp.direct_bdaddr,direct_bdaddr,sizeof(adv_cp.direct_bdaddr));
-  adv_cp.chan_map = chan_map;
-  adv_cp.filter = filter;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_SET_ADV_PARAMETERS;
-  rq.cparam = &adv_cp;
-  rq.clen = LE_SET_ADV_PARAMETERS_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_le_set_advertising_data(uint8_t length, const uint8_t data[])
-{
-  struct hci_request rq;
-  le_set_adv_data_cp adv_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&adv_cp, 0, sizeof(adv_cp));
-  adv_cp.length = length;
-  Osal_MemCpy(adv_cp.data, data, MIN(31,length));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_SET_ADV_DATA;
-  rq.cparam = &adv_cp;
-  rq.clen = LE_SET_ADV_DATA_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_le_set_advertise_enable(uint8_t enable)
-{
-  struct hci_request rq;
-  le_set_advertise_enable_cp adv_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&adv_cp, 0, sizeof(adv_cp));
-  adv_cp.enable = enable?1:0;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_SET_ADVERTISE_ENABLE;
-  rq.cparam = &adv_cp;
-  rq.clen = LE_SET_ADVERTISE_ENABLE_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_le_set_scan_parameters(uint8_t	type, uint16_t interval,
-                               uint16_t window, uint8_t own_bdaddr_type,
-                               uint8_t	filter)
-{
-  struct hci_request rq;
-  le_set_scan_parameters_cp scan_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&scan_cp, 0, sizeof(scan_cp));
-  scan_cp.type = type;
-  scan_cp.interval = interval;
-  scan_cp.window = window;
-  scan_cp.own_bdaddr_type = own_bdaddr_type;
-  scan_cp.filter = filter;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_SET_SCAN_PARAMETERS;
-  rq.cparam = &scan_cp;
-  rq.clen = LE_SET_SCAN_PARAMETERS_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_le_set_scan_enable(uint8_t enable, uint8_t filter_dup)
-{
-  struct hci_request rq;
-  le_set_scan_enable_cp scan_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&scan_cp, 0, sizeof(scan_cp));
-  scan_cp.enable = enable?1:0;
-  scan_cp.filter_dup = filter_dup;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_SET_SCAN_ENABLE;
-  rq.cparam = &scan_cp;
-  rq.clen = LE_SET_SCAN_ENABLE_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_le_rand(uint8_t random_number[8])
-{
-  struct hci_request rq;
-  le_rand_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_RAND;
-  rq.cparam = NULL;
-  rq.clen = 0;
-  rq.rparam = &resp;
-  rq.rlen = LE_RAND_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  Osal_MemCpy(random_number, resp.random, 8);
-  
-  return 0;
-}
-
-int hci_le_set_scan_resp_data(uint8_t length, const uint8_t data[])
-{
-  struct hci_request rq;
-  le_set_scan_response_data_cp scan_resp_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&scan_resp_cp, 0, sizeof(scan_resp_cp));
-  scan_resp_cp.length = length;
-  Osal_MemCpy(scan_resp_cp.data, data, MIN(31,length));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_SET_SCAN_RESPONSE_DATA;
-  rq.cparam = &scan_resp_cp;
-  rq.clen = LE_SET_SCAN_RESPONSE_DATA_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_le_read_advertising_channel_tx_power(int8_t *tx_power_level)
-{
-  struct hci_request rq;
-  le_read_adv_channel_tx_power_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_READ_ADV_CHANNEL_TX_POWER;
-  rq.cparam = NULL;
-  rq.clen = 0;
-  rq.rparam = &resp;
-  rq.rlen = LE_RAND_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  *tx_power_level = resp.level;
-  
-  return 0;
-}
-
-int hci_le_set_random_address(tBDAddr bdaddr)
-{
-  struct hci_request rq;
-  le_set_random_address_cp set_rand_addr_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&set_rand_addr_cp, 0, sizeof(set_rand_addr_cp));
-  Osal_MemCpy(set_rand_addr_cp.bdaddr, bdaddr, sizeof(tBDAddr));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_SET_RANDOM_ADDRESS;
-  rq.cparam = &set_rand_addr_cp;
-  rq.clen = LE_SET_RANDOM_ADDRESS_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_read_bd_addr(tBDAddr bdaddr)
-{
-  struct hci_request rq;
-  read_bd_addr_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_INFO_PARAM;
-  rq.ocf = OCF_READ_BD_ADDR;
-  rq.cparam = NULL;
-  rq.clen = 0;
-  rq.rparam = &resp;
-  rq.rlen = READ_BD_ADDR_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  Osal_MemCpy(bdaddr, resp.bdaddr, sizeof(tBDAddr));
-  
-  return 0;
-}
-
-int hci_le_create_connection(uint16_t interval,	uint16_t window, uint8_t initiator_filter, uint8_t peer_bdaddr_type,
-                             const tBDAddr peer_bdaddr,	uint8_t	own_bdaddr_type, uint16_t min_interval,	uint16_t max_interval,
-                             uint16_t latency,	uint16_t supervision_timeout, uint16_t min_ce_length, uint16_t max_ce_length)
-{
-  struct hci_request rq;
-  le_create_connection_cp create_cp;
-  uint8_t status;
-  
-  Osal_MemSet(&create_cp, 0, sizeof(create_cp));
-  create_cp.interval = interval;
-  create_cp.window =  window;
-  create_cp.initiator_filter = initiator_filter;
-  create_cp.peer_bdaddr_type = peer_bdaddr_type;
-  Osal_MemCpy(create_cp.peer_bdaddr, peer_bdaddr, sizeof(tBDAddr));
-  create_cp.own_bdaddr_type = own_bdaddr_type;
-  create_cp.min_interval=min_interval;
-  create_cp.max_interval=max_interval;
-  create_cp.latency = latency;
-  create_cp.supervision_timeout=supervision_timeout;
-  create_cp.min_ce_length=min_ce_length;
-  create_cp.max_ce_length=max_ce_length;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_CREATE_CONN;
-  rq.cparam = &create_cp;
-  rq.clen = LE_CREATE_CONN_CP_SIZE;
-  rq.event = EVT_CMD_STATUS;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return status;
-}
-
-int hci_le_create_connection_cancel(void)
-{
-  struct hci_request rq;
-  uint8_t status;
-
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_CREATE_CONN_CANCEL;
-  rq.rparam = &status;
-  rq.rlen = 1;
-
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-
-  return status;
-}
-
-int hci_le_encrypt(uint8_t key[16], uint8_t plaintextData[16], uint8_t encryptedData[16])
-{
-  struct hci_request rq;
-  le_encrypt_cp params;
-  le_encrypt_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemCpy(params.key, key, 16);
-  Osal_MemCpy(params.plaintext, plaintextData, 16);
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_ENCRYPT;
-  rq.cparam = &params;
-  rq.clen = LE_ENCRYPT_CP_SIZE;
-  rq.rparam = &resp;
-  rq.rlen = LE_ENCRYPT_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  Osal_MemCpy(encryptedData, resp.encdata, 16);
-  
-  return 0;
-}
-
-int hci_le_ltk_request_reply(uint8_t key[16])
-{
-  struct hci_request rq;
-  le_ltk_reply_cp params;
-  le_ltk_reply_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  params.handle = 1;
-  Osal_MemCpy(params.key, key, 16);
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_LTK_REPLY;
-  rq.cparam = &params;
-  rq.clen = LE_LTK_REPLY_CP_SIZE;
-  rq.rparam = &resp;
-  rq.rlen = LE_LTK_REPLY_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return resp.status;
-}
-
-int hci_le_ltk_request_neg_reply()
-{
-  struct hci_request rq;
-  le_ltk_neg_reply_cp params;
-  le_ltk_neg_reply_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  params.handle = 1;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_LTK_NEG_REPLY;
-  rq.cparam = &params;
-  rq.clen = LE_LTK_NEG_REPLY_CP_SIZE;
-  rq.rparam = &resp;
-  rq.rlen = LE_LTK_NEG_REPLY_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0)
-    return BLE_STATUS_TIMEOUT;
-  
-  return resp.status;
-}
-
-int hci_le_read_white_list_size(uint8_t *size)
-{
-  struct hci_request rq;
-  le_read_white_list_size_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_READ_WHITE_LIST_SIZE;
-  rq.rparam = &resp;
-  rq.rlen = LE_READ_WHITE_LIST_SIZE_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  *size = resp.size;
-  
-  return 0;
-}
-
-int hci_le_clear_white_list()
-{
-  struct hci_request rq;
-  uint8_t status;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_CLEAR_WHITE_LIST;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  return status;
-}
-
-int hci_le_add_device_to_white_list(uint8_t	bdaddr_type, tBDAddr bdaddr)
-{
-  struct hci_request rq;
-  le_add_device_to_white_list_cp params;
-  uint8_t status;
-  
-  params.bdaddr_type = bdaddr_type;
-  Osal_MemCpy(params.bdaddr, bdaddr, 6);
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_ADD_DEVICE_TO_WHITE_LIST;
-  rq.cparam = &params;
-  rq.clen = LE_ADD_DEVICE_TO_WHITE_LIST_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  return status;
-}
-
-int hci_le_remove_device_from_white_list(uint8_t bdaddr_type, tBDAddr bdaddr)
-{
-  struct hci_request rq;
-  le_remove_device_from_white_list_cp params;
-  uint8_t status;
-  
-  params.bdaddr_type = bdaddr_type;
-  Osal_MemCpy(params.bdaddr, bdaddr, 6);
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST;
-  rq.cparam = &params;
-  rq.clen = LE_REMOVE_DEVICE_FROM_WHITE_LIST_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  return status;
-}
-
-int hci_read_transmit_power_level(uint16_t *conn_handle, uint8_t type, int8_t * tx_level)
-{
-  struct hci_request rq;
-  read_transmit_power_level_cp params;
-  read_transmit_power_level_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  params.handle = *conn_handle;
-  params.type = type;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_HOST_CTL;
-  rq.ocf = OCF_READ_TRANSMIT_POWER_LEVEL;
-  rq.cparam = &params;
-  rq.clen = READ_TRANSMIT_POWER_LEVEL_CP_SIZE;
-  rq.rparam = &resp;
-  rq.rlen = READ_TRANSMIT_POWER_LEVEL_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  *conn_handle = resp.handle;
-  *tx_level = resp.level;
-  
-  return 0;
-}
-
-int hci_read_rssi(uint16_t *conn_handle, int8_t * rssi)
-{
-  struct hci_request rq;
-  read_rssi_cp params;
-  read_rssi_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  params.handle = *conn_handle;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_STATUS_PARAM;
-  rq.ocf = OCF_READ_RSSI;
-  rq.cparam = &params;
-  rq.clen = READ_RSSI_CP_SIZE;
-  rq.rparam = &resp;
-  rq.rlen = READ_RSSI_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  *conn_handle = resp.handle;
-  *rssi = resp.rssi;
-  
-  return 0;
-}
-
-int hci_le_read_local_supported_features(uint8_t *features)
-{
-  struct hci_request rq;
-  le_read_local_supported_features_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_READ_LOCAL_SUPPORTED_FEATURES;
-  rq.rparam = &resp;
-  rq.rlen = LE_READ_LOCAL_SUPPORTED_FEATURES_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  Osal_MemCpy(features, resp.features, sizeof(resp.features));
-  
-  return 0;
-}
-
-int hci_le_read_channel_map(uint16_t conn_handle, uint8_t ch_map[5])
-{
-  struct hci_request rq;
-  le_read_channel_map_cp params;
-  le_read_channel_map_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  params.handle = conn_handle;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_READ_CHANNEL_MAP;
-  rq.cparam = &params;
-  rq.clen = LE_READ_CHANNEL_MAP_CP_SIZE;
-  rq.rparam = &resp;
-  rq.rlen = LE_READ_CHANNEL_MAP_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  Osal_MemCpy(ch_map, resp.map, 5);
-  
-  return 0;
-}
-
-int hci_le_read_supported_states(uint8_t states[8])
-{
-  struct hci_request rq;
-  le_read_supported_states_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_READ_SUPPORTED_STATES;
-  rq.rparam = &resp;
-  rq.rlen = LE_READ_SUPPORTED_STATES_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  Osal_MemCpy(states, resp.states, 8);
-  
-  return 0;
-}
-
-int hci_le_receiver_test(uint8_t frequency)
-{
-  struct hci_request rq;
-  le_receiver_test_cp params;
-  uint8_t status;
-  
-  params.frequency = frequency;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_RECEIVER_TEST;
-  rq.cparam = &params;
-  rq.clen = LE_RECEIVER_TEST_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  return status;
-}
-
-int hci_le_transmitter_test(uint8_t frequency, uint8_t length, uint8_t payload)
-{
-  struct hci_request rq;
-  le_transmitter_test_cp params;
-  uint8_t status;
-  
-  params.frequency = frequency;
-  params.length = length;
-  params.payload = payload;
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_TRANSMITTER_TEST;
-  rq.cparam = &params;
-  rq.clen = LE_TRANSMITTER_TEST_CP_SIZE;
-  rq.rparam = &status;
-  rq.rlen = 1;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  return status;
-}
-
-int hci_le_test_end(uint16_t *num_pkts)
-{
-  struct hci_request rq;
-  le_test_end_rp resp;
-  
-  Osal_MemSet(&resp, 0, sizeof(resp));
-  
-  Osal_MemSet(&rq, 0, sizeof(rq));
-  rq.ogf = OGF_LE_CTL;
-  rq.ocf = OCF_LE_TEST_END;
-  rq.rparam = &resp;
-  rq.rlen = LE_TEST_END_RP_SIZE;
-  
-  if (hci_send_req(&rq, FALSE) < 0){
-    return BLE_STATUS_TIMEOUT;
-  }
-  
-  if (resp.status) {
-    return resp.status;
-  }
-  
-  *num_pkts = resp.num_pkts;
-  
-  return 0;
-}
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/utils/ble_gp_timer.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/bluenrg-hci/utils/ble_gp_timer.c	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2004, Swedish Institute of Computer Science.
+ * 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 Institute 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 INSTITUTE 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 INSTITUTE 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.
+ *
+ * This file is part of the Contiki operating system.
+ *
+ * Author: Adam Dunkels <adam@sics.se>
+ *
+ */
+
+#include "ble_clock.h"
+#include "ble_gp_timer.h"
+
+/*---------------------------------------------------------------------------*/
+/**
+ * Set a timer.
+ *
+ * This function sets a timer for a time sometime in the
+ * future. The function timer_expired() will evaluate to true after
+ * the timer has expired.
+ *
+ * @param[in] t         A pointer to the timer
+ * @param[in] interval  The interval before the timer expires.
+ *
+ */
+void
+Timer_Set(struct timer *t, tClockTime interval)
+{
+  t->interval = interval;
+  t->start = Clock_Time();
+}
+/*---------------------------------------------------------------------------*/
+/**
+ * Reset the timer with the same interval.
+ *
+ * This function resets the timer with the same interval that was
+ * given to the timer_set() function. The start point of the interval
+ * is the exact time that the timer last expired. Therefore, this
+ * function will cause the timer to be stable over time, unlike the
+ * timer_restart() function.
+ *
+ * \param t A pointer to the timer.
+ *
+ * \sa timer_restart()
+ */
+void
+Timer_Reset(struct timer *t)
+{
+  t->start += t->interval;
+}
+/*---------------------------------------------------------------------------*/
+/**
+ * Restart the timer from the current point in time
+ *
+ * This function restarts a timer with the same interval that was
+ * given to the timer_set() function. The timer will start at the
+ * current time.
+ *
+ * \note A periodic timer will drift if this function is used to reset
+ * it. For preioric timers, use the timer_reset() function instead.
+ *
+ * \param t A pointer to the timer.
+ *
+ * \sa timer_reset()
+ */
+void
+Timer_Restart(struct timer *t)
+{
+  t->start = Clock_Time();
+}
+/*---------------------------------------------------------------------------*/
+/**
+ * Check if a timer has expired.
+ *
+ * This function tests if a timer has expired and returns true or
+ * false depending on its status.
+ *
+ * \param t A pointer to the timer
+ *
+ * \return Non-zero if the timer has expired, zero otherwise.
+ *
+ */
+int
+Timer_Expired(struct timer *t)
+{
+  /* Note: Can not return diff >= t->interval so we add 1 to diff and return
+     t->interval < diff - required to avoid an internal error in mspgcc. */
+  tClockTime diff = (Clock_Time() - t->start) + 1;
+  return t->interval < diff;
+
+}
+/*---------------------------------------------------------------------------*/
+/**
+ * The time until the timer expires
+ *
+ * This function returns the time until the timer expires.
+ *
+ * \param t A pointer to the timer
+ *
+ * \return The time until the timer expires
+ *
+ */
+tClockTime
+Timer_Remaining(struct timer *t)
+{
+  return t->start + t->interval - Clock_Time();
+}
+/*---------------------------------------------------------------------------*/
+#ifdef __DMA_LP__
+
+tBleStatus Blue_NRG_HCI_Timer_Start(uint32_t expiryTime,
+                                    TIMER_HCI_TIMEOUT_NOTIFY_CALLBACK_TYPE timercb,
+                                    uint8_t *timerID)
+{
+  TIMER_Create(eTimerModuleID_BlueNRG_HCI, timerID, eTimerMode_SingleShot,
+               (pf_TIMER_TimerCallBack_t) timercb);
+  TIMER_Start(*timerID, expiryTime*1000/TIMERSERVER_TICK_VALUE);
+  
+  return (BLE_STATUS_SUCCESS);
+}
+
+/*---------------------------------------------------------------------------*/
+tBleStatus Blue_NRG_HCI_Timer_Stop(uint8_t timerID)
+{
+  TIMER_Delete(timerID);
+  
+  return (BLE_STATUS_SUCCESS);
+}
+
+#endif /* __DMA_LP__ */
+/*---------------------------------------------------------------------------*/
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/utils/ble_list.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/bluenrg-hci/utils/ble_list.c	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,118 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : ble_list.c
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : Circular Linked List Implementation.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+/******************************************************************************
+ * Include Files
+******************************************************************************/
+#include <ble_hal_types.h>
+#include "ble_list.h"
+
+/******************************************************************************
+ * Function Definitions 
+******************************************************************************/
+void list_init_head (tListNode * listHead)
+{
+	listHead->next = listHead;
+	listHead->prev = listHead;	
+}
+
+uint8_t list_is_empty (tListNode * listHead)
+{
+	return ((listHead->next == listHead)? TRUE:FALSE);
+}
+
+void list_insert_head (tListNode * listHead, tListNode * node)
+{
+	node->next = listHead->next;
+	node->prev = listHead;
+	listHead->next = node;
+	(node->next)->prev = node;
+}
+
+
+void list_insert_tail (tListNode * listHead, tListNode * node)
+{
+	node->next = listHead;
+	node->prev = listHead->prev;
+	listHead->prev = node;
+	(node->prev)->next = node;
+}
+
+
+void list_remove_node (tListNode * node)
+{
+	(node->prev)->next = node->next;
+	(node->next)->prev = node->prev;
+}
+
+
+void list_remove_head (tListNode * listHead, tListNode ** node )
+{
+	*node = listHead->next;
+	list_remove_node (listHead->next);
+	(*node)->next = NULL;
+	(*node)->prev = NULL;
+}
+
+
+void list_remove_tail (tListNode * listHead, tListNode ** node )
+{
+	*node = listHead->prev;
+	list_remove_node (listHead->prev);
+	(*node)->next = NULL;
+	(*node)->prev = NULL;
+}
+
+
+void list_insert_node_after (tListNode * node, tListNode * ref_node)
+{
+	node->next = ref_node->next;
+	node->prev = ref_node;
+	ref_node->next = node;
+	(node->next)->prev = node;
+}
+
+
+void list_insert_node_before (tListNode * node, tListNode * ref_node)
+{
+	node->next = ref_node;
+	node->prev = ref_node->prev;
+	ref_node->prev = node;
+	(node->prev)->next = node;
+}
+
+
+int list_get_size (tListNode * listHead)
+{
+	int size = 0;
+	tListNode * temp = listHead->next;
+	while (temp != listHead)
+	{
+		size++;
+		temp = temp->next;		
+	}
+	return (size);
+}
+
+void list_get_next_node (tListNode * ref_node, tListNode ** node)
+{
+    *node = ref_node->next;
+}
+
+
+void list_get_prev_node (tListNode * ref_node, tListNode ** node)
+{
+    *node = ref_node->prev;
+}
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/utils/ble_osal.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/bluenrg-hci/utils/ble_osal.c	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,72 @@
+/**
+******************************************************************************
+* @file    ble_osal.c 
+* @author  AMS - HEA&RF BU / CL
+* @version V1.0.0
+* @date    04-July-2014
+* @brief   Implementation of OS abstraction layer functions used by the
+*          library.
+******************************************************************************
+* @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
+  *
+  * 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 STMicroelectronics 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.
+  *
+  ******************************************************************************
+  */
+  
+/* Includes ------------------------------------------------------------------*/
+#include <string.h>
+#include <ble_osal.h>
+ 
+ /**
+ * @brief  Osal_MemCpy
+ * @param  dest: Pointer to the destination buffer
+ * @param  src : Pointer to the source buffer
+ * @param  size: Number of bytes to copy from the source to the destination
+ *               buffer
+ * @retval Pointer to the destination buffer
+ */
+void* Osal_MemCpy(void *dest, const void *src, unsigned int size)
+{
+    return(memcpy(dest,src,size)); 
+}
+
+/**
+ * @brief  Osal_MemSet
+ * @param  ptr  : Pointer to block of memory to fill  
+ * @param  value: Value to assign to each byte of the memory block
+ * @param  size : Number of bytes to be set to "value"
+ * @retval Pointer to the filled block of memory
+ */
+void* Osal_MemSet(void *ptr, int value, unsigned int size)
+{
+    return(memset(ptr,value,size));
+}
+
+/******************************************************************************
+ * local Functions
+ *****************************************************************************/ 
+ 
+ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/utils/gp_timer.c
--- a/source/bluenrg-hci/utils/gp_timer.c	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,155 +0,0 @@
-/*
- * Copyright (c) 2004, Swedish Institute of Computer Science.
- * 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 Institute 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 INSTITUTE 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 INSTITUTE 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.
- *
- * This file is part of the Contiki operating system.
- *
- * Author: Adam Dunkels <adam@sics.se>
- *
- */
-
-#include "clock.h"
-#include "gp_timer.h"
-
-/*---------------------------------------------------------------------------*/
-/**
- * Set a timer.
- *
- * This function sets a timer for a time sometime in the
- * future. The function timer_expired() will evaluate to true after
- * the timer has expired.
- *
- * @param[in] t         A pointer to the timer
- * @param[in] interval  The interval before the timer expires.
- *
- */
-void
-Timer_Set(struct timer *t, tClockTime interval)
-{
-  t->interval = interval;
-  t->start = Clock_Time();
-}
-/*---------------------------------------------------------------------------*/
-/**
- * Reset the timer with the same interval.
- *
- * This function resets the timer with the same interval that was
- * given to the timer_set() function. The start point of the interval
- * is the exact time that the timer last expired. Therefore, this
- * function will cause the timer to be stable over time, unlike the
- * timer_restart() function.
- *
- * \param t A pointer to the timer.
- *
- * \sa timer_restart()
- */
-void
-Timer_Reset(struct timer *t)
-{
-  t->start += t->interval;
-}
-/*---------------------------------------------------------------------------*/
-/**
- * Restart the timer from the current point in time
- *
- * This function restarts a timer with the same interval that was
- * given to the timer_set() function. The timer will start at the
- * current time.
- *
- * \note A periodic timer will drift if this function is used to reset
- * it. For preioric timers, use the timer_reset() function instead.
- *
- * \param t A pointer to the timer.
- *
- * \sa timer_reset()
- */
-void
-Timer_Restart(struct timer *t)
-{
-  t->start = Clock_Time();
-}
-/*---------------------------------------------------------------------------*/
-/**
- * Check if a timer has expired.
- *
- * This function tests if a timer has expired and returns true or
- * false depending on its status.
- *
- * \param t A pointer to the timer
- *
- * \return Non-zero if the timer has expired, zero otherwise.
- *
- */
-int
-Timer_Expired(struct timer *t)
-{
-  /* Note: Can not return diff >= t->interval so we add 1 to diff and return
-     t->interval < diff - required to avoid an internal error in mspgcc. */
-  tClockTime diff = (Clock_Time() - t->start) + 1;
-  return t->interval < diff;
-
-}
-/*---------------------------------------------------------------------------*/
-/**
- * The time until the timer expires
- *
- * This function returns the time until the timer expires.
- *
- * \param t A pointer to the timer
- *
- * \return The time until the timer expires
- *
- */
-tClockTime
-Timer_Remaining(struct timer *t)
-{
-  return t->start + t->interval - Clock_Time();
-}
-/*---------------------------------------------------------------------------*/
-#ifdef __DMA_LP__
-
-tBleStatus Blue_NRG_HCI_Timer_Start(uint32_t expiryTime,
-                                    TIMER_HCI_TIMEOUT_NOTIFY_CALLBACK_TYPE timercb,
-                                    uint8_t *timerID)
-{
-  TIMER_Create(eTimerModuleID_BlueNRG_HCI, timerID, eTimerMode_SingleShot,
-               (pf_TIMER_TimerCallBack_t) timercb);
-  TIMER_Start(*timerID, expiryTime*1000/TIMERSERVER_TICK_VALUE);
-  
-  return (BLE_STATUS_SUCCESS);
-}
-
-/*---------------------------------------------------------------------------*/
-tBleStatus Blue_NRG_HCI_Timer_Stop(uint8_t timerID)
-{
-  TIMER_Delete(timerID);
-  
-  return (BLE_STATUS_SUCCESS);
-}
-
-#endif /* __DMA_LP__ */
-/*---------------------------------------------------------------------------*/
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/utils/list.c
--- a/source/bluenrg-hci/utils/list.c	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,119 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : list.c
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : Circular Linked List Implementation.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-/******************************************************************************
- * Include Files
-******************************************************************************/
-#include <hal_types.h>
-#include "list.h"
-
-/******************************************************************************
- * Function Definitions 
-******************************************************************************/
-void list_init_head (tListNode * listHead)
-{
-	listHead->next = listHead;
-	listHead->prev = listHead;	
-}
-
-uint8_t list_is_empty (tListNode * listHead)
-{
-	return ((listHead->next == listHead)? TRUE:FALSE);
-}
-
-void list_insert_head (tListNode * listHead, tListNode * node)
-{
-	node->next = listHead->next;
-	node->prev = listHead;
-	listHead->next = node;
-	(node->next)->prev = node;
-}
-
-
-void list_insert_tail (tListNode * listHead, tListNode * node)
-{
-	node->next = listHead;
-	node->prev = listHead->prev;
-	listHead->prev = node;
-	(node->prev)->next = node;
-}
-
-
-void list_remove_node (tListNode * node)
-{
-	(node->prev)->next = node->next;
-	(node->next)->prev = node->prev;
-}
-
-
-void list_remove_head (tListNode * listHead, tListNode ** node )
-{
-	*node = listHead->next;
-	list_remove_node (listHead->next);
-	(*node)->next = NULL;
-	(*node)->prev = NULL;
-}
-
-
-void list_remove_tail (tListNode * listHead, tListNode ** node )
-{
-	*node = listHead->prev;
-	list_remove_node (listHead->prev);
-	(*node)->next = NULL;
-	(*node)->prev = NULL;
-}
-
-
-void list_insert_node_after (tListNode * node, tListNode * ref_node)
-{
-	node->next = ref_node->next;
-	node->prev = ref_node;
-	ref_node->next = node;
-	(node->next)->prev = node;
-}
-
-
-void list_insert_node_before (tListNode * node, tListNode * ref_node)
-{
-	node->next = ref_node;
-	node->prev = ref_node->prev;
-	ref_node->prev = node;
-	(node->prev)->next = node;
-}
-
-
-int list_get_size (tListNode * listHead)
-{
-	int size = 0;
-	tListNode * temp = listHead->next;
-	while (temp != listHead)
-	{
-		size++;
-		temp = temp->next;		
-	}
-	return (size);
-}
-
-void list_get_next_node (tListNode * ref_node, tListNode ** node)
-{
-    *node = ref_node->next;
-}
-
-
-void list_get_prev_node (tListNode * ref_node, tListNode ** node)
-{
-    *node = ref_node->prev;
-}
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/bluenrg-hci/utils/osal.c
--- a/source/bluenrg-hci/utils/osal.c	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,72 +0,0 @@
-/**
-******************************************************************************
-* @file    osal.c 
-* @author  AMS - HEA&RF BU / CL
-* @version V1.0.0
-* @date    04-July-2014
-* @brief   Implementation of OS abstraction layer functions used by the
-*          library.
-******************************************************************************
-* @attention
-  *
-  * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
-  *
-  * 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 STMicroelectronics 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.
-  *
-  ******************************************************************************
-  */
-  
-/* Includes ------------------------------------------------------------------*/
-#include <string.h>
-#include <osal.h>
- 
- /**
- * @brief  Osal_MemCpy
- * @param  dest: Pointer to the destination buffer
- * @param  src : Pointer to the source buffer
- * @param  size: Number of bytes to copy from the source to the destination
- *               buffer
- * @retval Pointer to the destination buffer
- */
-void* Osal_MemCpy(void *dest, const void *src, unsigned int size)
-{
-    return(memcpy(dest,src,size)); 
-}
-
-/**
- * @brief  Osal_MemSet
- * @param  ptr  : Pointer to block of memory to fill  
- * @param  value: Value to assign to each byte of the memory block
- * @param  size : Number of bytes to be set to "value"
- * @retval Pointer to the filled block of memory
- */
-void* Osal_MemSet(void *ptr, int value, unsigned int size)
-{
-    return(memset(ptr,value,size));
-}
-
-/******************************************************************************
- * local Functions
- *****************************************************************************/ 
- 
- /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/platform/ble_clock.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/platform/ble_clock.c	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,36 @@
+
+#include "ble_clock.h"
+#ifdef YOTTA_CFG_MBED_OS
+	#include "mbed-drivers/wait_api.h"
+	#include "mbed-drivers/rtc_time.h"
+#else
+    #include "wait_api.h"
+	#include "rtc_time.h"
+#endif
+
+const uint32_t CLOCK_SECOND = 1000;
+
+/*---------------------------------------------------------------------------*/
+
+void Clock_Init(void)
+{
+  //Not Used
+}
+
+/*---------------------------------------------------------------------------*/
+
+tClockTime Clock_Time(void)
+{
+	return clock();
+}
+
+/*---------------------------------------------------------------------------*/
+/**
+ * Wait for a multiple of 1 ms.
+ *
+ */
+void Clock_Wait(uint32_t i)
+{
+	wait_ms(i);
+}
+/*---------------------------------------------------------------------------*/
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/platform/btle.cpp
--- a/source/platform/btle.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/platform/btle.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -17,7 +17,7 @@
 
 /**
   ******************************************************************************
-  * @file    btle.cpp 
+  * @file    btle.cpp
   * @author  STMicroelectronics
   * @brief   Implementation BlueNRG Init and helper functions.
   ******************************************************************************
@@ -31,7 +31,7 @@
   * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  */ 
+  */
 
 
 #include "btle.h"
@@ -40,7 +40,7 @@
 #include "BlueNRGGap.h"
 #include "BlueNRGGattServer.h"
 #include "BlueNRGGattClient.h"
-#include "Utils.h"
+#include "ble_utils.h"
 
 #include "x_nucleo_idb0xa1_targets.h"
 
@@ -52,19 +52,19 @@
 /* C File Includes ------------------------------------------------------------------*/
 #include <stdio.h>
 #include <string.h>
-#include "hci.h"
-#include "hci_const.h"
+#include "ble_hci.h"
+#include "ble_hci_const.h"
 #include "bluenrg_aci.h"
 #include "bluenrg_hal_aci.h"
 #include "bluenrg_gap.h"
 #include "bluenrg_utils.h"
 
-#include "hal_types.h"
-#include "hal.h"
-#include "gp_timer.h"
-#include "osal.h"
-#include "sm.h"
-#include "debug.h"
+#include "ble_hal_types.h"
+#include "ble_hal.h"
+#include "ble_gp_timer.h"
+#include "ble_osal.h"
+#include "ble_sm.h"
+#include "ble_debug.h"
 
 #ifdef __cplusplus
 }
@@ -81,6 +81,7 @@
 uint16_t g_gap_service_handle = 0;
 uint16_t g_appearance_char_handle = 0;
 uint16_t g_device_name_char_handle = 0;
+uint16_t g_preferred_connection_parameters_char_handle = 0;
 
 /* Private variables ---------------------------------------------------------*/
 volatile uint8_t set_connectable = 1;
@@ -91,17 +92,13 @@
 /**************************************************************************/
 /*!
     @brief  Init the BTLE stack with the specified role
-    @param  isSetAddress boolean if address has been set
-    @param  role The device role
     @returns void
 */
 /**************************************************************************/
-void btleInit(bool isSetAddress, uint8_t role)
+void btleInit(void)
 {
     PRINTF("btleInit>>\n\r");
-    /* Avoid compiler warnings about unused variables. */
-    (void)isSetAddress;
-    
+
     int ret;
     uint8_t  hwVersion;
     uint16_t fwVersion;
@@ -113,7 +110,7 @@
     /* get the BlueNRG HW and FW versions */
     getBlueNRGVersion(&hwVersion, &fwVersion);
 
-    /* 
+    /*
      * Reset BlueNRG again otherwise we won't
      * be able to change its MAC address.
      * aci_hal_write_config_data() must be the first
@@ -135,35 +132,8 @@
                                         &stackMode);
     }
 
-    /* The Nucleo board must be configured as SERVER */
-    //check if isSetAddress is set then set address.
-#if 0
-    if(isSetAddress)
-    {
-        Gap::Address_t bleAddr;
-        Gap::AddressType_t addr_type;
-
-        BlueNRGGap::getInstance().getAddress(&addr_type, bleAddr);
-
-        ret = aci_hal_write_config_data(CONFIG_DATA_PUBADDR_OFFSET,
-                                        CONFIG_DATA_PUBADDR_LEN,
-                                        bleAddr);
-    } else {
-        
-        const Gap::Address_t BLE_address_BE = {0xFD,0x66,0x05,0x13,0xBE,0xBA};
-        BlueNRGGap::getInstance().setAddress(BLEProtocol::AddressType::RANDOM_STATIC, BLE_address_BE);
-        
-        ret = aci_hal_write_config_data(CONFIG_DATA_PUBADDR_OFFSET,
-                                        CONFIG_DATA_PUBADDR_LEN,
-                                        BLE_address_BE);
-    }
-#endif
-
-    const Gap::Address_t BLE_address_BE = {0xFD,0x66,0x05,0x13,0xBE,0xBA};
-    BlueNRGGap::getInstance().setAddress(BLEProtocol::AddressType::RANDOM_STATIC, BLE_address_BE);
-    
     ret = aci_gatt_init();
-    if(ret){
+    if(ret != BLE_STATUS_SUCCESS){
         PRINTF("GATT_Init failed.\n");
     }
     if (bnrg_expansion_board == IDB05A1) {
@@ -174,9 +144,19 @@
                                    &dev_name_char_handle,
                                    &appearance_char_handle);
     } else {
-        ret = aci_gap_init_IDB04A1(role, &service_handle, &dev_name_char_handle, &appearance_char_handle);
+        // IDB04A1 is configured as peripheral by default
+        ret = aci_gap_init_IDB04A1(GAP_PERIPHERAL_ROLE_IDB04A1, &service_handle, &dev_name_char_handle, &appearance_char_handle);
     }
-    
+
+    // read the default static address and inject it into the GAP object
+    {
+        Gap::Address_t BLE_address_BE = { 0 };
+        uint8_t data_len_out;
+        aci_hal_read_config_data(CONFIG_DATA_RANDOM_ADDRESS_IDB05A1, BDADDR_SIZE, &data_len_out, BLE_address_BE);
+        // FIXME error handling of this function
+        BlueNRGGap::getInstance().setAddress(BLEProtocol::AddressType::RANDOM_STATIC, BLE_address_BE);
+    }
+
     if(ret != BLE_STATUS_SUCCESS){
         PRINTF("GAP_Init failed.\n");
     }
@@ -190,22 +170,24 @@
                                        USE_FIXED_PIN_FOR_PAIRING,
                                        123456,
                                        BONDING);
-    if (ret == BLE_STATUS_SUCCESS) {
-        PRINTF("Auth Req set successfully.\n");
+    if (ret != BLE_STATUS_SUCCESS) {
+        PRINTF("Auth Req set failed.\n");
     }
-    
+
     aci_hal_set_tx_power_level(1,4);
-    
+
     g_gap_service_handle = service_handle;
     g_appearance_char_handle = appearance_char_handle;
-    g_device_name_char_handle = dev_name_char_handle; 
-    //Device Name is set from Accumulate Adv Data Payload or through setDeviceName API  
+    g_device_name_char_handle = dev_name_char_handle;
+    //Device Name is set from Accumulate Adv Data Payload or through setDeviceName API
     /*ret = aci_gatt_update_char_value(service_handle, dev_name_char_handle, 0,
                             strlen(name), (tHalUint8 *)name);*/
 
-#ifdef AST_FOR_MBED_OS
-    minar::Scheduler::postCallback(btle_handler);
-#endif
+    signalEventsToProcess();
+    // update the peripheral preferred conenction parameters handle
+    // This value is hardcoded at the moment.
+    g_preferred_connection_parameters_char_handle = 10;
+
     return;
 }
 
@@ -214,19 +196,18 @@
     @brief  mbedOS
 
     @param[in]  void
-    
+
     @returns
 */
 /**************************************************************************/
-#ifdef AST_FOR_MBED_OS
 int btle_handler_pending = 0;
 
 void btle_handler(void)
 {
     btle_handler_pending = 0;
+	BlueNRGGap::getInstance().Process();    
     HCI_Process();
 }
-#endif
 
 /* set BLE Version string */
 void setVersionString(uint8_t hwVersion, uint16_t fwVersion)
@@ -274,7 +255,7 @@
     @brief  Not Used
 
     @param[in]  void
-    
+
     @returns
 */
 void SPI_Poll(void)
@@ -282,7 +263,7 @@
     //HAL_GPIO_EXTI_Callback_Poll(BNRG_SPI_EXTI_PIN);
     return;
 }
-   
+
 void Attribute_Modified_CB(evt_blue_aci *blue_evt)
 {
     uint16_t conn_handle;
@@ -323,7 +304,7 @@
             currentHandle = BlueNRGGattServer::CHAR_DESC_HANDLE;
         }
         PRINTF("currentHandle %d\n\r", currentHandle);
-        if((p_char->getProperties() & 
+        if((p_char->getProperties() &
             (GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE)) &&
             currentHandle == BlueNRGGattServer::CHAR_DESC_HANDLE) {
 
@@ -332,21 +313,22 @@
             PRINTF("*****NOTIFICATION CASE\n\r");
             //Now Check if data written in Enable or Disable
             if((uint16_t)att_data[0]==1) {
-                //PRINTF("Notify ENABLED\n\r"); 
+                //PRINTF("Notify ENABLED\n\r");
                 BlueNRGGattServer::getInstance().HCIEvent(GattServerEvents::GATT_EVENT_UPDATES_ENABLED, charDescHandle);
             } else {
-                //PRINTF("Notify DISABLED\n\r"); 
+                //PRINTF("Notify DISABLED\n\r");
                 BlueNRGGattServer::getInstance().HCIEvent(GattServerEvents::GATT_EVENT_UPDATES_DISABLED, charDescHandle);
             }
+            return;
         }
-                    
+
         //Check if attr handle property is WRITEABLE, in the case generate GATT_EVENT_DATA_WRITTEN Event
         if((p_char->getProperties() &
             (GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE)) &&
             currentHandle == BlueNRGGattServer::CHAR_VALUE_HANDLE) {
-                    
+
             PRINTF("*****WRITE CASE\n\r");
-                   
+
             GattWriteCallbackParams writeParams;
             writeParams.connHandle = conn_handle;
             writeParams.handle = p_char->getValueAttribute().getHandle();
@@ -355,17 +337,31 @@
             writeParams.data = att_data;
             writeParams.offset = offset;
 
-            BlueNRGGattServer::getInstance().HCIDataWrittenEvent(&writeParams);
-
             //BlueNRGGattServer::getInstance().handleEvent(GattServerEvents::GATT_EVENT_DATA_WRITTEN, attr_handle);
             //Write the actual Data to the Attr Handle? (uint8_1[])att_data contains the data
             if ((p_char->getValueAttribute().getValuePtr() != NULL) && (p_char->getValueAttribute().getLength() > 0)) {
-                BlueNRGGattServer::getInstance().write(p_char->getValueAttribute().getHandle(),
-                                                       (uint8_t*)att_data,
-                                                       data_length,
-                                                       false);
+                BlueNRGGattServer::getInstance().write(
+                    p_char->getValueAttribute().getHandle(),
+                    (uint8_t*)att_data,
+                    data_length,
+                    false
+                );
             }
-        } 
+
+            BlueNRGGattServer::getInstance().HCIDataWrittenEvent(&writeParams);
+        } else {
+            PRINTF("*****WRITE DESCRIPTOR CASE\n\r");
+
+            GattWriteCallbackParams writeParams;
+            writeParams.connHandle = conn_handle;
+            writeParams.handle = attr_handle;
+            writeParams.writeOp = GattWriteCallbackParams::OP_WRITE_REQ;//Where to find this property in BLUENRG?
+            writeParams.len = data_length;
+            writeParams.data = att_data;
+            writeParams.offset = offset;
+
+            BlueNRGGattServer::getInstance().HCIDataWrittenEvent(&writeParams);
+        }
     }
 
 }
@@ -380,39 +376,39 @@
 
     @param[in]  pckt
                 Event Packet sent by the stack to be decoded
-    
+
     @returns
     */
     /**************************************************************************/
     extern void HCI_Event_CB(void *pckt) {
         hci_uart_pckt *hci_pckt = (hci_uart_pckt*)pckt;
         hci_event_pckt *event_pckt = (hci_event_pckt*)hci_pckt->data;
-        
+
         if(hci_pckt->type != HCI_EVENT_PKT)
           return;
 
         switch(event_pckt->evt){
-            
+
         case EVT_DISCONN_COMPLETE:
             {
                 PRINTF("EVT_DISCONN_COMPLETE\n");
-                
+
                 evt_disconn_complete *evt = (evt_disconn_complete*)event_pckt->data;
-                
+
                 BlueNRGGap::getInstance().processDisconnectionEvent(evt->handle, (Gap::DisconnectionReason_t)evt->reason);
             }
             break;
-            
+
         case EVT_LE_META_EVENT:
             {
                 PRINTF("EVT_LE_META_EVENT\n");
-                
+
                 evt_le_meta_event *evt = (evt_le_meta_event *)event_pckt->data;
-                
+
                 switch(evt->subevent){
 
                 case EVT_LE_CONN_COMPLETE:
-                    {                            
+                    {
                         PRINTF("EVT_LE_CONN_COMPLETE\n");
                         Gap::Address_t ownAddr;
                         Gap::AddressType_t ownAddrType;
@@ -420,12 +416,19 @@
 
                         Gap::AddressType_t peerAddrType = BLEProtocol::AddressType::RANDOM_STATIC;
                         Gap::Role_t role;
-                        
+
                         evt_le_connection_complete *cc = (evt_le_connection_complete *)evt->data;
-                        
+
                         BlueNRGGap::getInstance().setConnectionHandle(cc->handle);
-                        BlueNRGGap::ConnectionParams_t connectionParams;
-                        BlueNRGGap::getInstance().getPreferredConnectionParams(&connectionParams);
+                        BlueNRGGap::ConnectionParams_t connectionParams = {
+                            /* minConnectionInterval = */ cc->interval,
+                            /* maxConnectionInterval = */ cc->interval,
+                            /* slaveLatency = */ cc->latency,
+                            /* connectionSupervisionTimeout = */ cc->supervision_timeout
+                        };
+
+                        BlueNRGGap::getInstance().setConnectionInterval(cc->interval);
+
                         switch (cc->peer_bdaddr_type) {
                             case PUBLIC_ADDR:
                                 peerAddrType = BLEProtocol::AddressType::PUBLIC;
@@ -439,7 +442,7 @@
                             case NON_RESOLVABLE_PRIVATE_ADDR:
                                 peerAddrType = BLEProtocol::AddressType::RANDOM_PRIVATE_NON_RESOLVABLE;
                                 break;
-                        }                                             
+                        }
                         //PRINTF("EVT_LE_CONN_COMPLETE LL role=%d\n", cc->role);
                         switch (cc->role) {
 			case 0: //master
@@ -452,7 +455,9 @@
                                 role = Gap::PERIPHERAL;
 				break;
                         }
-                        //PRINTF("EVT_LE_CONN_COMPLETE GAP role=%d\n", role);
+
+                        BlueNRGGap::getInstance().setGapRole(role);
+
                         BlueNRGGap::getInstance().processConnectionEvent(cc->handle,
                                                                          role,
                                                                          peerAddrType,
@@ -462,17 +467,17 @@
                                                                          &connectionParams);
                     }
                     break;
-          
+
         case EVT_LE_ADVERTISING_REPORT:
           PRINTF("EVT_LE_ADVERTISING_REPORT\n\r");
           /* FIXME: comment this otherwise it will be obscure and error prone if BlueNRG FW will be updated */
           // This event is generated only by X-NUCLEO-IDB05A1 version but not by X-NUCLEO-IDB04A1 (which generates DEVICE_FOUND EVT)
           // Formally the structure related to both events are identical except that for the ADV REPORT
           // there is one more field (number of reports) which is not forwarded to upper layer.
-          // Thus we need to move one byte over (((uint8_t*)evt->data)+1) before persing the ADV REPORT. 
+          // Thus we need to move one byte over (((uint8_t*)evt->data)+1) before persing the ADV REPORT.
           le_advertising_info *pr = (le_advertising_info*) (((uint8_t*)evt->data)+1);
           PRINTF("EVT_LE_ADVERTISING_REPORT evt_type=%d\n\r", pr->evt_type);
-          
+
           BlueNRGGap::getInstance().Discovery_CB(BlueNRGGap::DEVICE_FOUND,
                                                  pr->evt_type,
                                                  pr->bdaddr_type,
@@ -484,14 +489,40 @@
                 }
             }
             break;
-            
+
         case EVT_VENDOR:
-            {                
+            {
                 evt_blue_aci *blue_evt = (evt_blue_aci*)event_pckt->data;
                 //PRINTF("EVT_VENDOR %d\n", blue_evt->ecode);
-                
+
                 switch(blue_evt->ecode){
-                           
+
+                case EVT_BLUE_GATT_WRITE_PERMIT_REQ:
+                    {
+                        PRINTF("EVT_BLUE_GATT_WRITE_PERMIT_REQ\r\n");
+                        evt_gatt_write_permit_req* write_req = (evt_gatt_write_permit_req*)blue_evt->data;
+
+                        // ask the local server if the write operation is authorized
+                        uint8_t err_code = BlueNRGGattServer::getInstance().Write_Request_CB(
+                            write_req->conn_handle,
+                            write_req->attr_handle,
+                            write_req->data_length,
+                            write_req->data
+                        );
+                        uint8_t write_status = err_code == 0 ? 0 : 1;
+
+                        // reply to the shield
+                        aci_gatt_write_response(
+                            write_req->conn_handle,
+                            write_req->attr_handle,
+                            write_status,
+                            err_code,
+                            write_req->data_length,
+                            write_req->data
+                        );
+                    }
+                    break;
+
                 case EVT_BLUE_GATT_READ_PERMIT_REQ:
                     {
                         PRINTF("EVT_BLUE_GATT_READ_PERMIT_REQ_OK\n\r");
@@ -500,16 +531,16 @@
                         BlueNRGGattServer::getInstance().Read_Request_CB(pr->attr_handle);
                     }
                     break;
-                    
-                case EVT_BLUE_GATT_ATTRIBUTE_MODIFIED:         
+
+                case EVT_BLUE_GATT_ATTRIBUTE_MODIFIED:
                     {
                         PRINTF("EVT_BLUE_GATT_ATTRIBUTE_MODIFIED\n\r");
                         /* this callback is invoked when a GATT attribute is modified
                             extract callback data and pass to suitable handler function */
                         Attribute_Modified_CB(blue_evt);
                     }
-                    break;  
-                    
+                    break;
+
                     //Any cases for Data Sent Notifications?
                 case EVT_BLUE_GATT_NOTIFICATION:
                     //This is only relevant for Client Side Event
@@ -518,8 +549,8 @@
                 case EVT_BLUE_GATT_INDICATION:
                     //This is only relevant for Client Side Event
                     PRINTF("EVT_BLUE_GATT_INDICATION");
-                    break;   
-                    
+                    break;
+
         case EVT_BLUE_ATT_READ_BY_GROUP_TYPE_RESP:
           {
             PRINTF("EVT_BLUE_ATT_READ_BY_GROUP_TYPE_RESP\n\r");
@@ -604,12 +635,42 @@
             BlueNRGGattClient::getInstance().gattProcedureCompleteCB(evt->conn_handle, evt->error_code);
           }
           break;
-          
+
+        case EVT_BLUE_L2CAP_CONN_UPD_REQ:
+          {
+            PRINTF("EVT_BLUE_L2CAP_CONN_UPD_REQ\r\n");
+            evt_l2cap_conn_upd_req *evt = (evt_l2cap_conn_upd_req*)blue_evt->data;
+            if(bnrg_expansion_board == IDB05A1) {
+              // we assume the application accepts the request from the slave
+              aci_l2cap_connection_parameter_update_response_IDB05A1(evt->conn_handle,
+                                                                     evt->interval_min,
+                                                                     evt->interval_max,
+                                                                     evt->slave_latency,
+                                                                     evt->timeout_mult,
+                                                                     CONN_L1, CONN_L2,
+                                                                     evt->identifier,
+                                                                     0x0000);
+            }
+          }
+          break;
+
+        case EVT_BLUE_L2CAP_CONN_UPD_RESP:
+          {
+            PRINTF("EVT_BLUE_L2CAP_CONN_UPD_RESP\r\n");
+          }
+          break;
+
+        case EVT_LE_CONN_UPDATE_COMPLETE:
+          {
+            PRINTF("EVT_LE_CONN_UPDATE_COMPLETE\r\n");
+          }
+          break;
+
         case EVT_BLUE_GAP_DEVICE_FOUND:
           {
             evt_gap_device_found *pr = (evt_gap_device_found*)blue_evt->data;
             PRINTF("EVT_BLUE_GAP_DEVICE_FOUND evt_type=%d\n\r", pr->evt_type);
-            
+
             BlueNRGGap::getInstance().Discovery_CB(BlueNRGGap::DEVICE_FOUND,
                                                    pr->evt_type,
                                                    pr->bdaddr_type,
@@ -619,28 +680,28 @@
                                                    &pr->data_RSSI[pr->data_length]);
           }
           break;
-          
+
         case EVT_BLUE_GAP_PROCEDURE_COMPLETE:
           {
             evt_gap_procedure_complete *pr = (evt_gap_procedure_complete*)blue_evt->data;
             //PRINTF("EVT_BLUE_GAP_PROCEDURE_COMPLETE (code=0x%02X)\n\r", pr->procedure_code);
-            
+
             switch(pr->procedure_code) {
             case GAP_OBSERVATION_PROC_IDB05A1:
-              
+
               BlueNRGGap::getInstance().Discovery_CB(BlueNRGGap::DISCOVERY_COMPLETE, 0, 0, NULL, NULL, NULL, NULL);
               break;
             }
           }
-                    break;                                     
+                    break;
                 }
             }
             break;
-        }    
+        }
         return ;
     }
 
 
 #ifdef __cplusplus
 }
-#endif
+#endif
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/platform/clock.c
--- a/source/platform/clock.c	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-
-#include "clock.h"
-#include "wait_api.h"
-#include "rtc_time.h"
-
-const uint32_t CLOCK_SECOND = 1000;
-
-/*---------------------------------------------------------------------------*/
-
-void Clock_Init(void)
-{
-  //Not Used
-}
-
-/*---------------------------------------------------------------------------*/
-
-tClockTime Clock_Time(void)
-{
-	return clock();
-}
-
-/*---------------------------------------------------------------------------*/
-/**
- * Wait for a multiple of 1 ms.
- *
- */
-void Clock_Wait(uint32_t i)
-{
-	wait_ms(i);
-}
-/*---------------------------------------------------------------------------*/
-
-
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/platform/stm32_bluenrg_ble.cpp
--- a/source/platform/stm32_bluenrg_ble.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ b/source/platform/stm32_bluenrg_ble.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -52,8 +52,8 @@
 #endif
 
 #include "stm32_bluenrg_ble.h"
-#include "gp_timer.h"
-#include "debug.h"
+#include "ble_gp_timer.h"
+#include "ble_debug.h"
 
 	
 void BlueNRG_RST(void)
@@ -172,20 +172,12 @@
     bluenrgDeviceInstance.enable_irq();
 }
 
-#ifdef AST_FOR_MBED_OS
-/**
- * Call BTLE callback handler.
- * @param  None
- * @retval None
- */
-void Call_BTLE_Handler(void)
-{
-	if(!btle_handler_pending) {
-		btle_handler_pending = 1;
-		minar::Scheduler::postCallback(btle_handler);
-	}
+void signalEventsToProcess(void)  {
+    if(btle_handler_pending == 0) {
+        btle_handler_pending = 1;
+        bluenrgDeviceInstance.signalEventsToProcess(BLE::DEFAULT_INSTANCE);
+    }
 }
-#endif
 
 /**
  * @brief  Disable SPI IRQ.
@@ -224,4 +216,4 @@
 // End of C function wrappers
 ////////////////////////////////////////
  
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/utils/Payload.cpp
--- a/source/utils/Payload.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,118 +0,0 @@
-/* mbed Microcontroller Library
-* Copyright (c) 2006-2013 ARM Limited
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-*     http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#include <Payload.h>   
-
-Payload::Payload() {
-    stringLength = 0;
-    payloadUnitCount = 0;
-    payload = NULL;
-}
-
-Payload::Payload(const uint8_t *tokenString, uint8_t string_ength) {
-    // initialize private data members
-    stringLength = string_ength;
-    payloadUnitCount = 0;
-    payload = NULL;
-    
-    int index = 0;
-    while( index!=stringLength) {
-        int len=tokenString[index];
-        index=index+1+len;
-        payloadUnitCount++;               
-    }
-    
-    UnitPayload *obj = new UnitPayload[payloadUnitCount];
-    int i=0;
-    int c=0;
-    int j,k;
-
-    while(i<payloadUnitCount)
-    {   
-        obj[i].length=tokenString[c];
-        obj[i].id=tokenString[c+1];
-
-        obj[i].data = new uint8_t[obj[i].length];
-        for(j=c+2,k=0;(j<(c+obj[i].length+1))&&(k<obj[i].length-1);j++,k++)
-        {
-            obj[i].data[k]=tokenString[j];
-            
-        }           
-        
-        c=c+obj[i].length+1;
-        i++;
-
-    }
-    payload = obj;
-}
-
-uint8_t Payload::getPayloadUnitCount() {
-    return payloadUnitCount;
-}
-
-uint8_t Payload::getIDAtIndex(int index) {
-    return payload[index].get_id();
-}
-
-uint8_t Payload::getLengthAtIndex(int index) {
-    return payload[index].get_length();
-}
-
-uint8_t* Payload::getDataAtIndex(int index) {
-    return payload[index].get_data();
-}
-
-int8_t Payload::getInt8AtIndex(int index) {
-    uint8_t* str = payload[index].get_data();
-    int8_t value = (int8_t)str[0];
-    return value;
-}
-
-uint16_t Payload::getUint16AtIndex(int index) {
-    uint16_t* str = (uint16_t*)payload[index].get_data();
-    uint16_t value = str[0];
-    return value;    
-}
-
-uint8_t* Payload::getSerializedAdDataAtIndex(int index) {
-    uint8_t length = payload[index].get_length();
-    uint8_t* data = payload[index].get_data();
-    uint8_t id = payload[index].get_id();
-    uint8_t *serializedAdData = new uint8_t[length];
-    
-    serializedAdData[0] = id;
-    for(int i=0; i<length-1; i++) {
-        serializedAdData[i+1] = data[i];
-    }
-    return serializedAdData;
-}
-
-Payload::~Payload() {
-    int i = 0;
-
-    if(payload) {
-        while(i<payloadUnitCount) {
-            if(payload->data) {
-                delete[] payload->data;
-                payload->data = NULL;
-            }
-        }
-        delete[] payload;
-        payload = NULL;
-    }
-
-}
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/utils/Utils.cpp
--- a/source/utils/Utils.cpp	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,93 +0,0 @@
-/* mbed Microcontroller Library
-* Copyright (c) 2006-2013 ARM Limited
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-*     http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#include "Utils.h"
-
-/**************************************************************************/
-/*!
-    @brief  sets values of EN_HIGH_POWER and PA_LEVEL corresponding to dBMLevel of tx power
-
-*/
-/**************************************************************************/
-tBleStatus getHighPowerAndPALevelValue(int8_t dBMLevel, int8_t& EN_HIGH_POWER, int8_t& PA_LEVEL) {
-    tBleStatus ret = BLE_STATUS_SUCCESS;
-
-    if(dBMLevel==-18) {
-        EN_HIGH_POWER = 0;
-        PA_LEVEL = 0;         
-    }
-    else if(dBMLevel==-15) {
-        EN_HIGH_POWER = 0;
-        PA_LEVEL = 1;
-    }
-    else if(dBMLevel==-14) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 0;
-    }
-    else if(dBMLevel==-12) {
-        EN_HIGH_POWER = 0;
-        PA_LEVEL = 2;
-    }
-    else if(dBMLevel==-11) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 1;
-    }
-    else if(dBMLevel==-9) {
-        EN_HIGH_POWER = 0;
-        PA_LEVEL = 3;
-    }
-    else if(dBMLevel==-8) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 2;
-    }
-    else if(dBMLevel==-6) {
-        EN_HIGH_POWER = 0;
-        PA_LEVEL = 4;
-    }
-    else if(dBMLevel==-5) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 3;
-    }
-    else if(dBMLevel==-2) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 4;
-    }
-    else if(dBMLevel==0) {
-        EN_HIGH_POWER = 0;
-        PA_LEVEL = 6;
-    }
-    else if(dBMLevel==2) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 5;
-    }
-    else if(dBMLevel==4) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 6;
-    }
-    else if(dBMLevel==5) {
-        EN_HIGH_POWER = 0;
-        PA_LEVEL = 7;
-    }
-    else if(dBMLevel==8) {
-        EN_HIGH_POWER = 1;
-        PA_LEVEL = 7;
-    }
-    else {
-        ret = ERR_INVALID_HCI_CMD_PARAMS;
-    }
-
-    return ret;
-}
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/utils/ble_payload.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/utils/ble_payload.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,117 @@
+/* mbed Microcontroller Library
+* Copyright (c) 2006-2013 ARM Limited
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*     http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <ble_payload.h>   
+
+Payload::Payload() {
+    stringLength = 0;
+    payloadUnitCount = 0;
+    payload = NULL;
+}
+
+Payload::Payload(const uint8_t *tokenString, uint8_t string_ength) {
+    // initialize private data members
+    stringLength = string_ength;
+    payloadUnitCount = 0;
+    payload = NULL;
+    
+    int index = 0;
+    while( index!=stringLength) {
+        int len=tokenString[index];
+        index=index+1+len;
+        payloadUnitCount++;               
+    }
+    
+    UnitPayload *obj = new UnitPayload[payloadUnitCount];
+    int i=0;
+    int c=0;
+    int j,k;
+
+    while(i<payloadUnitCount)
+    {   
+        obj[i].length=tokenString[c];
+        obj[i].id=tokenString[c+1];
+
+        obj[i].data = new uint8_t[obj[i].length];
+        for(j=c+2,k=0;(j<(c+obj[i].length+1))&&(k<obj[i].length-1);j++,k++)
+        {
+            obj[i].data[k]=tokenString[j];
+            
+        }           
+        
+        c=c+obj[i].length+1;
+        i++;
+
+    }
+    payload = obj;
+}
+
+uint8_t Payload::getPayloadUnitCount() {
+    return payloadUnitCount;
+}
+
+uint8_t Payload::getIDAtIndex(int index) {
+    return payload[index].get_id();
+}
+
+uint8_t Payload::getLengthAtIndex(int index) {
+    return payload[index].get_length();
+}
+
+uint8_t* Payload::getDataAtIndex(int index) {
+    return payload[index].get_data();
+}
+
+int8_t Payload::getInt8AtIndex(int index) {
+    uint8_t* str = payload[index].get_data();
+    int8_t value = (int8_t)str[0];
+    return value;
+}
+
+uint16_t Payload::getUint16AtIndex(int index) {
+    uint16_t* str = (uint16_t*)payload[index].get_data();
+    uint16_t value = str[0];
+    return value;    
+}
+
+uint8_t* Payload::getSerializedAdDataAtIndex(int index) {
+    uint8_t length = payload[index].get_length();
+    uint8_t* data = payload[index].get_data();
+    uint8_t id = payload[index].get_id();
+    uint8_t *serializedAdData = new uint8_t[length];
+    
+    serializedAdData[0] = id;
+    for(int i=0; i<length-1; i++) {
+        serializedAdData[i+1] = data[i];
+    }
+    return serializedAdData;
+}
+
+Payload::~Payload() {
+    int i = 0;
+
+    if(payload) {
+        while(i<payloadUnitCount) {
+            if(payload->data) {
+                delete[] payload->data;
+                payload->data = NULL;
+            }
+        }
+        delete[] payload;
+        payload = NULL;
+    }
+
+}
diff -r 0c2cb16a7166 -r 3a7d9f923493 source/utils/ble_utils.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/utils/ble_utils.cpp	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,93 @@
+/* mbed Microcontroller Library
+* Copyright (c) 2006-2013 ARM Limited
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*     http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include "ble_utils.h"
+
+/**************************************************************************/
+/*!
+    @brief  sets values of EN_HIGH_POWER and PA_LEVEL corresponding to dBMLevel of tx power
+
+*/
+/**************************************************************************/
+tBleStatus getHighPowerAndPALevelValue(int8_t dBMLevel, int8_t& EN_HIGH_POWER, int8_t& PA_LEVEL) {
+    tBleStatus ret = BLE_STATUS_SUCCESS;
+
+    if(dBMLevel==-18) {
+        EN_HIGH_POWER = 0;
+        PA_LEVEL = 0;         
+    }
+    else if(dBMLevel==-15) {
+        EN_HIGH_POWER = 0;
+        PA_LEVEL = 1;
+    }
+    else if(dBMLevel==-14) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 0;
+    }
+    else if(dBMLevel==-12) {
+        EN_HIGH_POWER = 0;
+        PA_LEVEL = 2;
+    }
+    else if(dBMLevel==-11) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 1;
+    }
+    else if(dBMLevel==-9) {
+        EN_HIGH_POWER = 0;
+        PA_LEVEL = 3;
+    }
+    else if(dBMLevel==-8) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 2;
+    }
+    else if(dBMLevel==-6) {
+        EN_HIGH_POWER = 0;
+        PA_LEVEL = 4;
+    }
+    else if(dBMLevel==-5) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 3;
+    }
+    else if(dBMLevel==-2) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 4;
+    }
+    else if(dBMLevel==0) {
+        EN_HIGH_POWER = 0;
+        PA_LEVEL = 6;
+    }
+    else if(dBMLevel==2) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 5;
+    }
+    else if(dBMLevel==4) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 6;
+    }
+    else if(dBMLevel==5) {
+        EN_HIGH_POWER = 0;
+        PA_LEVEL = 7;
+    }
+    else if(dBMLevel==8) {
+        EN_HIGH_POWER = 1;
+        PA_LEVEL = 7;
+    }
+    else {
+        ret = ERR_INVALID_HCI_CMD_PARAMS;
+    }
+
+    return ret;
+}
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/BlueNRGDevice.h
--- a/x-nucleo-idb0xa1/BlueNRGDevice.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/BlueNRGDevice.h	Thu Sep 15 16:59:44 2016 +0100
@@ -40,7 +40,11 @@
 
 #include "btle.h"
 
-#include "mbed.h"
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif 
 #include "ble/blecommon.h"
 #include "ble/BLEInstanceBase.h"
 #include "ble/BLE.h"
@@ -88,6 +92,8 @@
     int32_t spiWrite(uint8_t* data1, uint8_t* data2, uint8_t Nb_bytes1, uint8_t Nb_bytes2);
     void disable_irq();
     void enable_irq();
+
+    virtual void processEvents();
     
 private:
     bool isInitialized;
@@ -102,4 +108,4 @@
     SecurityManager *sm;
 };
 
-#endif
+#endif
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/BlueNRGDiscoveredCharacteristic.h
--- a/x-nucleo-idb0xa1/BlueNRGDiscoveredCharacteristic.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/BlueNRGDiscoveredCharacteristic.h	Thu Sep 15 16:59:44 2016 +0100
@@ -30,7 +30,7 @@
              GattAttribute::Handle_t  declHandleIn,
              GattAttribute::Handle_t  valueHandleIn,
              GattAttribute::Handle_t  lastHandleIn);
-  
+
   void setup(BlueNRGGattClient         *gattcIn,
              Gap::Handle_t            connectionHandleIn,
              UUID   uuidIn,
@@ -38,6 +38,9 @@
              GattAttribute::Handle_t  declHandleIn,
              GattAttribute::Handle_t  valueHandleIn,
              GattAttribute::Handle_t  lastHandleIn);
+
+
+  void setLastHandle(GattAttribute::Handle_t  lastHandleIn);
 };
 
-#endif /* __BLUENRG_DISCOVERED_CHARACTERISTIC_H__ */
+#endif /* __BLUENRG_DISCOVERED_CHARACTERISTIC_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/BlueNRGGap.h
--- a/x-nucleo-idb0xa1/BlueNRGGap.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/BlueNRGGap.h	Thu Sep 15 16:59:44 2016 +0100
@@ -16,7 +16,7 @@
 
 /**
   ******************************************************************************
-  * @file    BlueNRGGap.h 
+  * @file    BlueNRGGap.h
   * @author  STMicroelectronics
   * @brief   Header file for BlueNRG BLE_API Gap Class
   ******************************************************************************
@@ -30,12 +30,16 @@
   * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  */ 
-  
+  */
+
 #ifndef __BLUENRG_GAP_H__
 #define __BLUENRG_GAP_H__
 
-#include "mbed.h"
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif 
 #include "ble/blecommon.h"
 #include "btle.h"
 #include "ble/GapAdvertisingParams.h"
@@ -66,10 +70,6 @@
 #define MAX_INT_CONN   0x0C80 //=>4000msec
 #define DEF_INT_CONN   0x0140 //=>400msec (default value for connection interval)
 
-#define LOCAL_NAME_MAX_SIZE 9 //8 + 1(AD_DATA_TYPE)
-#define UUID_BUFFER_SIZE 17 //Either 8*2(16-bit UUIDs) or 4*4(32-bit UUIDs) or 1*16(128-bit UUIDs) +1(AD_DATA_TYPE)
-#define ADV_DATA_MAX_SIZE 31
-
 /**************************************************************************/
 /*!
     \brief
@@ -88,10 +88,10 @@
         DEVICE_FOUND,
         DISCOVERY_COMPLETE
     };
-    
+
     /* Functions that must be implemented from Gap */
-    virtual ble_error_t setAddress(addr_type_t type,   const Address_t address);
-    virtual ble_error_t getAddress(addr_type_t *typeP, Address_t address);
+    virtual ble_error_t setAddress(addr_type_t type, const BLEProtocol::AddressBytes_t address);
+    virtual ble_error_t getAddress(BLEProtocol::AddressType_t *typeP, BLEProtocol::AddressBytes_t address);
     virtual ble_error_t setAdvertisingData(const GapAdvertisingData &, const GapAdvertisingData &);
     virtual ble_error_t startAdvertising(const GapAdvertisingParams &);
     virtual ble_error_t stopAdvertising(void);
@@ -136,9 +136,9 @@
 
     void     setConnectionHandle(uint16_t con_handle);
     uint16_t getConnectionHandle(void);
-    
+
     bool getIsSetAddress();
-    
+
     Timeout getAdvTimeout(void) const {
         return advTimeout;
     }
@@ -146,15 +146,19 @@
         return AdvToFlag;
     }
     void setAdvToFlag(void);
-    
+
     void Process(void);
 
     GapScanningParams* getScanningParams(void);
 
     virtual ble_error_t startRadioScan(const GapScanningParams &scanningParams);
 
+    void setConnectionInterval(uint16_t interval);
+    void setGapRole(Role_t role);
+
 private:
     uint16_t m_connectionHandle;
+    Role_t gapRole;
     AddressType_t addr_type;
     Address_t _peerAddr;
     AddressType_t _peerAddrType;
@@ -164,23 +168,9 @@
     bool isSetAddress;
     uint8_t deviceAppearance[2];
 
-    uint8_t local_name_length;
-    uint8_t local_name[ADV_DATA_MAX_SIZE];//LOCAL_NAME_MAX_SIZE];
-
-    uint8_t servUuidlength;
-    uint8_t servUuidData[UUID_BUFFER_SIZE];
-
-    uint8_t AdvLen;
-    uint8_t AdvData[ADV_DATA_MAX_SIZE];
-
-    uint8_t txPowLevSet;
-    
     Timeout advTimeout;
     bool AdvToFlag;
 
-    const uint8_t *scan_response_payload;
-    uint8_t scan_rsp_length;
-
     static uint16_t SCAN_DURATION_UNITS_TO_MSEC(uint16_t duration) {
         return (duration * 625) / 1000;
     }
@@ -202,7 +192,7 @@
 
     ble_error_t updateAdvertisingData(void);
 
-    BlueNRGGap(): txPowLevSet(0) {
+    BlueNRGGap() {
         m_connectionHandle = BLE_CONN_HANDLE_INVALID;
         addr_type = BLEProtocol::AddressType::RANDOM_STATIC;
 
@@ -216,6 +206,9 @@
 
     BlueNRGGap(BlueNRGGap const &);
     void operator=(BlueNRGGap const &);
+
+    GapAdvertisingData _advData;
+    GapAdvertisingData _scanResponse;
 };
 
-#endif // ifndef __BLUENRG_GAP_H__
+#endif // ifndef __BLUENRG_GAP_H__
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/BlueNRGGattClient.h
--- a/x-nucleo-idb0xa1/BlueNRGGattClient.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/BlueNRGGattClient.h	Thu Sep 15 16:59:44 2016 +0100
@@ -34,7 +34,11 @@
 #ifndef __BLUENRG_GATT_CLIENT_H__
 #define __BLUENRG_GATT_CLIENT_H__
 
-#include "mbed.h"
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif 
 #include "ble/blecommon.h"
 #include "btle.h"
 #include "ble/GattClient.h"
@@ -178,5 +182,4 @@
   
 };
 
-#endif /* __BLUENRG_GATT_CLIENT_H__ */
-
+#endif /* __BLUENRG_GATT_CLIENT_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/BlueNRGGattServer.h
--- a/x-nucleo-idb0xa1/BlueNRGGattServer.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/BlueNRGGattServer.h	Thu Sep 15 16:59:44 2016 +0100
@@ -15,7 +15,7 @@
 */
 /**
   ******************************************************************************
-  * @file    BlueNRGGattServer.cpp 
+  * @file    BlueNRGGattServer.cpp
   * @author  STMicroelectronics
   * @brief   Header file for BLE_API GattServer Class
   ******************************************************************************
@@ -30,11 +30,15 @@
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
   */
- 
+
 #ifndef __BLUENRG_GATT_SERVER_H__
 #define __BLUENRG_GATT_SERVER_H__
 
-#include "mbed.h"
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif 
 #include "ble/blecommon.h"
 #include "btle.h"
 #include "ble/GattService.h"
@@ -53,13 +57,13 @@
         static BlueNRGGattServer m_instance;
         return m_instance;
     }
-    
+
     enum HandleEnum_t {
         CHAR_HANDLE = 0,
         CHAR_VALUE_HANDLE,
         CHAR_DESC_HANDLE
     };
-    
+
     /* Functions that must be implemented from GattServer */
     virtual ble_error_t addService(GattService &);
     virtual ble_error_t read(GattAttribute::Handle_t attributeHandle, uint8_t buffer[], uint16_t *lengthP);
@@ -67,24 +71,33 @@
     virtual ble_error_t write(GattAttribute::Handle_t, const uint8_t[], uint16_t, bool localOnly = false);
     virtual ble_error_t write(Gap::Handle_t connectionHandle, GattAttribute::Handle_t, const uint8_t[], uint16_t, bool localOnly = false);
     virtual ble_error_t initializeGATTDatabase(void);
-    
+
     virtual bool isOnDataReadAvailable() const {
         return true;
     }
 
     virtual ble_error_t reset(void);
-    
+
     /* BlueNRG Functions */
     void eventCallback(void);
     //void hwCallback(void *pckt);
     ble_error_t Read_Request_CB(uint16_t attributeHandle);
+    uint8_t Write_Request_CB(
+        uint16_t connection_handle, uint16_t attr_handle,
+        uint8_t data_length, const uint8_t* data
+    );
     GattCharacteristic* getCharacteristicFromHandle(uint16_t charHandle);
     void HCIDataWrittenEvent(const GattWriteCallbackParams *params);
     void HCIDataReadEvent(const GattReadCallbackParams *params);
     void HCIEvent(GattServerEvents::gattEvent_e type, uint16_t charHandle);
     void HCIDataSentEvent(unsigned count);
-    
+
 private:
+
+    // compute the number of attribute record needed by a service
+    static uint16_t computeAttributesRecord(GattService& service);
+
+
     static const int MAX_SERVICE_COUNT = 10;
     uint8_t serviceCount;
     uint8_t characteristicCount;
@@ -101,12 +114,12 @@
 
     BlueNRGGattServer(BlueNRGGattServer const &);
     void operator=(BlueNRGGattServer const &);
-    
-    static const int CHAR_DESC_TYPE_16_BIT=0x01; 
-    static const int CHAR_DESC_TYPE_128_BIT=0x02;    
+
+    static const int CHAR_DESC_TYPE_16_BIT=0x01;
+    static const int CHAR_DESC_TYPE_128_BIT=0x02;
     static const int CHAR_DESC_SECURITY_PERMISSION=0x00;
-    static const int CHAR_DESC_ACCESS_PERMISSION=0x03;  
-    static const int CHAR_ATTRIBUTE_LEN_IS_FIXED=0x00;        
+    static const int CHAR_DESC_ACCESS_PERMISSION=0x03;
+    static const int CHAR_ATTRIBUTE_LEN_IS_FIXED=0x00;
 };
 
-#endif
+#endif
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_clock.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_clock.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,58 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : ble_clock.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.1
+* Date               : 19-July-2012
+* Description        : Header file for clock library, that gives a simple time
+*                       reference to the BLE Stack.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+#ifndef __CLOCK_H__
+#define __CLOCK_H__
+
+#include <ble_hal_types.h>
+
+/**
+ * Number of clocks in one seconds.
+ * This value must be set by each platorm implementation, basing on its needs.
+ */
+extern const uint32_t CLOCK_SECOND;
+
+typedef uint32_t tClockTime;
+
+/**
+ * This function initializes the clock library and should be called before
+ * any other Stack functions.
+ *
+ */
+void Clock_Init(void);
+
+/**
+ * This function returns the current system clock time. it is used by
+ * the host stack and has to be implemented.
+ *
+ * @return The current clock time, measured in system ticks.
+ */
+tClockTime Clock_Time(void);
+
+/**
+ * This function waits for a given number of milliseconds.
+ *
+ */
+void Clock_Wait(uint32_t i);
+
+/**
+ * It suspends system clock.
+ *
+ */
+void Clock_Suspend(void);
+
+
+#endif /* __CLOCK_H__ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_compiler.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_compiler.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,34 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : compiler.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : Compiler-dependent macros.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#ifdef __ICCARM__
+#define PACKED
+#else
+#ifdef __GNUC__
+#undef __packed
+#define __packed
+#define PACKED __attribute__((packed))
+#else
+#define PACKED
+#define __packed
+#endif
+#endif
+
+/* Change this define to 1 if zero-length arrays are not supported by your compiler. */
+#define VARIABLE_SIZE 1
+
+#endif /* DOXYGEN_SHOULD_SKIP_THIS */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_debug.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_debug.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,73 @@
+/**
+  ******************************************************************************
+  * @file    ble_debug.h
+  * @author  CL
+  * @version V1.0.0
+  * @date    04-July-2014
+  * @brief   This file defines print functions for debug purposes.
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
+  *
+  * 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 STMicroelectronics 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.
+  *
+  ******************************************************************************
+  */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __DEBUG_H
+#define __DEBUG_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include <string.h>
+
+/* Exported macro ------------------------------------------------------------*/
+//#define DEBUG
+#ifdef DEBUG
+#include <stdio.h>
+#define PRINTF(...) printf(__VA_ARGS__)
+#else
+#define PRINTF(...)
+#endif
+
+/* Print the data travelling over the SPI in the .csv format for the GUI*/
+//#define PRINT_CSV_FORMAT
+#ifdef PRINT_CSV_FORMAT
+#include <stdio.h>
+#define PRINT_CSV(...) printf(__VA_ARGS__)
+#else
+#define PRINT_CSV(...)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __DEBUG_H */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_gp_timer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_gp_timer.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,105 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : gp_timer.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : General purpose timer library.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+#ifndef __GP_TIMER_H__
+#define __GP_TIMER_H__
+
+#include "ble_clock.h"
+#include "ble_status.h"
+#ifdef __DMA_LP__
+#include "stm32xx_timerserver.h"
+#endif /* __DMA_LP__ */
+
+/**
+ * timer
+ *
+ * A structure that represents a timer. Use Timer_Set() to set the timer.
+ *
+ */
+struct timer {
+    
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+    
+  tClockTime start;
+  tClockTime interval;
+  
+#endif
+};
+
+typedef void (* TIMER_HCI_TIMEOUT_NOTIFY_CALLBACK_TYPE)(void);
+
+/**
+ * Timer_Set
+ *
+ * @param[in] t             Pointer to a timer structure
+ * @param[in] interval      timeout value
+ *
+ * This function sets the timeout value of a timer.
+ *
+ */
+void Timer_Set(struct timer *t, tClockTime interval);
+
+/**
+ * Timer_Reset
+ * 
+ * @param[in] t     Pointer to a timer structure
+ * 
+ * This function resets the timer with the same interval given
+ * with Timer_Set, starting from the time it previously expired.
+ * 
+ */ 
+void Timer_Reset(struct timer *t);
+
+/**
+ * Timer_Restart
+ * 
+ * @param[in]  t   Pointer to a timer structure
+ * 
+ * This function resets the timer with the same interval given
+ * with Timer_Set, starting from the current time.
+ * 
+ */ 
+void Timer_Restart(struct timer *t);
+
+/**
+ * Timer_Expired
+ *
+ * @param[in] t    Pointer to a timer structure
+ *
+ * This function returns TRUE if timer is expired, FALSE otherwise.
+ *
+ */
+int Timer_Expired(struct timer *t);
+
+/**
+ * Timer_Expired
+ *
+ * @param[in] t    Pointer to a timer structure
+ *
+ * This function returns the time needed for expiration.
+ *
+ * @return  Time before timer's expiration.
+ */
+tClockTime Timer_Remaining(struct timer *t);
+
+#ifdef __DMA_LP__
+tBleStatus Blue_NRG_HCI_Timer_Start(uint32_t expiryTime,
+		TIMER_HCI_TIMEOUT_NOTIFY_CALLBACK_TYPE timercb,
+					   uint8_t *timerID);
+
+tBleStatus Blue_NRG_HCI_Timer_Stop(uint8_t timerID);
+#endif /* __DMA_LP__ */
+
+#endif /* __GP_TIMER_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_hal.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_hal.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,108 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : hal.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : Header file which defines Hardware abstraction layer APIs
+*                       used by the BLE stack. It defines the set of functions
+*                       which needs to be ported to the target platform.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+#ifndef __HAL_H__
+#define __HAL_H__
+
+/******************************************************************************
+ * Includes
+ *****************************************************************************/
+#include <ble_hal_types.h>
+#include <ble_status.h>
+
+
+/******************************************************************************
+ * Macros
+ *****************************************************************************/
+/* Little Endian buffer to host endianess conversion */
+#define LE_TO_HOST_16(ptr)  (uint16_t) ( ((uint16_t) \
+                                           *((uint8_t *)ptr)) | \
+                                          ((uint16_t) \
+                                           *((uint8_t *)ptr + 1) << 8 ) )
+
+#define LE_TO_HOST_32(ptr)   (uint32_t) ( ((uint32_t) \
+                                           *((uint8_t *)ptr)) | \
+                                           ((uint32_t) \
+                                            *((uint8_t *)ptr + 1) << 8)  | \
+                                           ((uint32_t) \
+                                            *((uint8_t *)ptr + 2) << 16) | \
+                                           ((uint32_t) \
+                                            *((uint8_t *)ptr + 3) << 24) )
+
+/* Big Endian buffer to host endianess conversion */
+#define BE_TO_HOST_16(ptr)  (uint16_t) ( ((uint16_t) \
+                                           *((uint8_t *)ptr)) << 8 | \
+                                          ((uint16_t) \
+                                           *((uint8_t *)ptr + 1) ) )
+											
+/* Store Value into a buffer in Little Endian Format */
+#define HOST_TO_LE_16(buf, val)    ( ((buf)[0] =  (uint8_t) (val)    ) , \
+                                   ((buf)[1] =  (uint8_t) (val>>8) ) )
+
+#define HOST_TO_LE_32(buf, val)    ( ((buf)[0] =  (uint8_t) (val)     ) , \
+                                   ((buf)[1] =  (uint8_t) (val>>8)  ) , \
+                                   ((buf)[2] =  (uint8_t) (val>>16) ) , \
+                                   ((buf)[3] =  (uint8_t) (val>>24) ) ) 
+
+
+/* Store Value into a buffer in Big Endian Format */
+#define HOST_TO_BE_16(buf, val)    ( ((buf)[1] =  (uint8_t) (val)    ) , \
+                                   ((buf)[0] =  (uint8_t) (val>>8) ) )
+
+#define DISABLE_INTERRUPTS() __disable_interrupt()
+#define ENABLE_INTERRUPTS() __enable_interrupt()
+#define SAVE_PRIMASK() uint32_t uwPRIMASK_Bit = __get_PRIMASK()
+#define ATOMIC_SECTION_BEGIN() uint32_t uwPRIMASK_Bit = __get_PRIMASK(); \
+                                __disable_interrupt(); \
+/* Must be called in the same or in a lower scope of SUSPEND_INTERRUPTS */
+#define ATOMIC_SECTION_END() __set_PRIMASK(uwPRIMASK_Bit)
+ 
+/******************************************************************************
+ * Types
+ *****************************************************************************/
+
+/******************************************************************************
+ * Function Prototypes
+ *****************************************************************************/
+
+/**
+ * Writes data to a serial interface.
+ *
+ * @param[in]  data1    1st buffer
+ * @param[in]  data2    2nd buffer
+ * @param[in]  n_bytes1 number of bytes in 1st buffer
+ * @param[in]  n_bytes2 number of bytes in 2nd buffer
+ */
+void Hal_Write_Serial(const void* data1, const void* data2, int32_t n_bytes1, int32_t n_bytes2);
+
+/**
+ * Enable interrupts from HCI controller.
+ */
+void Enable_SPI_IRQ(void);
+
+/**
+ * Disable interrupts from BLE controller.
+ */
+void Disable_SPI_IRQ(void);
+
+void signalEventsToProcess(void);
+
+void Hal_Init_Timer(void);
+uint32_t Hal_Get_Timer_Value(void);
+void Hal_Start_Timer(uint32_t timeout);
+void Hal_Stop_Timer(void);
+
+#endif /* __HAL_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_hal_types.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_hal_types.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,57 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : ble_hal_types.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : This header file defines the basic data types used by the
+*                       BLE stack.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+#ifndef __HAL_TYPES_H__
+#define __HAL_TYPES_H__
+
+#include <stdint.h>
+
+#ifndef NULL
+#define NULL ((void *)0)
+#endif
+
+#ifndef __LITTLE_ENDIAN
+#define __LITTLE_ENDIAN 0
+#define __BIG_ENDIAN    1
+#endif
+
+/* Byte order conversions */
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+#define htobs(d)  (d)
+#define htobl(d)  (d)
+#define btohs(d)  (d)
+#define btohl(d)  (d)
+#elif __BYTE_ORDER == __BIG_ENDIAN
+#define htobs(d)  (d<<8|d>>8)
+#define htobl(d)  (d<<24|((d<<8)&0x00ff0000)|((d>>8)&0x0000ff00)|((d>>24)&0x000000ff))
+#define btohs(d)  (d<<8|d>>8)
+#define btohl(d)  (d<<24|((d<<8)&0x00ff0000)|((d>>8)&0x0000ff00)|((d>>24)&0x000000ff))
+#else
+#error "Unknown byte order"
+#endif
+
+typedef uint8_t BOOL;
+
+#ifndef TRUE 
+#define TRUE (1)
+#endif
+
+#ifndef FALSE
+#define FALSE (0)
+#endif
+
+
+
+#endif /* __HAL_TYPES_H__ */ 
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_hci.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_hci.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,241 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : hci.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : Constants and functions for HCI layer. See Bluetooth Core
+*                      v 4.0, Vol. 2, Part E.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+#ifndef __HCI_H_
+#define __HCI_H_
+
+#include "ble_hal_types.h"
+#include "ble_link_layer.h"
+#include <ble_list.h>
+
+#define HCI_READ_PACKET_SIZE                    128 //71
+
+/*** Data types ***/
+
+/* structure used to read received data */
+typedef struct _tHciDataPacket
+{
+  tListNode currentNode;
+  uint8_t dataBuff[HCI_READ_PACKET_SIZE];
+  uint8_t data_len;
+} tHciDataPacket;
+
+typedef enum
+{
+  BUSY,
+  AVAILABLE
+} HCI_CMD_STATUS_t;
+
+/**
+ * @defgroup HCI_Error_codes HCI Error codes
+ * @{
+ */
+#define HCI_UNKNOWN_COMMAND                     0x01
+#define HCI_NO_CONNECTION                       0x02
+#define HCI_HARDWARE_FAILURE                    0x03
+#define HCI_PAGE_TIMEOUT                        0x04
+#define HCI_AUTHENTICATION_FAILURE              0x05
+#define HCI_PIN_OR_KEY_MISSING                  0x06
+#define HCI_MEMORY_FULL                         0x07
+#define HCI_CONNECTION_TIMEOUT                  0x08
+#define HCI_MAX_NUMBER_OF_CONNECTIONS		0x09
+#define HCI_MAX_NUMBER_OF_SCO_CONNECTIONS	0x0a
+#define HCI_ACL_CONNECTION_EXISTS               0x0b
+#define HCI_COMMAND_DISALLOWED                  0x0c
+#define HCI_REJECTED_LIMITED_RESOURCES          0x0d
+#define HCI_REJECTED_SECURITY                   0x0e
+#define HCI_REJECTED_PERSONAL                   0x0f
+#define HCI_HOST_TIMEOUT                        0x10
+#define HCI_UNSUPPORTED_FEATURE                 0x11
+#define HCI_INVALID_PARAMETERS                  0x12
+#define HCI_OE_USER_ENDED_CONNECTION            0x13
+#define HCI_OE_LOW_RESOURCES                    0x14
+#define HCI_OE_POWER_OFF                        0x15
+#define HCI_CONNECTION_TERMINATED               0x16
+#define HCI_REPEATED_ATTEMPTS                   0x17
+#define HCI_PAIRING_NOT_ALLOWED                 0x18
+#define HCI_UNKNOWN_LMP_PDU                     0x19
+#define HCI_UNSUPPORTED_REMOTE_FEATURE          0x1a
+#define HCI_SCO_OFFSET_REJECTED                 0x1b
+#define HCI_SCO_INTERVAL_REJECTED               0x1c
+#define HCI_AIR_MODE_REJECTED                   0x1d
+#define HCI_INVALID_LMP_PARAMETERS              0x1e
+#define HCI_UNSPECIFIED_ERROR                   0x1f
+#define HCI_UNSUPPORTED_LMP_PARAMETER_VALUE     0x20
+#define HCI_ROLE_CHANGE_NOT_ALLOWED             0x21
+#define HCI_LMP_RESPONSE_TIMEOUT                0x22
+#define HCI_LMP_ERROR_TRANSACTION_COLLISION     0x23
+#define HCI_LMP_PDU_NOT_ALLOWED                 0x24
+#define HCI_ENCRYPTION_MODE_NOT_ACCEPTED        0x25
+#define HCI_UNIT_LINK_KEY_USED                  0x26
+#define HCI_QOS_NOT_SUPPORTED                   0x27
+#define HCI_INSTANT_PASSED                      0x28
+#define HCI_PAIRING_NOT_SUPPORTED               0x29
+#define HCI_TRANSACTION_COLLISION               0x2a
+#define HCI_QOS_UNACCEPTABLE_PARAMETER          0x2c
+#define HCI_QOS_REJECTED                        0x2d
+#define HCI_CLASSIFICATION_NOT_SUPPORTED        0x2e
+#define HCI_INSUFFICIENT_SECURITY               0x2f
+#define HCI_PARAMETER_OUT_OF_RANGE              0x30
+#define HCI_ROLE_SWITCH_PENDING                 0x32
+#define HCI_SLOT_VIOLATION                      0x34
+#define HCI_ROLE_SWITCH_FAILED                  0x35
+#define HCI_EIR_TOO_LARGE                       0x36
+#define HCI_SIMPLE_PAIRING_NOT_SUPPORTED        0x37
+#define HCI_HOST_BUSY_PAIRING                   0x38
+#define HCI_CONN_REJ_NO_CH_FOUND                0x39
+#define HCI_CONTROLLER_BUSY                     0x3A
+#define HCI_UNACCEPTABLE_CONN_INTERV            0x3B
+#define HCI_DIRECTED_ADV_TIMEOUT                0x3C
+#define HCI_CONN_TERM_MIC_FAIL                  0x3D
+#define HCI_CONN_FAIL_TO_BE_ESTABL              0x3E
+#define HCI_MAC_CONN_FAILED                     0x3F
+/**
+ * @}
+ */
+
+
+/*
+ * HCI library functions.
+ * Each function returns 0 in case of success, otherwise one of the error codes.
+ */
+
+int hci_reset(void);
+
+int hci_disconnect(uint16_t handle, uint8_t reason);
+
+int hci_le_set_advertise_enable(uint8_t enable);
+
+int hci_le_set_advertising_parameters(uint16_t min_interval, uint16_t max_interval, uint8_t advtype,
+		uint8_t own_bdaddr_type, uint8_t direct_bdaddr_type, const tBDAddr direct_bdaddr, uint8_t chan_map,
+		uint8_t filter);
+
+int hci_le_set_scan_parameters(uint8_t type, uint16_t interval,
+                               uint16_t window, uint8_t own_bdaddr_type,
+                               uint8_t filter);
+
+int hci_le_set_scan_enable(uint8_t enable, uint8_t filter_dup);
+
+int hci_le_set_advertising_data(uint8_t length, const uint8_t data[]);
+
+int hci_le_set_scan_resp_data(uint8_t length, const uint8_t data[]);
+
+int hci_le_rand(uint8_t random_number[8]);
+
+int hci_le_read_advertising_channel_tx_power(int8_t *tx_power_level);
+
+int hci_acl_data(const uint8_t * data, uint16_t len);
+
+int hci_le_set_random_address(tBDAddr bdaddr);
+
+int hci_read_bd_addr(tBDAddr bdaddr);
+
+int hci_le_read_white_list_size(uint8_t *size);
+
+int hci_le_clear_white_list(void);
+
+int hci_le_add_device_to_white_list(uint8_t bdaddr_type, tBDAddr bdaddr);
+
+int hci_le_remove_device_from_white_list(uint8_t bdaddr_type, tBDAddr bdaddr);
+
+int hci_le_encrypt(uint8_t key[16], uint8_t plaintextData[16], uint8_t encryptedData[16]);
+
+int hci_le_ltk_request_reply(uint8_t key[16]);
+
+int hci_le_ltk_request_neg_reply(void);
+
+int hci_le_read_buffer_size(uint16_t *pkt_len, uint8_t *max_pkt);
+
+int hci_le_create_connection(uint16_t interval,	uint16_t window, uint8_t initiator_filter, uint8_t peer_bdaddr_type,
+                             const tBDAddr peer_bdaddr,	uint8_t	own_bdaddr_type, uint16_t min_interval,	uint16_t max_interval,
+                             uint16_t latency, uint16_t supervision_timeout, uint16_t min_ce_length, uint16_t max_ce_length);
+
+int hci_le_create_connection_cancel(void);
+
+int hci_read_transmit_power_level(uint16_t *conn_handle, uint8_t type, int8_t *tx_level);
+
+int hci_read_rssi(uint16_t *conn_handle, int8_t *rssi);
+
+int hci_le_read_local_supported_features(uint8_t *features);
+
+int hci_le_read_channel_map(uint16_t conn_handle, uint8_t ch_map[5]);
+
+int hci_le_read_supported_states(uint8_t states[8]);
+
+int hci_le_receiver_test(uint8_t frequency);
+
+int hci_le_transmitter_test(uint8_t frequency, uint8_t length, uint8_t payload);
+
+int hci_le_test_end(uint16_t *num_pkts);
+
+int hci_le_read_local_version(uint8_t *hci_version, uint16_t *hci_revision, uint8_t *lmp_pal_version, 
+			      uint16_t *manufacturer_name, uint16_t *lmp_pal_subversion);
+
+/**
+ * This function must be used to pass the packet received from the HCI
+ * interface to the BLE Stack HCI state machine.
+ *
+ * @param[in] hciReadPacket    The packet that is received from HCI interface.
+ *
+ */
+void HCI_Input(tHciDataPacket *hciReadPacket);
+
+/**
+ * Initialization function. Must be done before any data can be received from
+ * BLE controller.
+ */
+void HCI_Init(void);
+
+/**
+ * Callback used to pass events to application.
+ *
+ * @param[in] pckt    The event.
+ *
+ */
+extern void HCI_Event_CB(void *pckt);
+
+/**
+ * Processing function that must be called after an event is received from
+ * HCI interface. Must be called outside ISR. It will call HCI_Event_CB if
+ * necessary.
+*/
+void HCI_Process(void);
+
+/**
+ * @brief Check if queue of HCI event is empty or not.
+ * @note This funtion can be used to check if the event queue from BlueNRG is empty. This
+ *        is useful when checking if it is safe to go to sleep. 
+ * @return TRUE if event queue is empty. FALSE otherwhise.
+ */
+BOOL HCI_Queue_Empty(void);
+/**
+ * Iterrupt service routine that must be called when the BlueNRG 
+ * reports a packet received or an event to the host through the 
+ * BlueNRG interrupt line.
+ */
+#ifdef __DMA_LP__
+void HCI_Isr(uint8_t *buffer, uint8_t event_payload_len);
+void HCI_Process_Notification_Request(void);
+void HCI_Cmd_Status(HCI_CMD_STATUS_t Hci_Cmd_Status);
+void HCI_Wait_For_Response(void);
+#else
+void HCI_Isr(void);
+#endif /* __DMA_LP__ */
+
+extern tListNode hciReadPktPool;
+extern tListNode hciReadPktRxQueue;
+
+#endif /* __HCI_H_ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_hci_const.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_hci_const.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,563 @@
+/******************************************************************************
+*
+*  File Description 
+*  ---------------------
+* This file defines constants and functions for HCI layer.
+* See Bluetooth Core v 4.0, Vol. 2, Part E.
+* 
+*******************************************************************************/
+
+#ifndef __HCI_INTERNAL_H_
+#define __HCI_INTERNAL_H_
+
+#include "ble_compiler.h"
+#include "ble_hal_types.h"
+#include "ble_clock.h"
+#include "ble_link_layer.h"
+#include "ble_hci.h"
+
+#define DEFAULT_TIMEOUT (CLOCK_SECOND/10)
+
+/**
+ * Maximum payload of HCI commands that can be sent. Change this value if needed.
+ * This value can be up to 255.
+ */
+#define HCI_MAX_PAYLOAD_SIZE 128
+
+/* HCI Packet types */
+#define HCI_COMMAND_PKT		0x01
+#define HCI_ACLDATA_PKT		0x02
+#define HCI_SCODATA_PKT		0x03
+#define HCI_EVENT_PKT		0x04
+#define HCI_VENDOR_PKT		0xff
+
+typedef __packed struct _hci_uart_pckt{
+  uint8_t type;
+  uint8_t data[VARIABLE_SIZE];
+} PACKED hci_uart_pckt;
+#define HCI_HDR_SIZE 1
+
+typedef __packed struct _hci_command_hdr{
+  uint16_t opcode;		/* OCF & OGF */
+  uint8_t  plen;
+} PACKED hci_command_hdr;
+#define HCI_COMMAND_HDR_SIZE 	3
+
+typedef __packed struct _hci_event_pckt{
+  uint8_t evt;
+  uint8_t plen;
+  uint8_t data[VARIABLE_SIZE];
+} PACKED hci_event_pckt;
+#define HCI_EVENT_HDR_SIZE 	2
+
+typedef __packed struct _hci_acl_hdr{
+  uint16_t handle;		/* Handle & Flags(PB, BC) */
+  uint16_t dlen;
+} PACKED hci_acl_hdr;
+#define HCI_ACL_HDR_SIZE 	4
+
+/* Link Control */
+#define OGF_LINK_CTL	        0x01
+
+#define OCF_DISCONNECT          0x0006
+typedef __packed struct _disconnect_cp{
+  uint16_t handle;
+  uint8_t  reason;
+} PACKED disconnect_cp;
+#define DISCONNECT_CP_SIZE      3
+
+/* Host Controller and Baseband */
+#define OGF_HOST_CTL            0x03
+
+#define OCF_SET_EVENT_MASK      0x0001
+#define OCF_RESET               0x0003
+
+#define OCF_READ_TRANSMIT_POWER_LEVEL	0x002D
+typedef __packed struct _read_transmit_power_level_cp{
+  uint16_t handle;
+  uint8_t  type;
+} PACKED read_transmit_power_level_cp;
+#define READ_TRANSMIT_POWER_LEVEL_CP_SIZE 3
+typedef __packed struct _read_transmit_power_level_rp{
+  uint8_t  status;
+  uint16_t handle;
+  int8_t   level;
+} PACKED read_transmit_power_level_rp;
+#define READ_TRANSMIT_POWER_LEVEL_RP_SIZE 4
+
+#define OCF_SET_CONTROLLER_TO_HOST_FC   0x0031
+#define OCF_HOST_BUFFER_SIZE            0x0033
+#define OCF_HOST_NUM_COMP_PKTS          0x0035
+
+/* Informational Parameters */
+#define OGF_INFO_PARAM                  0x04
+
+#define OCF_READ_LOCAL_VERSION          0x0001
+typedef __packed struct _read_local_version_rp{
+  uint8_t  status;
+  uint8_t  hci_version;
+  uint16_t hci_revision;
+  uint8_t  lmp_pal_version;
+  uint16_t manufacturer_name;
+  uint16_t lmp_pal_subversion;
+} PACKED read_local_version_rp;
+#define READ_LOCAL_VERSION_RP_SIZE      9
+
+#define OCF_READ_LOCAL_COMMANDS         0x0002
+#define OCF_READ_LOCAL_FEATURES         0x0003
+
+#define OCF_READ_BD_ADDR                0x0009
+typedef __packed struct _read_bd_addr_rp{
+  uint8_t status;
+  tBDAddr bdaddr;
+} PACKED read_bd_addr_rp;
+#define READ_BD_ADDR_RP_SIZE            7
+
+/* Status params */
+#define OGF_STATUS_PARAM                0x05
+
+#define OCF_READ_RSSI                   0x0005
+typedef __packed struct _read_rssi_cp{
+  uint16_t handle;
+} PACKED read_rssi_cp;
+#define READ_RSSI_CP_SIZE               2
+typedef __packed struct _read_rssi_rp{
+  uint8_t  status;
+  uint16_t handle;
+  int8_t   rssi;
+} PACKED read_rssi_rp;
+#define READ_RSSI_RP_SIZE               4
+
+
+/* LE commands */
+#define OGF_LE_CTL                      0x08
+
+#define OCF_LE_SET_EVENT_MASK           0x0001
+typedef __packed struct _le_set_event_mask_cp{
+  uint8_t mask[8];
+} PACKED le_set_event_mask_cp;
+#define LE_SET_EVENT_MASK_CP_SIZE       8
+
+#define OCF_LE_READ_BUFFER_SIZE         0x0002
+typedef __packed struct _le_read_buffer_size_rp{
+  uint8_t  status;
+  uint16_t pkt_len;
+  uint8_t  max_pkt;
+} PACKED le_read_buffer_size_rp;
+#define LE_READ_BUFFER_SIZE_RP_SIZE     4
+
+#define OCF_LE_READ_LOCAL_SUPPORTED_FEATURES	0x0003
+typedef __packed struct _le_read_local_supported_features_rp{
+  uint8_t status;
+  uint8_t features[8];
+} PACKED le_read_local_supported_features_rp;
+#define LE_READ_LOCAL_SUPPORTED_FEATURES_RP_SIZE 9
+
+#define OCF_LE_SET_RANDOM_ADDRESS		0x0005
+typedef __packed struct _le_set_random_address_cp{
+  tBDAddr bdaddr;
+} PACKED le_set_random_address_cp;
+#define LE_SET_RANDOM_ADDRESS_CP_SIZE 6
+
+#define OCF_LE_SET_ADV_PARAMETERS	0x0006
+typedef __packed struct _le_set_adv_parameters_cp{
+  uint16_t min_interval;
+  uint16_t max_interval;
+  uint8_t  advtype;
+  uint8_t  own_bdaddr_type;
+  uint8_t  direct_bdaddr_type;
+  tBDAddr  direct_bdaddr;
+  uint8_t  chan_map;
+  uint8_t  filter;
+} PACKED le_set_adv_parameters_cp;
+#define LE_SET_ADV_PARAMETERS_CP_SIZE 15
+
+#define OCF_LE_READ_ADV_CHANNEL_TX_POWER	0x0007
+typedef __packed struct _le_read_adv_channel_tx_power_rp{
+  uint8_t status;
+  int8_t  level;
+} PACKED le_read_adv_channel_tx_power_rp;
+#define LE_READ_ADV_CHANNEL_TX_POWER_RP_SIZE 2
+
+#define OCF_LE_SET_ADV_DATA		0x0008
+typedef __packed struct _le_set_adv_data_cp{
+  uint8_t length;
+  uint8_t data[31];
+} PACKED le_set_adv_data_cp;
+#define LE_SET_ADV_DATA_CP_SIZE 32
+
+#define OCF_LE_SET_SCAN_RESPONSE_DATA		0x0009
+typedef __packed struct _le_set_scan_response_data_cp{
+  uint8_t length;
+  uint8_t data[31];
+} PACKED le_set_scan_response_data_cp;
+#define LE_SET_SCAN_RESPONSE_DATA_CP_SIZE 32
+
+#define OCF_LE_SET_ADVERTISE_ENABLE		0x000A
+typedef __packed struct _le_set_advertise_enable_cp{
+  uint8_t enable;
+} PACKED le_set_advertise_enable_cp;
+#define LE_SET_ADVERTISE_ENABLE_CP_SIZE 1
+
+#define OCF_LE_SET_SCAN_PARAMETERS		0x000B
+typedef __packed struct _le_set_scan_parameters_cp{
+  uint8_t  type;
+  uint16_t interval;
+  uint16_t window;
+  uint8_t  own_bdaddr_type;
+  uint8_t  filter;
+} PACKED le_set_scan_parameters_cp;
+#define LE_SET_SCAN_PARAMETERS_CP_SIZE 7
+
+#define OCF_LE_SET_SCAN_ENABLE			0x000C
+typedef __packed struct _le_set_scan_enable_cp{
+  uint8_t enable;
+  uint8_t filter_dup;
+} PACKED le_set_scan_enable_cp;
+#define LE_SET_SCAN_ENABLE_CP_SIZE 2
+
+#define OCF_LE_CREATE_CONN			0x000D
+typedef __packed struct _le_create_connection_cp{
+  uint16_t interval;
+  uint16_t window;
+  uint8_t  initiator_filter;
+  uint8_t  peer_bdaddr_type;
+  tBDAddr  peer_bdaddr;
+  uint8_t  own_bdaddr_type;
+  uint16_t min_interval;
+  uint16_t max_interval;
+  uint16_t latency;
+  uint16_t supervision_timeout;
+  uint16_t min_ce_length;
+  uint16_t max_ce_length;
+} PACKED le_create_connection_cp;
+#define LE_CREATE_CONN_CP_SIZE 25
+
+#define OCF_LE_CREATE_CONN_CANCEL		0x000E
+
+#define OCF_LE_READ_WHITE_LIST_SIZE		0x000F
+typedef __packed struct _le_read_white_list_size_rp{
+  uint8_t status;
+  uint8_t size;
+} PACKED le_read_white_list_size_rp;
+#define LE_READ_WHITE_LIST_SIZE_RP_SIZE 2
+
+#define OCF_LE_CLEAR_WHITE_LIST			0x0010
+
+#define OCF_LE_ADD_DEVICE_TO_WHITE_LIST		0x0011
+typedef __packed struct _le_add_device_to_white_list_cp{
+  uint8_t bdaddr_type;
+  tBDAddr bdaddr;
+} PACKED le_add_device_to_white_list_cp;
+#define LE_ADD_DEVICE_TO_WHITE_LIST_CP_SIZE 7
+
+#define OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST	0x0012
+typedef __packed struct _le_remove_device_from_white_list_cp{
+  uint8_t bdaddr_type;
+  tBDAddr bdaddr;
+} PACKED le_remove_device_from_white_list_cp;
+#define LE_REMOVE_DEVICE_FROM_WHITE_LIST_CP_SIZE 7
+
+#define OCF_LE_CONN_UPDATE			0x0013
+typedef __packed struct _le_connection_update_cp{
+  uint16_t handle;
+  uint16_t min_interval;
+  uint16_t max_interval;
+  uint16_t latency;
+  uint16_t supervision_timeout;
+  uint16_t min_ce_length;
+  uint16_t max_ce_length;
+} PACKED le_connection_update_cp;
+#define LE_CONN_UPDATE_CP_SIZE 14
+
+#define OCF_LE_SET_HOST_CHANNEL_CLASSIFICATION	0x0014
+typedef __packed struct _le_set_host_channel_classification_cp{
+  uint8_t map[5];
+} PACKED le_set_host_channel_classification_cp;
+#define LE_SET_HOST_CHANNEL_CLASSIFICATION_CP_SIZE 5
+
+#define OCF_LE_READ_CHANNEL_MAP			0x0015
+typedef __packed struct _le_read_channel_map_cp{
+  uint16_t handle;
+} PACKED le_read_channel_map_cp;
+#define LE_READ_CHANNEL_MAP_CP_SIZE 2
+
+typedef __packed struct _le_read_channel_map_rp{
+  uint8_t  status;
+  uint16_t handle;
+  uint8_t  map[5];
+} PACKED le_read_channel_map_rp;
+#define LE_READ_CHANNEL_MAP_RP_SIZE 8
+
+#define OCF_LE_READ_REMOTE_USED_FEATURES	0x0016
+typedef __packed struct _le_read_remote_used_features_cp{
+  uint16_t handle;
+} PACKED le_read_remote_used_features_cp;
+#define LE_READ_REMOTE_USED_FEATURES_CP_SIZE 2
+
+#define OCF_LE_ENCRYPT				0x0017
+typedef __packed struct _le_encrypt_cp{
+  uint8_t key[16];
+  uint8_t plaintext[16];
+} PACKED le_encrypt_cp;
+#define LE_ENCRYPT_CP_SIZE 32
+
+typedef __packed struct _le_encrypt_rp{
+  uint8_t status;
+  uint8_t encdata[16];
+} PACKED le_encrypt_rp;
+#define LE_ENCRYPT_RP_SIZE 17
+
+#define OCF_LE_RAND				0x0018
+typedef __packed struct _le_rand_rp{
+  uint8_t status;
+  uint8_t random[8];
+} PACKED le_rand_rp;
+#define LE_RAND_RP_SIZE 9
+
+#define OCF_LE_START_ENCRYPTION			0x0019
+typedef __packed struct _le_start_encryption_cp{
+  uint16_t handle;
+  uint8_t  random[8];
+  uint16_t diversifier;
+  uint8_t  key[16];
+} PACKED le_start_encryption_cp;
+#define LE_START_ENCRYPTION_CP_SIZE 28
+
+#define OCF_LE_LTK_REPLY			0x001A
+typedef __packed struct _le_ltk_reply_cp{
+  uint16_t handle;
+  uint8_t  key[16];
+} PACKED le_ltk_reply_cp;
+#define LE_LTK_REPLY_CP_SIZE 18
+
+typedef __packed struct _le_ltk_reply_rp{
+  uint8_t  status;
+  uint16_t handle;
+} PACKED le_ltk_reply_rp;
+#define LE_LTK_REPLY_RP_SIZE 3
+
+#define OCF_LE_LTK_NEG_REPLY			0x001B
+typedef __packed struct _le_ltk_neg_reply_cp{
+  uint16_t handle;
+} PACKED le_ltk_neg_reply_cp;
+#define LE_LTK_NEG_REPLY_CP_SIZE 2
+
+typedef __packed struct _le_ltk_neg_reply_rp{
+  uint8_t  status;
+  uint16_t handle;
+} PACKED le_ltk_neg_reply_rp;
+#define LE_LTK_NEG_REPLY_RP_SIZE 3
+
+#define OCF_LE_READ_SUPPORTED_STATES		0x001C
+typedef __packed struct _le_read_supported_states_rp{
+  uint8_t status;
+  uint8_t states[8];
+} PACKED le_read_supported_states_rp;
+#define LE_READ_SUPPORTED_STATES_RP_SIZE 9
+
+#define OCF_LE_RECEIVER_TEST			0x001D
+typedef __packed struct _le_receiver_test_cp{
+  uint8_t frequency;
+} PACKED le_receiver_test_cp;
+#define LE_RECEIVER_TEST_CP_SIZE 1
+
+#define OCF_LE_TRANSMITTER_TEST			0x001E
+typedef __packed struct _le_transmitter_test_cp{
+  uint8_t frequency;
+  uint8_t length;
+  uint8_t payload;
+} PACKED le_transmitter_test_cp;
+#define LE_TRANSMITTER_TEST_CP_SIZE 3
+
+#define OCF_LE_TEST_END				0x001F
+typedef __packed struct _le_test_end_rp{
+  uint8_t  status;
+  uint16_t num_pkts;
+} PACKED le_test_end_rp;
+#define LE_TEST_END_RP_SIZE 3
+
+/* Vendor specific commands */
+#define OGF_VENDOR_CMD		0x3f
+
+
+/*------------- Events -------------*/
+#define EVT_CONN_COMPLETE		0x03
+typedef __packed struct _evt_conn_complete{
+  uint8_t  status;
+  uint16_t handle;
+  tBDAddr  bdaddr;
+  uint8_t  link_type;
+  uint8_t  encr_mode;
+} PACKED evt_conn_complete;
+#define EVT_CONN_COMPLETE_SIZE 13
+
+#define EVT_DISCONN_COMPLETE		0x05
+typedef __packed struct _evt_disconn_complete{
+  uint8_t  status;
+  uint16_t handle;
+  uint8_t  reason;
+} PACKED evt_disconn_complete;
+#define EVT_DISCONN_COMPLETE_SIZE 4
+
+#define EVT_ENCRYPT_CHANGE		0x08
+typedef __packed struct _evt_encrypt_change{
+  uint8_t  status;
+  uint16_t handle;
+  uint8_t  encrypt;
+} PACKED evt_encrypt_change;
+#define EVT_ENCRYPT_CHANGE_SIZE 5
+
+#define EVT_READ_REMOTE_VERSION_COMPLETE	0x0C
+
+#define EVT_CMD_COMPLETE 		0x0E
+typedef __packed struct _evt_cmd_complete{
+  uint8_t  ncmd;
+  uint16_t opcode;
+} PACKED evt_cmd_complete;
+#define EVT_CMD_COMPLETE_SIZE 3
+
+#define EVT_CMD_STATUS 			0x0F
+typedef __packed struct _evt_cmd_status{
+  uint8_t  status;
+  uint8_t  ncmd;
+  uint16_t opcode;
+} PACKED evt_cmd_status;
+#define EVT_CMD_STATUS_SIZE 4
+
+#define EVT_HARDWARE_ERROR		0x10
+typedef __packed struct _evt_hardware_error{
+  uint8_t code;
+} PACKED evt_hardware_error;
+#define EVT_HARDWARE_ERROR_SIZE 1
+
+#define EVT_NUM_COMP_PKTS		0x13
+typedef __packed struct _evt_num_comp_pkts{
+  uint8_t num_hndl;
+  /* variable length part */
+} PACKED evt_num_comp_pkts;
+#define EVT_NUM_COMP_PKTS_SIZE 1
+
+/* variable length part of evt_num_comp_pkts. */
+typedef __packed struct _evt_num_comp_pkts_param{
+  uint16_t hndl;
+  uint16_t num_comp_pkts;
+} PACKED evt_num_comp_pkts_param;
+#define EVT_NUM_COMP_PKTS_PARAM_SIZE 1
+
+#define EVT_DATA_BUFFER_OVERFLOW		0x1A
+typedef __packed struct _evt_data_buffer_overflow{
+  uint8_t link_type;
+} PACKED evt_data_buffer_overflow;
+#define EVT_DATA_BUFFER_OVERFLOW_SIZE 1
+
+#define EVT_ENCRYPTION_KEY_REFRESH_COMPLETE	0x30
+typedef __packed struct _evt_encryption_key_refresh_complete{
+  uint8_t  status;
+  uint16_t handle;
+} PACKED evt_encryption_key_refresh_complete;
+#define EVT_ENCRYPTION_KEY_REFRESH_COMPLETE_SIZE 3
+
+#define EVT_LE_META_EVENT	0x3E
+typedef __packed struct _evt_le_meta_event{
+  uint8_t subevent;
+  uint8_t data[VARIABLE_SIZE];
+} PACKED evt_le_meta_event;
+#define EVT_LE_META_EVENT_SIZE 1
+
+#define EVT_LE_CONN_COMPLETE	0x01
+typedef __packed struct _evt_le_connection_complete{
+  uint8_t  status;
+  uint16_t handle;
+  uint8_t  role;
+  uint8_t  peer_bdaddr_type;
+  tBDAddr  peer_bdaddr;
+  uint16_t interval;
+  uint16_t latency;
+  uint16_t supervision_timeout;
+  uint8_t  master_clock_accuracy;
+} PACKED evt_le_connection_complete;
+#define EVT_LE_CONN_COMPLETE_SIZE 18
+
+#define EVT_LE_ADVERTISING_REPORT	0x02
+typedef __packed struct _le_advertising_info{
+  uint8_t evt_type;
+  uint8_t bdaddr_type;
+  tBDAddr bdaddr;
+  uint8_t data_length;
+  uint8_t data_RSSI[VARIABLE_SIZE]; // RSSI is last octect (signed integer).
+} PACKED le_advertising_info;
+#define LE_ADVERTISING_INFO_SIZE 9
+
+#define EVT_LE_CONN_UPDATE_COMPLETE	0x03
+typedef __packed struct _evt_le_connection_update_complete{
+  uint8_t  status;
+  uint16_t handle;
+  uint16_t interval;
+  uint16_t latency;
+  uint16_t supervision_timeout;
+} PACKED evt_le_connection_update_complete;
+#define EVT_LE_CONN_UPDATE_COMPLETE_SIZE 9
+
+#define EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE	0x04
+typedef __packed struct _evt_le_read_remote_used_features_complete{
+  uint8_t  status;
+  uint16_t handle;
+  uint8_t  features[8];
+} PACKED evt_le_read_remote_used_features_complete;
+#define EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE_SIZE 11
+
+#define EVT_LE_LTK_REQUEST	0x05
+typedef __packed struct _evt_le_long_term_key_request{
+  uint16_t handle;
+  uint8_t  random[8];
+  uint16_t ediv;
+} PACKED evt_le_long_term_key_request;
+#define EVT_LE_LTK_REQUEST_SIZE 12
+
+/**
+* The event code in the @ref hci_event_pckt structure. If event code is EVT_VENDOR,
+* application can use @ref evt_blue_aci structure to parse the packet.
+*/
+#define EVT_VENDOR	0xFF
+
+
+/* Command opcode pack/unpack */
+#define cmd_opcode_pack(ogf, ocf)	(uint16_t)((ocf & 0x03ff)|(ogf << 10))
+#define cmd_opcode_ogf(op)		(op >> 10)
+#define cmd_opcode_ocf(op)		(op & 0x03ff)
+
+
+struct hci_request {
+  uint16_t ogf;
+  uint16_t ocf;
+  int      event;
+  void     *cparam;
+  int      clen;
+  void     *rparam;
+  int      rlen;
+};
+
+void hci_send_cmd(uint16_t ogf, uint16_t ocf, uint8_t plen, void *param);
+
+typedef enum {
+  WAITING_TYPE,
+  WAITING_OPCODE1,
+  WAITING_OPCODE2,
+  WAITING_EVENT_CODE,
+  WAITING_HANDLE,
+  WAITING_HANDLE_FLAG,
+  WAITING_PARAM_LEN,
+  WAITING_DATA_LEN1,
+  WAITING_DATA_LEN2,
+  WAITING_PAYLOAD
+}hci_state;
+
+typedef void (*hci_packet_complete_callback)(void *pckt, uint16_t len);
+
+/* HCI library functions. */
+void hci_init(void);
+
+int hci_send_req(struct hci_request *r, BOOL async);
+
+#endif /* __HCI_INTERNAL_H_ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_link_layer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_link_layer.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,161 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : link_layer.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : Header file for BlueNRG's link layer. It contains
+*                      definition of functions for link layer, most of which are
+*                      mapped to HCI commands.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+#ifndef _LINK_LAYER_H
+#define _LINK_LAYER_H
+
+#include <ble_status.h>
+
+/**
+ *@addtogroup GAP GAP
+ *@brief API for GAP layer.
+ *@{
+ */
+
+/**
+ *@name Advertising filter 
+ *Advertising policy for filtering (white list related) 
+ *@{
+ */
+#define NO_WHITE_LIST_USE           (0x00)  /**< Process scan and connection requests from all devices (i.e., the White List is not in use) */
+#define WHITE_LIST_FOR_ONLY_SCAN    (0x01)  /**< Process connection requests from all devices and only scan requests from devices that are in the White List */
+#define WHITE_LIST_FOR_ONLY_CONN    (0x02)  /**< Process scan requests from all devices and only connection requests from devices that are in the White List */
+#define WHITE_LIST_FOR_ALL          (0x03)  /**< Process scan and connection requests only from devices in the White List. */
+/**
+ * @}
+ */
+
+
+/**
+ * Bluetooth 48 bit address (in little-endian order).
+ */
+typedef	uint8_t	tBDAddr[6];
+
+
+/**
+ *@name Bluetooth address types
+ * Bluetooth address types
+ *@{
+ */
+#define PUBLIC_ADDR                 (0)
+#define RANDOM_ADDR                 (1)
+#define STATIC_RANDOM_ADDR          (1)
+#define RESOLVABLE_PRIVATE_ADDR     (2)
+#define NON_RESOLVABLE_PRIVATE_ADDR (3)
+/**
+ * @}
+ */
+   
+/**
+ *@name Directed advertising types
+ * Type of advertising during directed advertising
+ *@{
+ */
+#define HIGH_DUTY_CYCLE_DIRECTED_ADV   (1)
+#define LOW_DUTY_CYCLE_DIRECTED_ADV    (4)
+/**
+ * @}
+ */
+
+/** 
+ * @name Advertising type
+ * @{
+ */
+
+/**
+ * undirected scannable and connectable 
+ */ 
+#define ADV_IND         (0x00)
+
+/** 
+ * directed non scannable
+ */
+#define ADV_DIRECT_IND  (0x01)
+
+/**
+ * scannable non connectable
+ */
+#define ADV_SCAN_IND    (0x02)
+
+/**
+ * non-connectable and no scan response (used for passive scan)
+ */
+#define ADV_NONCONN_IND (0x03)
+
+/**
+ * scan response 
+ */
+#define SCAN_RSP        (0x04)
+
+/**
+ * @}
+ */
+
+/* 0X05-0XFF RESERVED */
+
+/**
+ * @name Advertising ranges
+ * @{
+ */
+
+/** 
+ * lowest allowed interval value for connectable types(20ms)..multiple of 625us
+ */
+#define ADV_INTERVAL_LOWEST_CONN    (0X0020)
+
+/** 
+ * highest allowed interval value (10.24s)..multiple of 625us.
+ */
+#define ADV_INTERVAL_HIGHEST        (0X4000)
+
+/** 
+ * lowest allowed interval value for non connectable types
+ * (100ms)..multiple of 625us.
+ */
+#define ADV_INTERVAL_LOWEST_NONCONN (0X00a0)
+
+/**
+ * @}
+ */
+
+/**
+ * @name Advertising channels
+ * @{
+ */
+#define ADV_CH_37 0x01
+#define ADV_CH_38 0x02
+#define ADV_CH_39 0x04
+/**
+ * @}
+ */
+
+/**
+ * @name Scan_types Scan types
+ * @{
+ */
+#define PASSIVE_SCAN    0
+#define ACTIVE_SCAN     1
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+
+#endif /* _LINK_LAYER_H */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_list.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_list.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,47 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : list.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : Header file for linked list library.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+#ifndef _LIST_H_
+#define _LIST_H_
+
+typedef struct _tListNode {
+	struct _tListNode * next;
+	struct _tListNode * prev;
+}tListNode, *pListNode;
+
+void list_init_head (tListNode * listHead);
+
+uint8_t list_is_empty (tListNode * listHead);
+
+void list_insert_head (tListNode * listHead, tListNode * node);
+
+void list_insert_tail (tListNode * listHead, tListNode * node);
+
+void list_remove_node (tListNode * node);
+
+void list_remove_head (tListNode * listHead, tListNode ** node );
+
+void list_remove_tail (tListNode * listHead, tListNode ** node );
+
+void list_insert_node_after (tListNode * node, tListNode * ref_node);
+
+void list_insert_node_before (tListNode * node, tListNode * ref_node);
+
+int list_get_size (tListNode * listHead);
+
+void list_get_next_node (tListNode * ref_node, tListNode ** node);
+
+void list_get_prev_node (tListNode * ref_node, tListNode ** node);
+
+#endif /* _LIST_H_ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_osal.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_osal.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,81 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : ble_osal.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : This header file defines the OS abstraction layer used by
+*                      the BLE stack. OSAL defines the set of functions
+*                      which needs to be ported to target operating system and
+*                      target platform.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+#ifndef __OSAL_H__
+#define __OSAL_H__
+
+/******************************************************************************
+ * Includes
+ *****************************************************************************/
+#include <ble_hal_types.h>
+#ifdef __ICCARM__
+#include <intrinsics.h>
+#endif
+
+/******************************************************************************
+ * Macros
+ *****************************************************************************/
+
+
+/******************************************************************************
+ * Function Prototypes
+ *****************************************************************************/
+
+/**
+ * This function copies size number of bytes from a 
+ * memory location pointed by src to a destination 
+ * memory location pointed by dest
+ * 
+ * @param[in] dest Destination address
+ * @param[in] src  Source address
+ * @param[in] size size in the bytes  
+ * 
+ * @return  Address of the destination
+ */
+ 
+extern void* Osal_MemCpy(void *dest,const void *src, unsigned int size);
+
+
+/**
+ * This function sets first number of bytes, specified
+ * by size, to the destination memory pointed by ptr 
+ * to the specified value
+ * 
+ * @param[in] ptr    Destination address
+ * @param[in] value  Value to be set
+ * @param[in] size   Size in the bytes  
+ * 
+ * @return  Address of the destination
+ */
+ 
+extern void* Osal_MemSet(void *ptr, int value, unsigned int size);
+
+/**
+ * Osal_Get_Cur_Time
+ * 
+ * returns the current time in milliseconds
+ */
+/**
+ * Returns the number of ticks (1 tick = 1 millisecond)
+ * 
+ * @return  Time in milliseconds
+ */
+uint32_t Osal_Get_Cur_Time(void);
+
+
+#endif /* __OSAL_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_sm.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_sm.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,158 @@
+/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
+* File Name          : sm.h
+* Author             : AMS - HEA&RF BU
+* Version            : V1.0.0
+* Date               : 19-July-2012
+* Description        : Header file for BlueNRG's security manager.
+********************************************************************************
+* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*******************************************************************************/
+
+#ifndef __SM_H__
+#define __SM_H__
+
+/******************************************************************************
+* Macros
+*****************************************************************************/
+
+/**
+ *@addtogroup GAP GAP
+ *@brief API for GAP layer.
+ *@{
+ */
+
+/* IO capabilities */
+/**
+ * @anchor IO_capabilities
+ * @name IO capabilities
+ * @{
+ */
+#define IO_CAP_DISPLAY_ONLY				(0x00)
+#define IO_CAP_DISPLAY_YES_NO			(0x01)
+#define IO_CAP_KEYBOARD_ONLY			(0x02)
+#define IO_CAP_NO_INPUT_NO_OUTPUT		(0x03)
+#define IO_CAP_KEYBOARD_DISPLAY			(0x04)
+/**
+ * @}
+ */
+
+/**
+ * @anchor Auth_req
+ * @name Authentication requirements
+ * @{
+ */
+#define BONDING				            (0x01)
+#define NO_BONDING				        (0x00)
+/**
+ * @}
+ */
+
+/**
+ * @anchor MITM_req
+ * @name MITM protection requirements
+ * @{
+ */
+#define MITM_PROTECTION_NOT_REQUIRED	(0x00)
+#define MITM_PROTECTION_REQUIRED        (0x01)
+/**
+ * @}
+ */
+
+/**
+ * @anchor OOB_Data
+ * @name Out-Of-Band data
+ * @{
+ */
+#define OOB_AUTH_DATA_ABSENT		    (0x00)
+#define OOB_AUTH_DATA_PRESENT      		(0x01)
+/**
+ * @}
+ */
+
+/**
+ * @anchor Author_req
+ * @name Authorization requirements
+ * @{
+ */
+#define AUTHORIZATION_NOT_REQUIRED	     (0x00)
+#define AUTHORIZATION_REQUIRED			 (0x01)
+/**
+ * @}
+ */
+
+/**
+ * @anchor Conn_authorization
+ * @name Connection authorization
+ * @{
+ */
+#define CONNECTION_AUTHORIZED			(0x01)
+#define CONNECTION_REJECTED				(0x02)
+/**
+ * @}
+ */
+
+/**
+ * @anchor Use_fixed_pin
+ * @name Use fixed pin
+ * @{
+ */
+#define USE_FIXED_PIN_FOR_PAIRING		(0x0)
+#define DONOT_USE_FIXED_PIN_FOR_PAIRING	(0x01)
+/**
+ * @}
+ */
+
+/**
+ * @anchor link_security_status
+ * @name Link security status
+ * @{
+ */
+#define SM_LINK_AUTHENTICATED               (0x01)
+#define SM_LINK_AUTHORIZED                  (0x02)
+#define SM_LINK_ENCRYPTED                   (0x04)
+/**
+ * @}
+ */
+
+/**
+ * @anchor SMP_pairing_failed_codes
+ * @name SMP pairing failed reason codes
+ * @{
+ */
+#define PASSKEY_ENTRY_FAILED			(0x01)
+#define OOB_NOT_AVAILABLE				(0x02)
+#define AUTH_REQ_CANNOT_BE_MET			(0x03)
+#define CONFIRM_VALUE_FAILED			(0x04)
+#define PAIRING_NOT_SUPPORTED			(0x05)
+#define INSUFF_ENCRYPTION_KEY_SIZE		(0x06)
+#define CMD_NOT_SUPPORTED				(0x07)
+#define UNSPECIFIED_REASON				(0x08)
+#define VERY_EARLY_NEXT_ATTEMPT			(0x09)
+#define SM_INVALID_PARAMS				(0x0A)
+/**
+ * @}
+ */
+
+/**
+ * @anchor pairing_failed_codes
+ * @name Pairing failed error codes
+ * Error codes in @ref EVT_BLUE_GAP_PAIRING_CMPLT event
+ * @{
+ */
+#define SM_PAIRING_SUCCESS              (0x00)
+#define SM_PAIRING_TIMEOUT              (0x01)
+#define SM_PAIRING_FAILED               (0x02)
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+#endif /* __SM_H__ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/ble_status.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/ble_status.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/bluenrg-hci/ble_status.h	Thu Sep 15 16:59:44 2016 +0100
@@ -15,7 +15,7 @@
 #ifndef __BLE_STATUS_H__
 #define __BLE_STATUS_H__
 
-#include <hal_types.h>
+#include <ble_hal_types.h>
 
 
 /** @defgroup ble_status Bluetooth Status/Error Codes
@@ -112,4 +112,4 @@
  */
  
 
-#endif /* __BLE_STATUS_H__ */
+#endif /* __BLE_STATUS_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/bluenrg_aci_const.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_aci_const.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_aci_const.h	Thu Sep 15 16:59:44 2016 +0100
@@ -16,9 +16,9 @@
 #ifndef __BLUENRG_ACI_CONST_H_
 #define __BLUENRG_ACI_CONST_H_
 
-#include "compiler.h"
-#include "link_layer.h"
-#include "hci_const.h"
+#include "ble_compiler.h"
+#include "ble_link_layer.h"
+#include "ble_hci_const.h"
 #include "bluenrg_gatt_server.h"
 
 #ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -777,4 +777,4 @@
  * @}
  */
 
-#endif /* __BLUENRG_ACI_CONST_H_ */
+#endif /* __BLUENRG_ACI_CONST_H_ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/bluenrg_gap.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_gap.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_gap.h	Thu Sep 15 16:59:44 2016 +0100
@@ -15,7 +15,7 @@
 #ifndef __BNRG_GAP_H__
 #define __BNRG_GAP_H__
 
-#include <link_layer.h>
+#include <ble_link_layer.h>
 
 /**
  *@addtogroup GAP GAP
@@ -227,4 +227,4 @@
  * @}
  */
 
-#endif /* __BNRG_GAP_H__ */
+#endif /* __BNRG_GAP_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/bluenrg_gatt_server.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_gatt_server.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_gatt_server.h	Thu Sep 15 16:59:44 2016 +0100
@@ -16,7 +16,7 @@
 #ifndef __BNRG_GATT_SERVER_H__
 #define __BNRG_GATT_SERVER_H__
 
-#include "compiler.h"
+#include "ble_compiler.h"
 #include "ble_status.h"
 
 /**
@@ -220,4 +220,4 @@
 
 
 
-#endif /* __BNRG_GATT_SERVER_H__ */
+#endif /* __BNRG_GATT_SERVER_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/bluenrg_updater_aci.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_updater_aci.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_updater_aci.h	Thu Sep 15 16:59:44 2016 +0100
@@ -16,7 +16,7 @@
 #ifndef __BLUENRG_UPDATER_ACI_H__
 #define __BLUENRG_UPDATER_ACI_H__
 
-#include <compiler.h>
+#include <ble_compiler.h>
 
 /**
  * @defgroup Updater Updater
@@ -75,4 +75,4 @@
 
 
 
-#endif /* __BLUENRG_UPDATER_ACI_H__ */
+#endif /* __BLUENRG_UPDATER_ACI_H__ */
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/bluenrg_utils.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_utils.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/bluenrg-hci/bluenrg_utils.h	Thu Sep 15 16:59:44 2016 +0100
@@ -27,8 +27,8 @@
 #endif
 
 /* Includes ------------------------------------------------------------------*/
-#include "hal_types.h" 
-#include "compiler.h"
+#include "ble_hal_types.h" 
+#include "ble_compiler.h"
 
 /* Exported types ------------------------------------------------------------*/
 typedef struct{
@@ -201,4 +201,4 @@
 
 #endif /*__BLUENRG_UTILS_H */
 
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/clock.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/clock.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,59 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : clock.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.1
-* Date               : 19-July-2012
-* Description        : Header file for clock library, that gives a simple time
-*                       reference to the BLE Stack.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#ifndef __CLOCK_H__
-#define __CLOCK_H__
-
-#include <hal_types.h>
-
-/**
- * Number of clocks in one seconds.
- * This value must be set by each platorm implementation, basing on its needs.
- */
-extern const uint32_t CLOCK_SECOND;
-
-typedef uint32_t tClockTime;
-
-/**
- * This function initializes the clock library and should be called before
- * any other Stack functions.
- *
- */
-void Clock_Init(void);
-
-/**
- * This function returns the current system clock time. it is used by
- * the host stack and has to be implemented.
- *
- * @return The current clock time, measured in system ticks.
- */
-tClockTime Clock_Time(void);
-
-/**
- * This function waits for a given number of milliseconds.
- *
- */
-void Clock_Wait(uint32_t i);
-
-/**
- * It suspends system clock.
- *
- */
-void Clock_Suspend(void);
-
-
-#endif /* __CLOCK_H__ */
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/compiler.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/compiler.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : compiler.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : Compiler-dependent macros.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#ifdef __ICCARM__
-#define PACKED
-#else
-#ifdef __GNUC__
-#undef __packed
-#define __packed
-#define PACKED __attribute__((packed))
-#else
-#define PACKED
-#define __packed
-#endif
-#endif
-
-/* Change this define to 1 if zero-length arrays are not supported by your compiler. */
-#define VARIABLE_SIZE 1
-
-#endif /* DOXYGEN_SHOULD_SKIP_THIS */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/debug.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/debug.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,73 +0,0 @@
-/**
-  ******************************************************************************
-  * @file    debug.h 
-  * @author  CL
-  * @version V1.0.0
-  * @date    04-July-2014
-  * @brief   This file defines print functions for debug purposes.
-  ******************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
-  *
-  * 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 STMicroelectronics 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.
-  *
-  ******************************************************************************
-  */
-
-/* Define to prevent recursive inclusion -------------------------------------*/
-#ifndef __DEBUG_H
-#define __DEBUG_H
-
-#ifdef __cplusplus
- extern "C" {
-#endif 
-
-/* Includes ------------------------------------------------------------------*/
-#include <string.h>
-
-/* Exported macro ------------------------------------------------------------*/
-//#define DEBUG
-#ifdef DEBUG
-#include <stdio.h>
-#define PRINTF(...) printf(__VA_ARGS__)
-#else
-#define PRINTF(...)
-#endif
-
-/* Print the data travelling over the SPI in the .csv format for the GUI*/
-//#define PRINT_CSV_FORMAT 
-#ifdef PRINT_CSV_FORMAT
-#include <stdio.h>
-#define PRINT_CSV(...) printf(__VA_ARGS__)
-#else
-#define PRINT_CSV(...)
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __DEBUG_H */
-
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/gp_timer.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/gp_timer.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,105 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : gp_timer.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : General purpose timer library.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#ifndef __GP_TIMER_H__
-#define __GP_TIMER_H__
-
-#include "clock.h"
-#include "ble_status.h"
-#ifdef __DMA_LP__
-#include "stm32xx_timerserver.h"
-#endif /* __DMA_LP__ */
-
-/**
- * timer
- *
- * A structure that represents a timer. Use Timer_Set() to set the timer.
- *
- */
-struct timer {
-    
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-    
-  tClockTime start;
-  tClockTime interval;
-  
-#endif
-};
-
-typedef void (* TIMER_HCI_TIMEOUT_NOTIFY_CALLBACK_TYPE)(void);
-
-/**
- * Timer_Set
- *
- * @param[in] t             Pointer to a timer structure
- * @param[in] interval      timeout value
- *
- * This function sets the timeout value of a timer.
- *
- */
-void Timer_Set(struct timer *t, tClockTime interval);
-
-/**
- * Timer_Reset
- * 
- * @param[in] t     Pointer to a timer structure
- * 
- * This function resets the timer with the same interval given
- * with Timer_Set, starting from the time it previously expired.
- * 
- */ 
-void Timer_Reset(struct timer *t);
-
-/**
- * Timer_Restart
- * 
- * @param[in]  t   Pointer to a timer structure
- * 
- * This function resets the timer with the same interval given
- * with Timer_Set, starting from the current time.
- * 
- */ 
-void Timer_Restart(struct timer *t);
-
-/**
- * Timer_Expired
- *
- * @param[in] t    Pointer to a timer structure
- *
- * This function returns TRUE if timer is expired, FALSE otherwise.
- *
- */
-int Timer_Expired(struct timer *t);
-
-/**
- * Timer_Expired
- *
- * @param[in] t    Pointer to a timer structure
- *
- * This function returns the time needed for expiration.
- *
- * @return  Time before timer's expiration.
- */
-tClockTime Timer_Remaining(struct timer *t);
-
-#ifdef __DMA_LP__
-tBleStatus Blue_NRG_HCI_Timer_Start(uint32_t expiryTime,
-		TIMER_HCI_TIMEOUT_NOTIFY_CALLBACK_TYPE timercb,
-					   uint8_t *timerID);
-
-tBleStatus Blue_NRG_HCI_Timer_Stop(uint8_t timerID);
-#endif /* __DMA_LP__ */
-
-#endif /* __GP_TIMER_H__ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/hal.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/hal.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,111 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : hal.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : Header file which defines Hardware abstraction layer APIs
-*                       used by the BLE stack. It defines the set of functions
-*                       which needs to be ported to the target platform.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-#ifndef __HAL_H__
-#define __HAL_H__
-
-/******************************************************************************
- * Includes
- *****************************************************************************/
-#include <hal_types.h>
-#include <ble_status.h>
-
-
-/******************************************************************************
- * Macros
- *****************************************************************************/
-/* Little Endian buffer to host endianess conversion */
-#define LE_TO_HOST_16(ptr)  (uint16_t) ( ((uint16_t) \
-                                           *((uint8_t *)ptr)) | \
-                                          ((uint16_t) \
-                                           *((uint8_t *)ptr + 1) << 8 ) )
-
-#define LE_TO_HOST_32(ptr)   (uint32_t) ( ((uint32_t) \
-                                           *((uint8_t *)ptr)) | \
-                                           ((uint32_t) \
-                                            *((uint8_t *)ptr + 1) << 8)  | \
-                                           ((uint32_t) \
-                                            *((uint8_t *)ptr + 2) << 16) | \
-                                           ((uint32_t) \
-                                            *((uint8_t *)ptr + 3) << 24) )
-
-/* Big Endian buffer to host endianess conversion */
-#define BE_TO_HOST_16(ptr)  (uint16_t) ( ((uint16_t) \
-                                           *((uint8_t *)ptr)) << 8 | \
-                                          ((uint16_t) \
-                                           *((uint8_t *)ptr + 1) ) )
-											
-/* Store Value into a buffer in Little Endian Format */
-#define HOST_TO_LE_16(buf, val)    ( ((buf)[0] =  (uint8_t) (val)    ) , \
-                                   ((buf)[1] =  (uint8_t) (val>>8) ) )
-
-#define HOST_TO_LE_32(buf, val)    ( ((buf)[0] =  (uint8_t) (val)     ) , \
-                                   ((buf)[1] =  (uint8_t) (val>>8)  ) , \
-                                   ((buf)[2] =  (uint8_t) (val>>16) ) , \
-                                   ((buf)[3] =  (uint8_t) (val>>24) ) ) 
-
-
-/* Store Value into a buffer in Big Endian Format */
-#define HOST_TO_BE_16(buf, val)    ( ((buf)[1] =  (uint8_t) (val)    ) , \
-                                   ((buf)[0] =  (uint8_t) (val>>8) ) )
-
-#define DISABLE_INTERRUPTS() __disable_interrupt()
-#define ENABLE_INTERRUPTS() __enable_interrupt()
-#define SAVE_PRIMASK() uint32_t uwPRIMASK_Bit = __get_PRIMASK()
-#define ATOMIC_SECTION_BEGIN() uint32_t uwPRIMASK_Bit = __get_PRIMASK(); \
-                                __disable_interrupt(); \
-/* Must be called in the same or in a lower scope of SUSPEND_INTERRUPTS */
-#define ATOMIC_SECTION_END() __set_PRIMASK(uwPRIMASK_Bit)
- 
-/******************************************************************************
- * Types
- *****************************************************************************/
-
-/******************************************************************************
- * Function Prototypes
- *****************************************************************************/
-
-/**
- * Writes data to a serial interface.
- *
- * @param[in]  data1    1st buffer
- * @param[in]  data2    2nd buffer
- * @param[in]  n_bytes1 number of bytes in 1st buffer
- * @param[in]  n_bytes2 number of bytes in 2nd buffer
- */
-void Hal_Write_Serial(const void* data1, const void* data2, int32_t n_bytes1, int32_t n_bytes2);
-
-/**
- * Enable interrupts from HCI controller.
- */
-void Enable_SPI_IRQ(void);
-
-/**
- * Disable interrupts from BLE controller.
- */
-void Disable_SPI_IRQ(void);
-
-/**
- * Call BTLE callback handler.
- */
-void Call_BTLE_Handler(void);
-
-void Hal_Init_Timer(void);
-uint32_t Hal_Get_Timer_Value(void);
-void Hal_Start_Timer(uint32_t timeout);
-void Hal_Stop_Timer(void);
-
-#endif /* __HAL_H__ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/hal_types.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/hal_types.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,58 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : hal_types.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : This header file defines the basic data types used by the
-*                       BLE stack.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-#ifndef __HAL_TYPES_H__
-#define __HAL_TYPES_H__
-
-#include <stdint.h>
-
-#ifndef NULL
-#define NULL ((void *)0)
-#endif
-
-#ifndef __LITTLE_ENDIAN
-#define __LITTLE_ENDIAN 0
-#define __BIG_ENDIAN    1
-#endif
-
-/* Byte order conversions */
-#if __BYTE_ORDER == __LITTLE_ENDIAN
-#define htobs(d)  (d)
-#define htobl(d)  (d)
-#define btohs(d)  (d)
-#define btohl(d)  (d)
-#elif __BYTE_ORDER == __BIG_ENDIAN
-#define htobs(d)  (d<<8|d>>8)
-#define htobl(d)  (d<<24|((d<<8)&0x00ff0000)|((d>>8)&0x0000ff00)|((d>>24)&0x000000ff))
-#define btohs(d)  (d<<8|d>>8)
-#define btohl(d)  (d<<24|((d<<8)&0x00ff0000)|((d>>8)&0x0000ff00)|((d>>24)&0x000000ff))
-#else
-#error "Unknown byte order"
-#endif
-
-typedef uint8_t BOOL;
-
-#ifndef TRUE 
-#define TRUE (1)
-#endif
-
-#ifndef FALSE
-#define FALSE (0)
-#endif
-
-
-
-#endif /* __HAL_TYPES_H__ */ 
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/hci.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/hci.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,241 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : hci.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : Constants and functions for HCI layer. See Bluetooth Core
-*                      v 4.0, Vol. 2, Part E.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#ifndef __HCI_H_
-#define __HCI_H_
-
-#include "hal_types.h"
-#include "link_layer.h"
-#include <list.h>
-
-#define HCI_READ_PACKET_SIZE                    128 //71
-
-/*** Data types ***/
-
-/* structure used to read received data */
-typedef struct _tHciDataPacket
-{
-  tListNode currentNode;
-  uint8_t dataBuff[HCI_READ_PACKET_SIZE];
-  uint8_t data_len;
-} tHciDataPacket;
-
-typedef enum
-{
-  BUSY,
-  AVAILABLE
-} HCI_CMD_STATUS_t;
-
-/**
- * @defgroup HCI_Error_codes HCI Error codes
- * @{
- */
-#define HCI_UNKNOWN_COMMAND                     0x01
-#define HCI_NO_CONNECTION                       0x02
-#define HCI_HARDWARE_FAILURE                    0x03
-#define HCI_PAGE_TIMEOUT                        0x04
-#define HCI_AUTHENTICATION_FAILURE              0x05
-#define HCI_PIN_OR_KEY_MISSING                  0x06
-#define HCI_MEMORY_FULL                         0x07
-#define HCI_CONNECTION_TIMEOUT                  0x08
-#define HCI_MAX_NUMBER_OF_CONNECTIONS		0x09
-#define HCI_MAX_NUMBER_OF_SCO_CONNECTIONS	0x0a
-#define HCI_ACL_CONNECTION_EXISTS               0x0b
-#define HCI_COMMAND_DISALLOWED                  0x0c
-#define HCI_REJECTED_LIMITED_RESOURCES          0x0d
-#define HCI_REJECTED_SECURITY                   0x0e
-#define HCI_REJECTED_PERSONAL                   0x0f
-#define HCI_HOST_TIMEOUT                        0x10
-#define HCI_UNSUPPORTED_FEATURE                 0x11
-#define HCI_INVALID_PARAMETERS                  0x12
-#define HCI_OE_USER_ENDED_CONNECTION            0x13
-#define HCI_OE_LOW_RESOURCES                    0x14
-#define HCI_OE_POWER_OFF                        0x15
-#define HCI_CONNECTION_TERMINATED               0x16
-#define HCI_REPEATED_ATTEMPTS                   0x17
-#define HCI_PAIRING_NOT_ALLOWED                 0x18
-#define HCI_UNKNOWN_LMP_PDU                     0x19
-#define HCI_UNSUPPORTED_REMOTE_FEATURE          0x1a
-#define HCI_SCO_OFFSET_REJECTED                 0x1b
-#define HCI_SCO_INTERVAL_REJECTED               0x1c
-#define HCI_AIR_MODE_REJECTED                   0x1d
-#define HCI_INVALID_LMP_PARAMETERS              0x1e
-#define HCI_UNSPECIFIED_ERROR                   0x1f
-#define HCI_UNSUPPORTED_LMP_PARAMETER_VALUE     0x20
-#define HCI_ROLE_CHANGE_NOT_ALLOWED             0x21
-#define HCI_LMP_RESPONSE_TIMEOUT                0x22
-#define HCI_LMP_ERROR_TRANSACTION_COLLISION     0x23
-#define HCI_LMP_PDU_NOT_ALLOWED                 0x24
-#define HCI_ENCRYPTION_MODE_NOT_ACCEPTED        0x25
-#define HCI_UNIT_LINK_KEY_USED                  0x26
-#define HCI_QOS_NOT_SUPPORTED                   0x27
-#define HCI_INSTANT_PASSED                      0x28
-#define HCI_PAIRING_NOT_SUPPORTED               0x29
-#define HCI_TRANSACTION_COLLISION               0x2a
-#define HCI_QOS_UNACCEPTABLE_PARAMETER          0x2c
-#define HCI_QOS_REJECTED                        0x2d
-#define HCI_CLASSIFICATION_NOT_SUPPORTED        0x2e
-#define HCI_INSUFFICIENT_SECURITY               0x2f
-#define HCI_PARAMETER_OUT_OF_RANGE              0x30
-#define HCI_ROLE_SWITCH_PENDING                 0x32
-#define HCI_SLOT_VIOLATION                      0x34
-#define HCI_ROLE_SWITCH_FAILED                  0x35
-#define HCI_EIR_TOO_LARGE                       0x36
-#define HCI_SIMPLE_PAIRING_NOT_SUPPORTED        0x37
-#define HCI_HOST_BUSY_PAIRING                   0x38
-#define HCI_CONN_REJ_NO_CH_FOUND                0x39
-#define HCI_CONTROLLER_BUSY                     0x3A
-#define HCI_UNACCEPTABLE_CONN_INTERV            0x3B
-#define HCI_DIRECTED_ADV_TIMEOUT                0x3C
-#define HCI_CONN_TERM_MIC_FAIL                  0x3D
-#define HCI_CONN_FAIL_TO_BE_ESTABL              0x3E
-#define HCI_MAC_CONN_FAILED                     0x3F
-/**
- * @}
- */
-
-
-/*
- * HCI library functions.
- * Each function returns 0 in case of success, otherwise one of the error codes.
- */
-
-int hci_reset(void);
-
-int hci_disconnect(uint16_t handle, uint8_t reason);
-
-int hci_le_set_advertise_enable(uint8_t enable);
-
-int hci_le_set_advertising_parameters(uint16_t min_interval, uint16_t max_interval, uint8_t advtype,
-		uint8_t own_bdaddr_type, uint8_t direct_bdaddr_type, const tBDAddr direct_bdaddr, uint8_t chan_map,
-		uint8_t filter);
-
-int hci_le_set_scan_parameters(uint8_t type, uint16_t interval,
-                               uint16_t window, uint8_t own_bdaddr_type,
-                               uint8_t filter);
-
-int hci_le_set_scan_enable(uint8_t enable, uint8_t filter_dup);
-
-int hci_le_set_advertising_data(uint8_t length, const uint8_t data[]);
-
-int hci_le_set_scan_resp_data(uint8_t length, const uint8_t data[]);
-
-int hci_le_rand(uint8_t random_number[8]);
-
-int hci_le_read_advertising_channel_tx_power(int8_t *tx_power_level);
-
-int hci_acl_data(const uint8_t * data, uint16_t len);
-
-int hci_le_set_random_address(tBDAddr bdaddr);
-
-int hci_read_bd_addr(tBDAddr bdaddr);
-
-int hci_le_read_white_list_size(uint8_t *size);
-
-int hci_le_clear_white_list(void);
-
-int hci_le_add_device_to_white_list(uint8_t bdaddr_type, tBDAddr bdaddr);
-
-int hci_le_remove_device_from_white_list(uint8_t bdaddr_type, tBDAddr bdaddr);
-
-int hci_le_encrypt(uint8_t key[16], uint8_t plaintextData[16], uint8_t encryptedData[16]);
-
-int hci_le_ltk_request_reply(uint8_t key[16]);
-
-int hci_le_ltk_request_neg_reply(void);
-
-int hci_le_read_buffer_size(uint16_t *pkt_len, uint8_t *max_pkt);
-
-int hci_le_create_connection(uint16_t interval,	uint16_t window, uint8_t initiator_filter, uint8_t peer_bdaddr_type,
-                             const tBDAddr peer_bdaddr,	uint8_t	own_bdaddr_type, uint16_t min_interval,	uint16_t max_interval,
-                             uint16_t latency, uint16_t supervision_timeout, uint16_t min_ce_length, uint16_t max_ce_length);
-
-int hci_le_create_connection_cancel(void);
-
-int hci_read_transmit_power_level(uint16_t *conn_handle, uint8_t type, int8_t *tx_level);
-
-int hci_read_rssi(uint16_t *conn_handle, int8_t *rssi);
-
-int hci_le_read_local_supported_features(uint8_t *features);
-
-int hci_le_read_channel_map(uint16_t conn_handle, uint8_t ch_map[5]);
-
-int hci_le_read_supported_states(uint8_t states[8]);
-
-int hci_le_receiver_test(uint8_t frequency);
-
-int hci_le_transmitter_test(uint8_t frequency, uint8_t length, uint8_t payload);
-
-int hci_le_test_end(uint16_t *num_pkts);
-
-int hci_le_read_local_version(uint8_t *hci_version, uint16_t *hci_revision, uint8_t *lmp_pal_version, 
-			      uint16_t *manufacturer_name, uint16_t *lmp_pal_subversion);
-
-/**
- * This function must be used to pass the packet received from the HCI
- * interface to the BLE Stack HCI state machine.
- *
- * @param[in] hciReadPacket    The packet that is received from HCI interface.
- *
- */
-void HCI_Input(tHciDataPacket *hciReadPacket);
-
-/**
- * Initialization function. Must be done before any data can be received from
- * BLE controller.
- */
-void HCI_Init(void);
-
-/**
- * Callback used to pass events to application.
- *
- * @param[in] pckt    The event.
- *
- */
-extern void HCI_Event_CB(void *pckt);
-
-/**
- * Processing function that must be called after an event is received from
- * HCI interface. Must be called outside ISR. It will call HCI_Event_CB if
- * necessary.
-*/
-void HCI_Process(void);
-
-/**
- * @brief Check if queue of HCI event is empty or not.
- * @note This funtion can be used to check if the event queue from BlueNRG is empty. This
- *        is useful when checking if it is safe to go to sleep. 
- * @return TRUE if event queue is empty. FALSE otherwhise.
- */
-BOOL HCI_Queue_Empty(void);
-/**
- * Iterrupt service routine that must be called when the BlueNRG 
- * reports a packet received or an event to the host through the 
- * BlueNRG interrupt line.
- */
-#ifdef __DMA_LP__
-void HCI_Isr(uint8_t *buffer, uint8_t event_payload_len);
-void HCI_Process_Notification_Request(void);
-void HCI_Cmd_Status(HCI_CMD_STATUS_t Hci_Cmd_Status);
-void HCI_Wait_For_Response(void);
-#else
-void HCI_Isr(void);
-#endif /* __DMA_LP__ */
-
-extern tListNode hciReadPktPool;
-extern tListNode hciReadPktRxQueue;
-
-#endif /* __HCI_H_ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/hci_const.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/hci_const.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,563 +0,0 @@
-/******************************************************************************
-*
-*  File Description 
-*  ---------------------
-* This file defines constants and functions for HCI layer.
-* See Bluetooth Core v 4.0, Vol. 2, Part E.
-* 
-*******************************************************************************/
-
-#ifndef __HCI_INTERNAL_H_
-#define __HCI_INTERNAL_H_
-
-#include "compiler.h"
-#include "hal_types.h"
-#include "clock.h"
-#include "link_layer.h"
-#include "hci.h"
-
-#define DEFAULT_TIMEOUT (CLOCK_SECOND/10)
-
-/**
- * Maximum payload of HCI commands that can be sent. Change this value if needed.
- * This value can be up to 255.
- */
-#define HCI_MAX_PAYLOAD_SIZE 128
-
-/* HCI Packet types */
-#define HCI_COMMAND_PKT		0x01
-#define HCI_ACLDATA_PKT		0x02
-#define HCI_SCODATA_PKT		0x03
-#define HCI_EVENT_PKT		0x04
-#define HCI_VENDOR_PKT		0xff
-
-typedef __packed struct _hci_uart_pckt{
-  uint8_t type;
-  uint8_t data[VARIABLE_SIZE];
-} PACKED hci_uart_pckt;
-#define HCI_HDR_SIZE 1
-
-typedef __packed struct _hci_command_hdr{
-  uint16_t opcode;		/* OCF & OGF */
-  uint8_t  plen;
-} PACKED hci_command_hdr;
-#define HCI_COMMAND_HDR_SIZE 	3
-
-typedef __packed struct _hci_event_pckt{
-  uint8_t evt;
-  uint8_t plen;
-  uint8_t data[VARIABLE_SIZE];
-} PACKED hci_event_pckt;
-#define HCI_EVENT_HDR_SIZE 	2
-
-typedef __packed struct _hci_acl_hdr{
-  uint16_t handle;		/* Handle & Flags(PB, BC) */
-  uint16_t dlen;
-} PACKED hci_acl_hdr;
-#define HCI_ACL_HDR_SIZE 	4
-
-/* Link Control */
-#define OGF_LINK_CTL	        0x01
-
-#define OCF_DISCONNECT          0x0006
-typedef __packed struct _disconnect_cp{
-  uint16_t handle;
-  uint8_t  reason;
-} PACKED disconnect_cp;
-#define DISCONNECT_CP_SIZE      3
-
-/* Host Controller and Baseband */
-#define OGF_HOST_CTL            0x03
-
-#define OCF_SET_EVENT_MASK      0x0001
-#define OCF_RESET               0x0003
-
-#define OCF_READ_TRANSMIT_POWER_LEVEL	0x002D
-typedef __packed struct _read_transmit_power_level_cp{
-  uint16_t handle;
-  uint8_t  type;
-} PACKED read_transmit_power_level_cp;
-#define READ_TRANSMIT_POWER_LEVEL_CP_SIZE 3
-typedef __packed struct _read_transmit_power_level_rp{
-  uint8_t  status;
-  uint16_t handle;
-  int8_t   level;
-} PACKED read_transmit_power_level_rp;
-#define READ_TRANSMIT_POWER_LEVEL_RP_SIZE 4
-
-#define OCF_SET_CONTROLLER_TO_HOST_FC   0x0031
-#define OCF_HOST_BUFFER_SIZE            0x0033
-#define OCF_HOST_NUM_COMP_PKTS          0x0035
-
-/* Informational Parameters */
-#define OGF_INFO_PARAM                  0x04
-
-#define OCF_READ_LOCAL_VERSION          0x0001
-typedef __packed struct _read_local_version_rp{
-  uint8_t  status;
-  uint8_t  hci_version;
-  uint16_t hci_revision;
-  uint8_t  lmp_pal_version;
-  uint16_t manufacturer_name;
-  uint16_t lmp_pal_subversion;
-} PACKED read_local_version_rp;
-#define READ_LOCAL_VERSION_RP_SIZE      9
-
-#define OCF_READ_LOCAL_COMMANDS         0x0002
-#define OCF_READ_LOCAL_FEATURES         0x0003
-
-#define OCF_READ_BD_ADDR                0x0009
-typedef __packed struct _read_bd_addr_rp{
-  uint8_t status;
-  tBDAddr bdaddr;
-} PACKED read_bd_addr_rp;
-#define READ_BD_ADDR_RP_SIZE            7
-
-/* Status params */
-#define OGF_STATUS_PARAM                0x05
-
-#define OCF_READ_RSSI                   0x0005
-typedef __packed struct _read_rssi_cp{
-  uint16_t handle;
-} PACKED read_rssi_cp;
-#define READ_RSSI_CP_SIZE               2
-typedef __packed struct _read_rssi_rp{
-  uint8_t  status;
-  uint16_t handle;
-  int8_t   rssi;
-} PACKED read_rssi_rp;
-#define READ_RSSI_RP_SIZE               4
-
-
-/* LE commands */
-#define OGF_LE_CTL                      0x08
-
-#define OCF_LE_SET_EVENT_MASK           0x0001
-typedef __packed struct _le_set_event_mask_cp{
-  uint8_t mask[8];
-} PACKED le_set_event_mask_cp;
-#define LE_SET_EVENT_MASK_CP_SIZE       8
-
-#define OCF_LE_READ_BUFFER_SIZE         0x0002
-typedef __packed struct _le_read_buffer_size_rp{
-  uint8_t  status;
-  uint16_t pkt_len;
-  uint8_t  max_pkt;
-} PACKED le_read_buffer_size_rp;
-#define LE_READ_BUFFER_SIZE_RP_SIZE     4
-
-#define OCF_LE_READ_LOCAL_SUPPORTED_FEATURES	0x0003
-typedef __packed struct _le_read_local_supported_features_rp{
-  uint8_t status;
-  uint8_t features[8];
-} PACKED le_read_local_supported_features_rp;
-#define LE_READ_LOCAL_SUPPORTED_FEATURES_RP_SIZE 9
-
-#define OCF_LE_SET_RANDOM_ADDRESS		0x0005
-typedef __packed struct _le_set_random_address_cp{
-  tBDAddr bdaddr;
-} PACKED le_set_random_address_cp;
-#define LE_SET_RANDOM_ADDRESS_CP_SIZE 6
-
-#define OCF_LE_SET_ADV_PARAMETERS	0x0006
-typedef __packed struct _le_set_adv_parameters_cp{
-  uint16_t min_interval;
-  uint16_t max_interval;
-  uint8_t  advtype;
-  uint8_t  own_bdaddr_type;
-  uint8_t  direct_bdaddr_type;
-  tBDAddr  direct_bdaddr;
-  uint8_t  chan_map;
-  uint8_t  filter;
-} PACKED le_set_adv_parameters_cp;
-#define LE_SET_ADV_PARAMETERS_CP_SIZE 15
-
-#define OCF_LE_READ_ADV_CHANNEL_TX_POWER	0x0007
-typedef __packed struct _le_read_adv_channel_tx_power_rp{
-  uint8_t status;
-  int8_t  level;
-} PACKED le_read_adv_channel_tx_power_rp;
-#define LE_READ_ADV_CHANNEL_TX_POWER_RP_SIZE 2
-
-#define OCF_LE_SET_ADV_DATA		0x0008
-typedef __packed struct _le_set_adv_data_cp{
-  uint8_t length;
-  uint8_t data[31];
-} PACKED le_set_adv_data_cp;
-#define LE_SET_ADV_DATA_CP_SIZE 32
-
-#define OCF_LE_SET_SCAN_RESPONSE_DATA		0x0009
-typedef __packed struct _le_set_scan_response_data_cp{
-  uint8_t length;
-  uint8_t data[31];
-} PACKED le_set_scan_response_data_cp;
-#define LE_SET_SCAN_RESPONSE_DATA_CP_SIZE 32
-
-#define OCF_LE_SET_ADVERTISE_ENABLE		0x000A
-typedef __packed struct _le_set_advertise_enable_cp{
-  uint8_t enable;
-} PACKED le_set_advertise_enable_cp;
-#define LE_SET_ADVERTISE_ENABLE_CP_SIZE 1
-
-#define OCF_LE_SET_SCAN_PARAMETERS		0x000B
-typedef __packed struct _le_set_scan_parameters_cp{
-  uint8_t  type;
-  uint16_t interval;
-  uint16_t window;
-  uint8_t  own_bdaddr_type;
-  uint8_t  filter;
-} PACKED le_set_scan_parameters_cp;
-#define LE_SET_SCAN_PARAMETERS_CP_SIZE 7
-
-#define OCF_LE_SET_SCAN_ENABLE			0x000C
-typedef __packed struct _le_set_scan_enable_cp{
-  uint8_t enable;
-  uint8_t filter_dup;
-} PACKED le_set_scan_enable_cp;
-#define LE_SET_SCAN_ENABLE_CP_SIZE 2
-
-#define OCF_LE_CREATE_CONN			0x000D
-typedef __packed struct _le_create_connection_cp{
-  uint16_t interval;
-  uint16_t window;
-  uint8_t  initiator_filter;
-  uint8_t  peer_bdaddr_type;
-  tBDAddr  peer_bdaddr;
-  uint8_t  own_bdaddr_type;
-  uint16_t min_interval;
-  uint16_t max_interval;
-  uint16_t latency;
-  uint16_t supervision_timeout;
-  uint16_t min_ce_length;
-  uint16_t max_ce_length;
-} PACKED le_create_connection_cp;
-#define LE_CREATE_CONN_CP_SIZE 25
-
-#define OCF_LE_CREATE_CONN_CANCEL		0x000E
-
-#define OCF_LE_READ_WHITE_LIST_SIZE		0x000F
-typedef __packed struct _le_read_white_list_size_rp{
-  uint8_t status;
-  uint8_t size;
-} PACKED le_read_white_list_size_rp;
-#define LE_READ_WHITE_LIST_SIZE_RP_SIZE 2
-
-#define OCF_LE_CLEAR_WHITE_LIST			0x0010
-
-#define OCF_LE_ADD_DEVICE_TO_WHITE_LIST		0x0011
-typedef __packed struct _le_add_device_to_white_list_cp{
-  uint8_t bdaddr_type;
-  tBDAddr bdaddr;
-} PACKED le_add_device_to_white_list_cp;
-#define LE_ADD_DEVICE_TO_WHITE_LIST_CP_SIZE 7
-
-#define OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST	0x0012
-typedef __packed struct _le_remove_device_from_white_list_cp{
-  uint8_t bdaddr_type;
-  tBDAddr bdaddr;
-} PACKED le_remove_device_from_white_list_cp;
-#define LE_REMOVE_DEVICE_FROM_WHITE_LIST_CP_SIZE 7
-
-#define OCF_LE_CONN_UPDATE			0x0013
-typedef __packed struct _le_connection_update_cp{
-  uint16_t handle;
-  uint16_t min_interval;
-  uint16_t max_interval;
-  uint16_t latency;
-  uint16_t supervision_timeout;
-  uint16_t min_ce_length;
-  uint16_t max_ce_length;
-} PACKED le_connection_update_cp;
-#define LE_CONN_UPDATE_CP_SIZE 14
-
-#define OCF_LE_SET_HOST_CHANNEL_CLASSIFICATION	0x0014
-typedef __packed struct _le_set_host_channel_classification_cp{
-  uint8_t map[5];
-} PACKED le_set_host_channel_classification_cp;
-#define LE_SET_HOST_CHANNEL_CLASSIFICATION_CP_SIZE 5
-
-#define OCF_LE_READ_CHANNEL_MAP			0x0015
-typedef __packed struct _le_read_channel_map_cp{
-  uint16_t handle;
-} PACKED le_read_channel_map_cp;
-#define LE_READ_CHANNEL_MAP_CP_SIZE 2
-
-typedef __packed struct _le_read_channel_map_rp{
-  uint8_t  status;
-  uint16_t handle;
-  uint8_t  map[5];
-} PACKED le_read_channel_map_rp;
-#define LE_READ_CHANNEL_MAP_RP_SIZE 8
-
-#define OCF_LE_READ_REMOTE_USED_FEATURES	0x0016
-typedef __packed struct _le_read_remote_used_features_cp{
-  uint16_t handle;
-} PACKED le_read_remote_used_features_cp;
-#define LE_READ_REMOTE_USED_FEATURES_CP_SIZE 2
-
-#define OCF_LE_ENCRYPT				0x0017
-typedef __packed struct _le_encrypt_cp{
-  uint8_t key[16];
-  uint8_t plaintext[16];
-} PACKED le_encrypt_cp;
-#define LE_ENCRYPT_CP_SIZE 32
-
-typedef __packed struct _le_encrypt_rp{
-  uint8_t status;
-  uint8_t encdata[16];
-} PACKED le_encrypt_rp;
-#define LE_ENCRYPT_RP_SIZE 17
-
-#define OCF_LE_RAND				0x0018
-typedef __packed struct _le_rand_rp{
-  uint8_t status;
-  uint8_t random[8];
-} PACKED le_rand_rp;
-#define LE_RAND_RP_SIZE 9
-
-#define OCF_LE_START_ENCRYPTION			0x0019
-typedef __packed struct _le_start_encryption_cp{
-  uint16_t handle;
-  uint8_t  random[8];
-  uint16_t diversifier;
-  uint8_t  key[16];
-} PACKED le_start_encryption_cp;
-#define LE_START_ENCRYPTION_CP_SIZE 28
-
-#define OCF_LE_LTK_REPLY			0x001A
-typedef __packed struct _le_ltk_reply_cp{
-  uint16_t handle;
-  uint8_t  key[16];
-} PACKED le_ltk_reply_cp;
-#define LE_LTK_REPLY_CP_SIZE 18
-
-typedef __packed struct _le_ltk_reply_rp{
-  uint8_t  status;
-  uint16_t handle;
-} PACKED le_ltk_reply_rp;
-#define LE_LTK_REPLY_RP_SIZE 3
-
-#define OCF_LE_LTK_NEG_REPLY			0x001B
-typedef __packed struct _le_ltk_neg_reply_cp{
-  uint16_t handle;
-} PACKED le_ltk_neg_reply_cp;
-#define LE_LTK_NEG_REPLY_CP_SIZE 2
-
-typedef __packed struct _le_ltk_neg_reply_rp{
-  uint8_t  status;
-  uint16_t handle;
-} PACKED le_ltk_neg_reply_rp;
-#define LE_LTK_NEG_REPLY_RP_SIZE 3
-
-#define OCF_LE_READ_SUPPORTED_STATES		0x001C
-typedef __packed struct _le_read_supported_states_rp{
-  uint8_t status;
-  uint8_t states[8];
-} PACKED le_read_supported_states_rp;
-#define LE_READ_SUPPORTED_STATES_RP_SIZE 9
-
-#define OCF_LE_RECEIVER_TEST			0x001D
-typedef __packed struct _le_receiver_test_cp{
-  uint8_t frequency;
-} PACKED le_receiver_test_cp;
-#define LE_RECEIVER_TEST_CP_SIZE 1
-
-#define OCF_LE_TRANSMITTER_TEST			0x001E
-typedef __packed struct _le_transmitter_test_cp{
-  uint8_t frequency;
-  uint8_t length;
-  uint8_t payload;
-} PACKED le_transmitter_test_cp;
-#define LE_TRANSMITTER_TEST_CP_SIZE 3
-
-#define OCF_LE_TEST_END				0x001F
-typedef __packed struct _le_test_end_rp{
-  uint8_t  status;
-  uint16_t num_pkts;
-} PACKED le_test_end_rp;
-#define LE_TEST_END_RP_SIZE 3
-
-/* Vendor specific commands */
-#define OGF_VENDOR_CMD		0x3f
-
-
-/*------------- Events -------------*/
-#define EVT_CONN_COMPLETE		0x03
-typedef __packed struct _evt_conn_complete{
-  uint8_t  status;
-  uint16_t handle;
-  tBDAddr  bdaddr;
-  uint8_t  link_type;
-  uint8_t  encr_mode;
-} PACKED evt_conn_complete;
-#define EVT_CONN_COMPLETE_SIZE 13
-
-#define EVT_DISCONN_COMPLETE		0x05
-typedef __packed struct _evt_disconn_complete{
-  uint8_t  status;
-  uint16_t handle;
-  uint8_t  reason;
-} PACKED evt_disconn_complete;
-#define EVT_DISCONN_COMPLETE_SIZE 4
-
-#define EVT_ENCRYPT_CHANGE		0x08
-typedef __packed struct _evt_encrypt_change{
-  uint8_t  status;
-  uint16_t handle;
-  uint8_t  encrypt;
-} PACKED evt_encrypt_change;
-#define EVT_ENCRYPT_CHANGE_SIZE 5
-
-#define EVT_READ_REMOTE_VERSION_COMPLETE	0x0C
-
-#define EVT_CMD_COMPLETE 		0x0E
-typedef __packed struct _evt_cmd_complete{
-  uint8_t  ncmd;
-  uint16_t opcode;
-} PACKED evt_cmd_complete;
-#define EVT_CMD_COMPLETE_SIZE 3
-
-#define EVT_CMD_STATUS 			0x0F
-typedef __packed struct _evt_cmd_status{
-  uint8_t  status;
-  uint8_t  ncmd;
-  uint16_t opcode;
-} PACKED evt_cmd_status;
-#define EVT_CMD_STATUS_SIZE 4
-
-#define EVT_HARDWARE_ERROR		0x10
-typedef __packed struct _evt_hardware_error{
-  uint8_t code;
-} PACKED evt_hardware_error;
-#define EVT_HARDWARE_ERROR_SIZE 1
-
-#define EVT_NUM_COMP_PKTS		0x13
-typedef __packed struct _evt_num_comp_pkts{
-  uint8_t num_hndl;
-  /* variable length part */
-} PACKED evt_num_comp_pkts;
-#define EVT_NUM_COMP_PKTS_SIZE 1
-
-/* variable length part of evt_num_comp_pkts. */
-typedef __packed struct _evt_num_comp_pkts_param{
-  uint16_t hndl;
-  uint16_t num_comp_pkts;
-} PACKED evt_num_comp_pkts_param;
-#define EVT_NUM_COMP_PKTS_PARAM_SIZE 1
-
-#define EVT_DATA_BUFFER_OVERFLOW		0x1A
-typedef __packed struct _evt_data_buffer_overflow{
-  uint8_t link_type;
-} PACKED evt_data_buffer_overflow;
-#define EVT_DATA_BUFFER_OVERFLOW_SIZE 1
-
-#define EVT_ENCRYPTION_KEY_REFRESH_COMPLETE	0x30
-typedef __packed struct _evt_encryption_key_refresh_complete{
-  uint8_t  status;
-  uint16_t handle;
-} PACKED evt_encryption_key_refresh_complete;
-#define EVT_ENCRYPTION_KEY_REFRESH_COMPLETE_SIZE 3
-
-#define EVT_LE_META_EVENT	0x3E
-typedef __packed struct _evt_le_meta_event{
-  uint8_t subevent;
-  uint8_t data[VARIABLE_SIZE];
-} PACKED evt_le_meta_event;
-#define EVT_LE_META_EVENT_SIZE 1
-
-#define EVT_LE_CONN_COMPLETE	0x01
-typedef __packed struct _evt_le_connection_complete{
-  uint8_t  status;
-  uint16_t handle;
-  uint8_t  role;
-  uint8_t  peer_bdaddr_type;
-  tBDAddr  peer_bdaddr;
-  uint16_t interval;
-  uint16_t latency;
-  uint16_t supervision_timeout;
-  uint8_t  master_clock_accuracy;
-} PACKED evt_le_connection_complete;
-#define EVT_LE_CONN_COMPLETE_SIZE 18
-
-#define EVT_LE_ADVERTISING_REPORT	0x02
-typedef __packed struct _le_advertising_info{
-  uint8_t evt_type;
-  uint8_t bdaddr_type;
-  tBDAddr bdaddr;
-  uint8_t data_length;
-  uint8_t data_RSSI[VARIABLE_SIZE]; // RSSI is last octect (signed integer).
-} PACKED le_advertising_info;
-#define LE_ADVERTISING_INFO_SIZE 9
-
-#define EVT_LE_CONN_UPDATE_COMPLETE	0x03
-typedef __packed struct _evt_le_connection_update_complete{
-  uint8_t  status;
-  uint16_t handle;
-  uint16_t interval;
-  uint16_t latency;
-  uint16_t supervision_timeout;
-} PACKED evt_le_connection_update_complete;
-#define EVT_LE_CONN_UPDATE_COMPLETE_SIZE 9
-
-#define EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE	0x04
-typedef __packed struct _evt_le_read_remote_used_features_complete{
-  uint8_t  status;
-  uint16_t handle;
-  uint8_t  features[8];
-} PACKED evt_le_read_remote_used_features_complete;
-#define EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE_SIZE 11
-
-#define EVT_LE_LTK_REQUEST	0x05
-typedef __packed struct _evt_le_long_term_key_request{
-  uint16_t handle;
-  uint8_t  random[8];
-  uint16_t ediv;
-} PACKED evt_le_long_term_key_request;
-#define EVT_LE_LTK_REQUEST_SIZE 12
-
-/**
-* The event code in the @ref hci_event_pckt structure. If event code is EVT_VENDOR,
-* application can use @ref evt_blue_aci structure to parse the packet.
-*/
-#define EVT_VENDOR	0xFF
-
-
-/* Command opcode pack/unpack */
-#define cmd_opcode_pack(ogf, ocf)	(uint16_t)((ocf & 0x03ff)|(ogf << 10))
-#define cmd_opcode_ogf(op)		(op >> 10)
-#define cmd_opcode_ocf(op)		(op & 0x03ff)
-
-
-struct hci_request {
-  uint16_t ogf;
-  uint16_t ocf;
-  int      event;
-  void     *cparam;
-  int      clen;
-  void     *rparam;
-  int      rlen;
-};
-
-void hci_send_cmd(uint16_t ogf, uint16_t ocf, uint8_t plen, void *param);
-
-typedef enum {
-  WAITING_TYPE,
-  WAITING_OPCODE1,
-  WAITING_OPCODE2,
-  WAITING_EVENT_CODE,
-  WAITING_HANDLE,
-  WAITING_HANDLE_FLAG,
-  WAITING_PARAM_LEN,
-  WAITING_DATA_LEN1,
-  WAITING_DATA_LEN2,
-  WAITING_PAYLOAD
-}hci_state;
-
-typedef void (*hci_packet_complete_callback)(void *pckt, uint16_t len);
-
-/* HCI library functions. */
-void hci_init(void);
-
-int hci_send_req(struct hci_request *r, BOOL async);
-
-#endif /* __HCI_INTERNAL_H_ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/link_layer.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/link_layer.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,161 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : link_layer.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : Header file for BlueNRG's link layer. It contains
-*                      definition of functions for link layer, most of which are
-*                      mapped to HCI commands.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#ifndef _LINK_LAYER_H
-#define _LINK_LAYER_H
-
-#include <ble_status.h>
-
-/**
- *@addtogroup GAP GAP
- *@brief API for GAP layer.
- *@{
- */
-
-/**
- *@name Advertising filter 
- *Advertising policy for filtering (white list related) 
- *@{
- */
-#define NO_WHITE_LIST_USE           (0x00)  /**< Process scan and connection requests from all devices (i.e., the White List is not in use) */
-#define WHITE_LIST_FOR_ONLY_SCAN    (0x01)  /**< Process connection requests from all devices and only scan requests from devices that are in the White List */
-#define WHITE_LIST_FOR_ONLY_CONN    (0x02)  /**< Process scan requests from all devices and only connection requests from devices that are in the White List */
-#define WHITE_LIST_FOR_ALL          (0x03)  /**< Process scan and connection requests only from devices in the White List. */
-/**
- * @}
- */
-
-
-/**
- * Bluetooth 48 bit address (in little-endian order).
- */
-typedef	uint8_t	tBDAddr[6];
-
-
-/**
- *@name Bluetooth address types
- * Bluetooth address types
- *@{
- */
-#define PUBLIC_ADDR                 (0)
-#define RANDOM_ADDR                 (1)
-#define STATIC_RANDOM_ADDR          (1)
-#define RESOLVABLE_PRIVATE_ADDR     (2)
-#define NON_RESOLVABLE_PRIVATE_ADDR (3)
-/**
- * @}
- */
-   
-/**
- *@name Directed advertising types
- * Type of advertising during directed advertising
- *@{
- */
-#define HIGH_DUTY_CYCLE_DIRECTED_ADV   (1)
-#define LOW_DUTY_CYCLE_DIRECTED_ADV    (4)
-/**
- * @}
- */
-
-/** 
- * @name Advertising type
- * @{
- */
-
-/**
- * undirected scannable and connectable 
- */ 
-#define ADV_IND         (0x00)
-
-/** 
- * directed non scannable
- */
-#define ADV_DIRECT_IND  (0x01)
-
-/**
- * scannable non connectable
- */
-#define ADV_SCAN_IND    (0x02)
-
-/**
- * non-connectable and no scan response (used for passive scan)
- */
-#define ADV_NONCONN_IND (0x03)
-
-/**
- * scan response 
- */
-#define SCAN_RSP        (0x04)
-
-/**
- * @}
- */
-
-/* 0X05-0XFF RESERVED */
-
-/**
- * @name Advertising ranges
- * @{
- */
-
-/** 
- * lowest allowed interval value for connectable types(20ms)..multiple of 625us
- */
-#define ADV_INTERVAL_LOWEST_CONN    (0X0020)
-
-/** 
- * highest allowed interval value (10.24s)..multiple of 625us.
- */
-#define ADV_INTERVAL_HIGHEST        (0X4000)
-
-/** 
- * lowest allowed interval value for non connectable types
- * (100ms)..multiple of 625us.
- */
-#define ADV_INTERVAL_LOWEST_NONCONN (0X00a0)
-
-/**
- * @}
- */
-
-/**
- * @name Advertising channels
- * @{
- */
-#define ADV_CH_37 0x01
-#define ADV_CH_38 0x02
-#define ADV_CH_39 0x04
-/**
- * @}
- */
-
-/**
- * @name Scan_types Scan types
- * @{
- */
-#define PASSIVE_SCAN    0
-#define ACTIVE_SCAN     1
-/**
- * @}
- */
-
-/**
- * @}
- */
-
-
-#endif /* _LINK_LAYER_H */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/list.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/list.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,47 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : list.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : Header file for linked list library.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-#ifndef _LIST_H_
-#define _LIST_H_
-
-typedef struct _tListNode {
-	struct _tListNode * next;
-	struct _tListNode * prev;
-}tListNode, *pListNode;
-
-void list_init_head (tListNode * listHead);
-
-uint8_t list_is_empty (tListNode * listHead);
-
-void list_insert_head (tListNode * listHead, tListNode * node);
-
-void list_insert_tail (tListNode * listHead, tListNode * node);
-
-void list_remove_node (tListNode * node);
-
-void list_remove_head (tListNode * listHead, tListNode ** node );
-
-void list_remove_tail (tListNode * listHead, tListNode ** node );
-
-void list_insert_node_after (tListNode * node, tListNode * ref_node);
-
-void list_insert_node_before (tListNode * node, tListNode * ref_node);
-
-int list_get_size (tListNode * listHead);
-
-void list_get_next_node (tListNode * ref_node, tListNode ** node);
-
-void list_get_prev_node (tListNode * ref_node, tListNode ** node);
-
-#endif /* _LIST_H_ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/osal.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/osal.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,81 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : osal.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : This header file defines the OS abstraction layer used by
-*                      the BLE stack. OSAL defines the set of functions
-*                      which needs to be ported to target operating system and
-*                      target platform.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#ifndef __OSAL_H__
-#define __OSAL_H__
-
-/******************************************************************************
- * Includes
- *****************************************************************************/
-#include <hal_types.h>
-#ifdef __ICCARM__
-#include <intrinsics.h>
-#endif
-
-/******************************************************************************
- * Macros
- *****************************************************************************/
-
-
-/******************************************************************************
- * Function Prototypes
- *****************************************************************************/
-
-/**
- * This function copies size number of bytes from a 
- * memory location pointed by src to a destination 
- * memory location pointed by dest
- * 
- * @param[in] dest Destination address
- * @param[in] src  Source address
- * @param[in] size size in the bytes  
- * 
- * @return  Address of the destination
- */
- 
-extern void* Osal_MemCpy(void *dest,const void *src, unsigned int size);
-
-
-/**
- * This function sets first number of bytes, specified
- * by size, to the destination memory pointed by ptr 
- * to the specified value
- * 
- * @param[in] ptr    Destination address
- * @param[in] value  Value to be set
- * @param[in] size   Size in the bytes  
- * 
- * @return  Address of the destination
- */
- 
-extern void* Osal_MemSet(void *ptr, int value, unsigned int size);
-
-/**
- * Osal_Get_Cur_Time
- * 
- * returns the current time in milliseconds
- */
-/**
- * Returns the number of ticks (1 tick = 1 millisecond)
- * 
- * @return  Time in milliseconds
- */
-uint32_t Osal_Get_Cur_Time(void);
-
-
-#endif /* __OSAL_H__ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/bluenrg-hci/sm.h
--- a/x-nucleo-idb0xa1/bluenrg-hci/sm.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,158 +0,0 @@
-/******************** (C) COPYRIGHT 2012 STMicroelectronics ********************
-* File Name          : sm.h
-* Author             : AMS - HEA&RF BU
-* Version            : V1.0.0
-* Date               : 19-July-2012
-* Description        : Header file for BlueNRG's security manager.
-********************************************************************************
-* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
-* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
-* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
-* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
-* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
-* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
-*******************************************************************************/
-
-#ifndef __SM_H__
-#define __SM_H__
-
-/******************************************************************************
-* Macros
-*****************************************************************************/
-
-/**
- *@addtogroup GAP GAP
- *@brief API for GAP layer.
- *@{
- */
-
-/* IO capabilities */
-/**
- * @anchor IO_capabilities
- * @name IO capabilities
- * @{
- */
-#define IO_CAP_DISPLAY_ONLY				(0x00)
-#define IO_CAP_DISPLAY_YES_NO			(0x01)
-#define IO_CAP_KEYBOARD_ONLY			(0x02)
-#define IO_CAP_NO_INPUT_NO_OUTPUT		(0x03)
-#define IO_CAP_KEYBOARD_DISPLAY			(0x04)
-/**
- * @}
- */
-
-/**
- * @anchor Auth_req
- * @name Authentication requirements
- * @{
- */
-#define BONDING				            (0x01)
-#define NO_BONDING				        (0x00)
-/**
- * @}
- */
-
-/**
- * @anchor MITM_req
- * @name MITM protection requirements
- * @{
- */
-#define MITM_PROTECTION_NOT_REQUIRED	(0x00)
-#define MITM_PROTECTION_REQUIRED        (0x01)
-/**
- * @}
- */
-
-/**
- * @anchor OOB_Data
- * @name Out-Of-Band data
- * @{
- */
-#define OOB_AUTH_DATA_ABSENT		    (0x00)
-#define OOB_AUTH_DATA_PRESENT      		(0x01)
-/**
- * @}
- */
-
-/**
- * @anchor Author_req
- * @name Authorization requirements
- * @{
- */
-#define AUTHORIZATION_NOT_REQUIRED	     (0x00)
-#define AUTHORIZATION_REQUIRED			 (0x01)
-/**
- * @}
- */
-
-/**
- * @anchor Conn_authorization
- * @name Connection authorization
- * @{
- */
-#define CONNECTION_AUTHORIZED			(0x01)
-#define CONNECTION_REJECTED				(0x02)
-/**
- * @}
- */
-
-/**
- * @anchor Use_fixed_pin
- * @name Use fixed pin
- * @{
- */
-#define USE_FIXED_PIN_FOR_PAIRING		(0x0)
-#define DONOT_USE_FIXED_PIN_FOR_PAIRING	(0x01)
-/**
- * @}
- */
-
-/**
- * @anchor link_security_status
- * @name Link security status
- * @{
- */
-#define SM_LINK_AUTHENTICATED               (0x01)
-#define SM_LINK_AUTHORIZED                  (0x02)
-#define SM_LINK_ENCRYPTED                   (0x04)
-/**
- * @}
- */
-
-/**
- * @anchor SMP_pairing_failed_codes
- * @name SMP pairing failed reason codes
- * @{
- */
-#define PASSKEY_ENTRY_FAILED			(0x01)
-#define OOB_NOT_AVAILABLE				(0x02)
-#define AUTH_REQ_CANNOT_BE_MET			(0x03)
-#define CONFIRM_VALUE_FAILED			(0x04)
-#define PAIRING_NOT_SUPPORTED			(0x05)
-#define INSUFF_ENCRYPTION_KEY_SIZE		(0x06)
-#define CMD_NOT_SUPPORTED				(0x07)
-#define UNSPECIFIED_REASON				(0x08)
-#define VERY_EARLY_NEXT_ATTEMPT			(0x09)
-#define SM_INVALID_PARAMS				(0x0A)
-/**
- * @}
- */
-
-/**
- * @anchor pairing_failed_codes
- * @name Pairing failed error codes
- * Error codes in @ref EVT_BLUE_GAP_PAIRING_CMPLT event
- * @{
- */
-#define SM_PAIRING_SUCCESS              (0x00)
-#define SM_PAIRING_TIMEOUT              (0x01)
-#define SM_PAIRING_FAILED               (0x02)
-/**
- * @}
- */
-
-/**
- * @}
- */
-
-#endif /* __SM_H__ */
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/platform/btle.h
--- a/x-nucleo-idb0xa1/platform/btle.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/platform/btle.h	Thu Sep 15 16:59:44 2016 +0100
@@ -25,20 +25,21 @@
 
 #include <stdio.h>
 #include <string.h>
-	
-#include "hci.h"
-#include "bluenrg_aci.h"	
-#include "hci_const.h"	
+
+#include "ble_hci.h"
+#include "bluenrg_aci.h"
+#include "ble_hci_const.h"
 #include "bluenrg_hal_aci.h"
-#include "stm32_bluenrg_ble.h"	
+#include "stm32_bluenrg_ble.h"
 #include "bluenrg_gap.h"
 #include "bluenrg_gatt_server.h"
 
 extern uint16_t g_gap_service_handle;
 extern uint16_t g_appearance_char_handle;
 extern uint16_t g_device_name_char_handle;
-	
-void btleInit(bool isSetAddress, uint8_t role);
+extern uint16_t g_preferred_connection_parameters_char_handle;
+
+void btleInit(void);
 void SPI_Poll(void);
 void User_Process(void);
 void setConnectable(void);
@@ -49,13 +50,12 @@
                               uint16_t scan_window,
                               uint8_t own_address_type);
 
-#ifdef AST_FOR_MBED_OS
+
 extern int btle_handler_pending;
 extern void btle_handler(void);
-#endif
 
 #ifdef __cplusplus
 }
 #endif
 
-#endif
+#endif
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/platform/stm32_bluenrg_ble.h
--- a/x-nucleo-idb0xa1/platform/stm32_bluenrg_ble.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/platform/stm32_bluenrg_ble.h	Thu Sep 15 16:59:44 2016 +0100
@@ -45,8 +45,8 @@
 	 
 
 #include <stdint.h>
-#include "gp_timer.h"
-#include "hal.h"
+#include "ble_gp_timer.h"
+#include "ble_hal.h"
 
 /** @addtogroup BSP
  *  @{
@@ -101,4 +101,3 @@
 #endif /* __STM32_BLUENRG_BLE_H */
     
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
-
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/utils/Payload.h
--- a/x-nucleo-idb0xa1/utils/Payload.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,191 +0,0 @@
-/* mbed Microcontroller Library
-* Copyright (c) 2006-2013 ARM Limited
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-*     http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#include "mbed.h"
-#include "debug.h"
-
-#ifndef __PAYLOAD_H__
-#define __PAYLOAD_H__
-
-class UnitPayload
-{
-public:
-    uint8_t length;
-    uint8_t id;    
-    uint8_t *data;
-    uint8_t *idptr;    
-
-    
-    
-    void set_length(uint8_t l) {
-        length=l;   
-    }
-
-    void set_id(uint8_t i) {
-        id=i;
-    }
-
-    void set_data(uint8_t* data1) {
-        for(int j=0;j<length;j++)
-        {
-            data[j]=data1[j];
-        }   
-    }
-
-    uint8_t get_length()  {
-        return length;      
-    }
-
-    uint8_t get_id()  {
-        return id;      
-    }
-
-    uint8_t* get_data() {
-        return data;        
-    }
-
-};
-
-class Payload {
-    UnitPayload *payload;
-    int stringLength;
-    int payloadUnitCount;
-    
-public:
-    Payload(const uint8_t *tokenString, uint8_t string_ength);
-    Payload();
-    ~Payload();
-    uint8_t getPayloadUnitCount();
-    
-    uint8_t getIDAtIndex(int index);  
-    uint8_t getLengthAtIndex(int index);   
-    uint8_t* getDataAtIndex(int index);    
-    int8_t getInt8AtIndex(int index);  
-    uint16_t getUint16AtIndex(int index); 
-    uint8_t* getSerializedAdDataAtIndex(int index);         
-};
-
-
-class PayloadUnit {
-private:        
-    uint8_t* lenPtr;
-    uint8_t* adTypePtr;
-    uint8_t* dataPtr;
-    
-public:
-    PayloadUnit() {
-        lenPtr = NULL;
-        adTypePtr = NULL;
-        dataPtr = NULL;
-    }
-    
-    PayloadUnit(uint8_t *len, uint8_t *adType, uint8_t* data) {
-        lenPtr = len;
-        adTypePtr = adType;
-        dataPtr = data;
-    }    
-    
-    void setLenPtr(uint8_t *len)   {
-        lenPtr = len;
-    } 
-    
-    void setAdTypePtr(uint8_t *adType)   {
-        adTypePtr = adType;
-    }  
-    
-    void setDataPtr(uint8_t *data)   {
-        dataPtr = data;
-    }      
-    
-    uint8_t* getLenPtr()   {
-        return lenPtr;
-    } 
-    
-    uint8_t* getAdTypePtr()   {
-        return adTypePtr;
-    }  
-    
-    uint8_t* getDataPtr()   {
-        return dataPtr;
-    }   
-    
-    void printDataAsHex()   {
-        int i = 0;
-        PRINTF("AdData=");
-        for(i=0; i<*lenPtr-1; i++) {
-            PRINTF("0x%x ", dataPtr[i]);
-        }
-        PRINTF("\n");
-    }     
-    
-    void printDataAsString()   {
-        int i = 0;
-        PRINTF("AdData=");
-        for(i=0; i<*lenPtr-1; i++) {
-            PRINTF("%c", dataPtr[i]);
-        }
-        PRINTF("\n");
-    }                                                   
-    
-};
-
-class PayloadPtr {
-private:
-    PayloadUnit *unit;
-    int payloadUnitCount;
-public:    
-    PayloadPtr(const uint8_t *tokenString, uint8_t string_ength) {
-        // initialize private data members
-        int stringLength = string_ength;
-        payloadUnitCount = 0;
-        
-        int index = 0;
-        while(index!=stringLength) {
-            int len=tokenString[index];
-            index=index+1+len;
-            payloadUnitCount++;               
-        }
-        
-        // allocate memory to unit
-        unit = new PayloadUnit[payloadUnitCount];
-        int i = 0;
-        int nextUnitOffset = 0;
-        
-        while(i<payloadUnitCount) {           
-            unit[i].setLenPtr((uint8_t *)tokenString+nextUnitOffset);
-            unit[i].setAdTypePtr((uint8_t *)tokenString+nextUnitOffset+1);
-            unit[i].setDataPtr((uint8_t *)tokenString+nextUnitOffset+2);
-            
-            nextUnitOffset += *unit[i].getLenPtr()+1;
-            i++;
-
-        }
-    }
-    
-    PayloadUnit getUnitAtIndex(int index) {
-        return unit[index];
-    }
-    
-    int getPayloadUnitCount() { return payloadUnitCount; }
-    
-    ~PayloadPtr() {
-        if(unit) delete[] unit;
-
-        unit = NULL;
-    }
-};    
-
-#endif // __PAYLOAD_H__
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/utils/Utils.h
--- a/x-nucleo-idb0xa1/utils/Utils.h	Mon Jun 27 15:51:20 2016 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,47 +0,0 @@
-/* mbed Microcontroller Library
-* Copyright (c) 2006-2013 ARM Limited
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-*     http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-
-// utility functions
-
-#ifndef __UTIL_H__
-#define __UTIL_H__
-
-#include "ble_status.h"
-#include "hal_types.h"
-#include "mbed.h"
-
-#define STORE_LE_16(buf, val)    ( ((buf)[0] =  (uint8_t) (val)    ) , \
-    ((buf)[1] =  (uint8_t) (val>>8) ) )
-
-#define STORE_LE_32(buf, val)    ( ((buf)[0] =  (uint8_t) (val)     ) , \
-    ((buf)[1] =  (uint8_t) (val>>8)  ) , \
-    ((buf)[2] =  (uint8_t) (val>>16) ) , \
-    ((buf)[3] =  (uint8_t) (val>>24) ) )
-
-#define COPY_UUID_128(uuid_struct, uuid_15, uuid_14, uuid_13, uuid_12, uuid_11, uuid_10, uuid_9, uuid_8, uuid_7, uuid_6, uuid_5, uuid_4, uuid_3, uuid_2, uuid_1, uuid_0) \
-    do {\
-        uuid_struct[0] = uuid_0; uuid_struct[1] = uuid_1; uuid_struct[2] = uuid_2; uuid_struct[3] = uuid_3; \
-        uuid_struct[4] = uuid_4; uuid_struct[5] = uuid_5; uuid_struct[6] = uuid_6; uuid_struct[7] = uuid_7; \
-        uuid_struct[8] = uuid_8; uuid_struct[9] = uuid_9; uuid_struct[10] = uuid_10; uuid_struct[11] = uuid_11; \
-        uuid_struct[12] = uuid_12; uuid_struct[13] = uuid_13; uuid_struct[14] = uuid_14; uuid_struct[15] = uuid_15; \
-    }while(0)
-
-
-tBleStatus getHighPowerAndPALevelValue(int8_t dBMLevel, int8_t& EN_HIGH_POWER, int8_t& PA_LEVEL);
-
-#endif // __UTIL_H__
-		
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/utils/ble_payload.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/utils/ble_payload.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,195 @@
+/* mbed Microcontroller Library
+* Copyright (c) 2006-2013 ARM Limited
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*     http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifdef YOTTA_CFG_MBED_OS
+    #include "mbed-drivers/mbed.h"
+#else
+    #include "mbed.h"
+#endif 
+#include "ble_debug.h"
+
+#ifndef __PAYLOAD_H__
+#define __PAYLOAD_H__
+
+class UnitPayload
+{
+public:
+    uint8_t length;
+    uint8_t id;    
+    uint8_t *data;
+    uint8_t *idptr;    
+
+    
+    
+    void set_length(uint8_t l) {
+        length=l;   
+    }
+
+    void set_id(uint8_t i) {
+        id=i;
+    }
+
+    void set_data(uint8_t* data1) {
+        for(int j=0;j<length;j++)
+        {
+            data[j]=data1[j];
+        }   
+    }
+
+    uint8_t get_length()  {
+        return length;      
+    }
+
+    uint8_t get_id()  {
+        return id;      
+    }
+
+    uint8_t* get_data() {
+        return data;        
+    }
+
+};
+
+class Payload {
+    UnitPayload *payload;
+    int stringLength;
+    int payloadUnitCount;
+    
+public:
+    Payload(const uint8_t *tokenString, uint8_t string_ength);
+    Payload();
+    ~Payload();
+    uint8_t getPayloadUnitCount();
+    
+    uint8_t getIDAtIndex(int index);  
+    uint8_t getLengthAtIndex(int index);   
+    uint8_t* getDataAtIndex(int index);    
+    int8_t getInt8AtIndex(int index);  
+    uint16_t getUint16AtIndex(int index); 
+    uint8_t* getSerializedAdDataAtIndex(int index);         
+};
+
+
+class PayloadUnit {
+private:        
+    uint8_t* lenPtr;
+    uint8_t* adTypePtr;
+    uint8_t* dataPtr;
+    
+public:
+    PayloadUnit() {
+        lenPtr = NULL;
+        adTypePtr = NULL;
+        dataPtr = NULL;
+    }
+    
+    PayloadUnit(uint8_t *len, uint8_t *adType, uint8_t* data) {
+        lenPtr = len;
+        adTypePtr = adType;
+        dataPtr = data;
+    }    
+    
+    void setLenPtr(uint8_t *len)   {
+        lenPtr = len;
+    } 
+    
+    void setAdTypePtr(uint8_t *adType)   {
+        adTypePtr = adType;
+    }  
+    
+    void setDataPtr(uint8_t *data)   {
+        dataPtr = data;
+    }      
+    
+    uint8_t* getLenPtr()   {
+        return lenPtr;
+    } 
+    
+    uint8_t* getAdTypePtr()   {
+        return adTypePtr;
+    }  
+    
+    uint8_t* getDataPtr()   {
+        return dataPtr;
+    }   
+    
+    void printDataAsHex()   {
+        int i = 0;
+        PRINTF("AdData=");
+        for(i=0; i<*lenPtr-1; i++) {
+            PRINTF("0x%x ", dataPtr[i]);
+        }
+        PRINTF("\n");
+    }     
+    
+    void printDataAsString()   {
+        int i = 0;
+        PRINTF("AdData=");
+        for(i=0; i<*lenPtr-1; i++) {
+            PRINTF("%c", dataPtr[i]);
+        }
+        PRINTF("\n");
+    }                                                   
+    
+};
+
+class PayloadPtr {
+private:
+    PayloadUnit *unit;
+    int payloadUnitCount;
+public:    
+    PayloadPtr(const uint8_t *tokenString, uint8_t string_ength) {
+        // initialize private data members
+        int stringLength = string_ength;
+        payloadUnitCount = 0;
+        
+        int index = 0;
+        while(index!=stringLength) {
+            int len=tokenString[index];
+            index=index+1+len;
+            payloadUnitCount++;               
+        }
+        
+        // allocate memory to unit
+        unit = new PayloadUnit[payloadUnitCount];
+        int i = 0;
+        int nextUnitOffset = 0;
+        
+        while(i<payloadUnitCount) {           
+            unit[i].setLenPtr((uint8_t *)tokenString+nextUnitOffset);
+            unit[i].setAdTypePtr((uint8_t *)tokenString+nextUnitOffset+1);
+            unit[i].setDataPtr((uint8_t *)tokenString+nextUnitOffset+2);
+            
+            nextUnitOffset += *unit[i].getLenPtr()+1;
+            i++;
+
+        }
+    }
+    
+    PayloadUnit getUnitAtIndex(int index) {
+        return unit[index];
+    }
+    
+    int getPayloadUnitCount() { return payloadUnitCount; }
+    
+    ~PayloadPtr() {
+        if(unit) delete[] unit;
+
+        unit = NULL;
+    }
+};    
+
+#endif // __PAYLOAD_H__
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/utils/ble_utils.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/x-nucleo-idb0xa1/utils/ble_utils.h	Thu Sep 15 16:59:44 2016 +0100
@@ -0,0 +1,46 @@
+/* mbed Microcontroller Library
+* Copyright (c) 2006-2013 ARM Limited
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*     http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+
+// utility functions
+
+#ifndef __UTIL_H__
+#define __UTIL_H__
+
+#include "ble_status.h"
+#include "ble_hal_types.h"
+
+#define STORE_LE_16(buf, val)    ( ((buf)[0] =  (uint8_t) (val)    ) , \
+    ((buf)[1] =  (uint8_t) (val>>8) ) )
+
+#define STORE_LE_32(buf, val)    ( ((buf)[0] =  (uint8_t) (val)     ) , \
+    ((buf)[1] =  (uint8_t) (val>>8)  ) , \
+    ((buf)[2] =  (uint8_t) (val>>16) ) , \
+    ((buf)[3] =  (uint8_t) (val>>24) ) )
+
+#define COPY_UUID_128(uuid_struct, uuid_15, uuid_14, uuid_13, uuid_12, uuid_11, uuid_10, uuid_9, uuid_8, uuid_7, uuid_6, uuid_5, uuid_4, uuid_3, uuid_2, uuid_1, uuid_0) \
+    do {\
+        uuid_struct[0] = uuid_0; uuid_struct[1] = uuid_1; uuid_struct[2] = uuid_2; uuid_struct[3] = uuid_3; \
+        uuid_struct[4] = uuid_4; uuid_struct[5] = uuid_5; uuid_struct[6] = uuid_6; uuid_struct[7] = uuid_7; \
+        uuid_struct[8] = uuid_8; uuid_struct[9] = uuid_9; uuid_struct[10] = uuid_10; uuid_struct[11] = uuid_11; \
+        uuid_struct[12] = uuid_12; uuid_struct[13] = uuid_13; uuid_struct[14] = uuid_14; uuid_struct[15] = uuid_15; \
+    }while(0)
+
+
+tBleStatus getHighPowerAndPALevelValue(int8_t dBMLevel, int8_t& EN_HIGH_POWER, int8_t& PA_LEVEL);
+
+#endif // __UTIL_H__
+		
\ No newline at end of file
diff -r 0c2cb16a7166 -r 3a7d9f923493 x-nucleo-idb0xa1/x_nucleo_idb0xa1_targets.h
--- a/x-nucleo-idb0xa1/x_nucleo_idb0xa1_targets.h	Mon Jun 27 15:51:20 2016 +0200
+++ b/x-nucleo-idb0xa1/x_nucleo_idb0xa1_targets.h	Thu Sep 15 16:59:44 2016 +0100
@@ -65,7 +65,7 @@
 /* NOTE: Stack Mode 0x04 allows Simultaneous Scanning and Advertisement (SSAdv)
          Define macro 'SSADV' to enable it
 */
-//#define SSADV
+#define SSADV
 #if defined(SSADV)
 #define IDB0XA1_STACK_MODE (0x04)
 #else