Diff: source/BlueNRGGap.cpp

Revision:

242:058b2e731adc

Parent:

215:e8fa3129410a

Parent:

240:f487d8c86ce4

Child:

252:0c2cb16a7166

--- a/source/BlueNRGGap.cpp	Fri Mar 18 12:10:20 2016 +0100
+++ b/source/BlueNRGGap.cpp	Mon Jun 20 14:59:06 2016 +0200
@@ -31,9 +31,7 @@
   * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  */ 
-
-// ANDREA: Changed some types (e.g., tHalUint8 --> uint8_t)
+  */
 
 /** @defgroup BlueNRGGap
  *  @brief BlueNRG BLE_API GAP Adaptation
@@ -49,10 +47,6 @@
 //Local Variables
 //const char *local_name = NULL;
 //uint8_t local_name_length = 0;
-const uint8_t *scan_response_payload = NULL;
-uint8_t scan_rsp_length = 0;
-
-uint32_t advtInterval = BLUENRG_GAP_ADV_INTERVAL_MAX;
 
 /*
  * Utility to process GAP specific events (e.g., Advertising timeout)
@@ -105,14 +99,20 @@
     PRINTF("BlueNRGGap::setAdvertisingData\n\r");
     /* Make sure we don't exceed the advertising payload length */
     if (advData.getPayloadLen() > GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
+        PRINTF("Exceeded the advertising payload length\n\r");
         return BLE_ERROR_BUFFER_OVERFLOW;
     }
 
     /* Make sure we have a payload! */
-    if (advData.getPayloadLen() <= 0) {
-        return BLE_ERROR_PARAM_OUT_OF_RANGE;
-    } else { 
-        PayloadPtr loadPtr(advData.getPayload(), advData.getPayloadLen());        
+    if (advData.getPayloadLen() == 0) {
+        PRINTF("advData.getPayloadLen() == 0\n\r");
+        //return BLE_ERROR_PARAM_OUT_OF_RANGE;
+        local_name_length = 0;
+        servUuidlength = 0;
+        AdvLen = 0;
+    } else {
+        PayloadPtr loadPtr(advData.getPayload(), advData.getPayloadLen());
+
         for(uint8_t index=0; index<loadPtr.getPayloadUnitCount(); index++) {                  
             loadPtr.getUnitAtIndex(index);
 
@@ -157,7 +157,8 @@
                 }
                 
                 for(unsigned i=0; i<buffSize; i++) {
-                    PRINTF("loadPtr.getUnitAtIndex(index).getDataPtr()[%d] = 0x%x\n\r", i, loadPtr.getUnitAtIndex(index).getDataPtr()[i]);
+                    PRINTF("loadPtr.getUnitAtIndex(index).getDataPtr()[%d] = 0x%x\n\r",
+                            i, loadPtr.getUnitAtIndex(index).getDataPtr()[i]);
                 }
 #endif /* DEBUG */
                 break;
@@ -179,13 +180,16 @@
             case GapAdvertisingData::COMPLETE_LOCAL_NAME:                /**< Complete Local Name */
                 {
                 PRINTF("Advertising type: COMPLETE_LOCAL_NAME\n\r");
-                loadPtr.getUnitAtIndex(index).printDataAsString();       
-                local_name_length = *loadPtr.getUnitAtIndex(index).getLenPtr()-1;                        
-                local_name = (uint8_t*)loadPtr.getUnitAtIndex(index).getAdTypePtr();
-                PRINTF("Advertising type: COMPLETE_LOCAL_NAME local_name=%s\n\r", local_name);
-                //COMPLETE_LOCAL_NAME is only advertising device name. Gatt Device Name is not the same.(Must be set right after GAP/GATT init?)
-                
-                PRINTF("device_name length=%d\n\r", local_name_length);                                    
+                loadPtr.getUnitAtIndex(index).printDataAsString();
+                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, local_name_length);
+
                 break;
                 }
             case GapAdvertisingData::TX_POWER_LEVEL:                     /**< TX Power Level (in dBm) */
@@ -219,41 +223,23 @@
                 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]);
+                    PRINTF("Advertising type: SERVICE_DATA loadPtr.getUnitAtIndex(index).getDataPtr()[%d] = 0x%x\n\r",
+                            i, loadPtr.getUnitAtIndex(index).getDataPtr()[i]);
                 }
+#endif
                 AdvLen = buffSize+2; // the total ADV DATA LEN should include two more bytes: the buffer size byte; and the Service Data Type Value byte
                 AdvData[0] = buffSize+1; // the fisrt byte is the data buffer size (type+data)
                 AdvData[1] = AD_TYPE_SERVICE_DATA;
                 memcpy(AdvData+2, loadPtr.getUnitAtIndex(index).getDataPtr(), buffSize);
                 break;
                 }
-            case GapAdvertisingData::APPEARANCE:   
-                {
-                /* 
-                    Tested with GapAdvertisingData::GENERIC_PHONE. 
-                    for other appearances BLE Scanner android app is not behaving properly 
-                    */
-                PRINTF("Advertising type: APPEARANCE\n\r");
-                const char *deviceAppearance = NULL;                    
-                deviceAppearance = (const char*)loadPtr.getUnitAtIndex(index).getDataPtr();  // to be set later when startAdvertising() is called
-                
-#ifdef DEBUG
-                uint8_t Appearance[2] = {0, 0};
-                uint16_t devP = (uint16_t)*deviceAppearance;
-                STORE_LE_16(Appearance, (uint16_t)devP);
-#endif
-                
-                PRINTF("input: deviceAppearance= 0x%x 0x%x..., strlen(deviceAppearance)=%d\n\r", Appearance[1], Appearance[0], (uint8_t)*loadPtr.getUnitAtIndex(index).getLenPtr()-1);         /**< \ref Appearance */
-                
-                aci_gatt_update_char_value(g_gap_service_handle, g_appearance_char_handle, 0, 2, (uint8_t *)deviceAppearance);//not using array Appearance[2]
-                break;
-                }
+
             case GapAdvertisingData::ADVERTISING_INTERVAL:               /**< Advertising Interval */
                 {
                 PRINTF("Advertising type: ADVERTISING_INTERVAL\n\r");
-                advtInterval = (uint16_t)(*loadPtr.getUnitAtIndex(index).getDataPtr());
-                PRINTF("advtInterval=%d\n\r", (int)advtInterval);
+                //uint32_t advInt = (uint32_t)(*loadPtr.getUnitAtIndex(index).getDataPtr());
                 break;
                 }
             case GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA:        /**< Manufacturer Specific Data */                             
@@ -267,23 +253,32 @@
                 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
                 AdvLen = buffSize+2; // the total ADV DATA LEN should include two more bytes: the buffer size byte; and the Manufacturer Specific Data Type Value byte
                 AdvData[0] = buffSize+1; // the fisrt byte is the data buffer size (type+data)
                 AdvData[1] = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
                 memcpy(AdvData+2, loadPtr.getUnitAtIndex(index).getDataPtr(), buffSize);
                 break;
                 }
-                
-            }          
-        }
+            } // end switch
+
+        } //end for
+
         //Set the SCAN_RSP Payload
-        scan_response_payload = scanResponse.getPayload();
-        scan_rsp_length = scanResponse.getPayloadLen();
-        
+        if(scanResponse.getPayloadLen() > 0) {
+          scan_response_payload = scanResponse.getPayload();
+          scan_rsp_length = scanResponse.getPayloadLen();
+        }
+
+        /* 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) {
  
@@ -291,10 +286,15 @@
             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;
-                    default:
-                        return BLE_ERROR_UNSPECIFIED;
+                  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;
                 }
             }
         }
@@ -311,8 +311,7 @@
 
 /*
  * ADV timeout callback
- */   
-// ANDREA: mbedOS
+ */
 #ifdef AST_FOR_MBED_OS
 static void advTimeoutCB(void)
 {
@@ -369,10 +368,11 @@
 ble_error_t BlueNRGGap::startAdvertising(const GapAdvertisingParams &params)
 {
     tBleStatus ret;
+    ble_error_t rc;
 
     /* Make sure we support the advertising type */
     if (params.getAdvertisingType() == GapAdvertisingParams::ADV_CONNECTABLE_DIRECTED) {
-        /* ToDo: This requires a propery security implementation, etc. */
+        /* ToDo: This requires a proper security implementation, etc. */
         return BLE_ERROR_NOT_IMPLEMENTED;
     }
 
@@ -403,56 +403,77 @@
         return BLE_ERROR_PARAM_OUT_OF_RANGE;
     }
 
-    /* set scan response data */
-    ret = hci_le_set_scan_resp_data(scan_rsp_length, scan_response_payload);
-    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;
+    /*
+     * Advertising filter policy setting
+     * FIXME: the Security Manager should be implemented
+     */
+    AdvertisingPolicyMode_t mode = getAdvertisingPolicyMode();
+    if(mode != ADV_POLICY_IGNORE_WHITELIST) {
+        ret = aci_gap_configure_whitelist();
+        if(ret != BLE_STATUS_SUCCESS) {
+          PRINTF("aci_gap_configure_whitelist ret=0x%x\n\r", ret);
+          return BLE_ERROR_OPERATION_NOT_PERMITTED;
         }
     }
 
-    /*aci_gap_set_discoverable(Advertising_Event_Type, Adv_min_intvl, Adv_Max_Intvl, Addr_Type, Adv_Filter_Policy,
-                        Local_Name_Length, local_name, service_uuid_length, service_uuid_list, Slave_conn_intvl_min, Slave_conn_intvl_max);*/
-    /*LINK_LAYER.H DESCRIBES THE ADVERTISING TYPES*/ 
+    uint8_t advFilterPolicy = NO_WHITE_LIST_USE;
+    switch(mode) {
+        case ADV_POLICY_FILTER_SCAN_REQS:
+            advFilterPolicy = WHITE_LIST_FOR_ONLY_SCAN;
+            break;
+        case ADV_POLICY_FILTER_CONN_REQS:
+            advFilterPolicy = WHITE_LIST_FOR_ONLY_CONN;
+            break;
+        case ADV_POLICY_FILTER_ALL_REQS:
+            advFilterPolicy = WHITE_LIST_FOR_ALL;
+            break;
+        default:
+            advFilterPolicy = NO_WHITE_LIST_USE;
+            break;
+    }
+
+    /* Check the ADV type before setting scan response data */
+    if (params.getAdvertisingType() == GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED ||
+        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);
 
-    // Enable the else branch if you want to set default device name
-    char* name = NULL;
-    uint8_t nameLen = 0; 
-    if(local_name!=NULL) {
-        name = (char*)local_name;
-        PRINTF("name=%s\n\r", name); 
-        nameLen = local_name_length;
-    } /*else {
-        char str[] = "ST_BLE_DEV";
-        name = new char[strlen(str)+1];
-        name[0] = AD_TYPE_COMPLETE_LOCAL_NAME;
-        strcpy(name+1, str);
-        nameLen = strlen(name);
-        PRINTF("nameLen=%d\n\r", nameLen);
-        PRINTF("name=%s\n\r", name);      
-    }*/
+        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;
+            }
+        }
+    } else {
+        hci_le_set_scan_resp_data(0, NULL);
+    }
 
-    advtInterval = params.getIntervalInADVUnits(); // set advtInterval in case it is not already set by user application  
-    ret = aci_gap_set_discoverable(params.getAdvertisingType(), // Advertising_Event_Type                                
-        advtInterval,   // Adv_Interval_Min
-        advtInterval,   // Adv_Interval_Max
-        PUBLIC_ADDR, // Address_Type 
-        NO_WHITE_LIST_USE,  // Adv_Filter_Policy
-        nameLen, //local_name_length, // Local_Name_Length
-        (const char*)name, //local_name, // Local_Name
-        servUuidlength,  //Service_Uuid_Length
-        servUuidData, //Service_Uuid_List
-        0, // Slave_Conn_Interval_Min
-        0);  // Slave_Conn_Interval_Max
+    //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
+                                   0,                           // SlaveConnIntervMin
+                                   0);                          // SlaveConnIntervMax
 
     
-    PRINTF("!!!setting discoverable (servUuidlength=0x%x)\n", servUuidlength);
+    PRINTF("!!!setting discoverable (servUuidlength=0x%x)\n\r", servUuidlength);
     if(BLE_STATUS_SUCCESS!=ret) {
-       PRINTF("error occurred while setting discoverable (ret=0x%x)\n", ret);
+       PRINTF("error occurred while setting discoverable (ret=0x%x)\n\r", ret);
        switch (ret) {
          case BLE_STATUS_INVALID_PARAMS:
            return BLE_ERROR_INVALID_PARAM;
@@ -467,16 +488,45 @@
        }
     }
 
+    // Stop Advertising if an error occurs while updating ADV data
+    rc = updateAdvertisingData();
+    if(rc != BLE_ERROR_NONE) {
+      aci_gap_set_non_discoverable();
+      return rc;
+    }
+
+    state.advertising = 1;
+
+    AdvToFlag = false;
+    if(params.getTimeout() != 0) {
+        PRINTF("!!! attaching to!!!\n");
+#ifdef AST_FOR_MBED_OS
+        minar::Scheduler::postCallback(advTimeoutCB).delay(minar::milliseconds(params.getTimeout() * 1000));
+#else
+        advTimeout.attach(advTimeoutCB, params.getTimeout() * 1000);
+#endif
+    }
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t BlueNRGGap::updateAdvertisingData(void)
+{
+    tBleStatus ret;
+
     // Before updating the ADV data, delete COMPLETE_LOCAL_NAME and TX_POWER_LEVEL fields (if present)
-    if(AdvLen>0) {
-      if(name!=NULL) {
-        PRINTF("!!!calling aci_gap_delete_ad_type AD_TYPE_COMPLETE_LOCAL_NAME!!!\n");
+    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;
           }
@@ -486,46 +536,48 @@
       // If ADV Data Type is SERVICE DATA or MANUFACTURER SPECIFIC DATA,
       // we need to delete it to make the needed room in ADV payload
       if(AdvData[1]==AD_TYPE_SERVICE_DATA || AdvData[1]==AD_TYPE_MANUFACTURER_SPECIFIC_DATA) {
-        PRINTF("!!!calling aci_gap_delete_ad_type(AD_TYPE_TX_POWER_LEVEL)!!!\n");
         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", 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
 
-    state.advertising = 1;
+    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);
+    }
 
-    AdvToFlag = false;
-    if(params.getTimeout() != 0) {
-        PRINTF("!!! attaching to!!!\n");
-        // ANDREA: mbedOS
-#ifdef AST_FOR_MBED_OS
-        minar::Scheduler::postCallback(advTimeoutCB).delay(minar::milliseconds(params.getTimeout()));
-#else
-        advTimeout.attach(advTimeoutCB, params.getTimeout());
-#endif
-    }
-    
     return BLE_ERROR_NONE;
+
 }
 
 /**************************************************************************/
@@ -547,7 +599,7 @@
 ble_error_t BlueNRGGap::stopAdvertising(void)
 {
     tBleStatus ret;
-    
+
     if(state.advertising == 1) {
         //Set non-discoverable to stop advertising
         ret = aci_gap_set_non_discoverable();
@@ -590,31 +642,22 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::disconnect(Gap::DisconnectionReason_t reason)
+ble_error_t BlueNRGGap::disconnect(Handle_t connectionHandle, Gap::DisconnectionReason_t reason)
 {
-    /* avoid compiler warnings about unused variables */
-    (void)reason;
+    tBleStatus ret;
 
-    tBleStatus ret;
-    //For Reason codes check BlueTooth HCI Spec
-    
-    if(m_connectionHandle != BLE_CONN_HANDLE_INVALID) {
-        ret = aci_gap_terminate(m_connectionHandle, 0x16);//0x16 Connection Terminated by Local Host. 
+    ret = aci_gap_terminate(connectionHandle, reason);
 
-        if (BLE_STATUS_SUCCESS != ret){
-            PRINTF("Error in GAP termination (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;
-            }
+    if (BLE_STATUS_SUCCESS != ret){
+        PRINTF("Error in GAP termination (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;
         }
-        
-        //PRINTF("Disconnected from localhost!!\n\r") ;
-        m_connectionHandle = BLE_CONN_HANDLE_INVALID;
     }
     
     return BLE_ERROR_NONE;
@@ -639,49 +682,24 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::disconnect(Handle_t connectionHandle, Gap::DisconnectionReason_t reason)
+ble_error_t BlueNRGGap::disconnect(Gap::DisconnectionReason_t reason)
 {
-    /* avoid compiler warnings about unused variables */
-    (void)reason;
-
-    tBleStatus ret;
-    //For Reason codes check BlueTooth HCI Spec
-    
-    if(connectionHandle != BLE_CONN_HANDLE_INVALID) {
-        ret = aci_gap_terminate(connectionHandle, 0x16);//0x16 Connection Terminated by Local Host. 
-
-        if (BLE_STATUS_SUCCESS != ret){
-            PRINTF("Error in GAP termination (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;
-            }
-        }
-        
-        //PRINTF("Disconnected from localhost!!\n\r") ;
-        m_connectionHandle = BLE_CONN_HANDLE_INVALID;
-    }
-    
-    return BLE_ERROR_NONE;
+    return disconnect(m_connectionHandle, reason);
 }
 
 /**************************************************************************/
 /*!
     @brief  Sets the 16-bit connection handle
     
-    @param[in]  con_handle
+    @param[in]  conn_handle
                 Connection Handle which is set in the Gap Instance
                 
     @returns    void
 */
 /**************************************************************************/
-void BlueNRGGap::setConnectionHandle(uint16_t con_handle)
+void BlueNRGGap::setConnectionHandle(uint16_t conn_handle)
 {
-    m_connectionHandle = con_handle;
+    m_connectionHandle = conn_handle;
 }
 
 /**************************************************************************/
@@ -719,21 +737,28 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::setAddress(AddressType_t type, const Address_t address)
+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;
-    //copy address to bdAddr[6]
-    for(int i=0; i<BDADDR_SIZE; i++) {
-        bdaddr[i] = address[i];
-        //PRINTF("i[%d]:0x%x\n\r",i,bdaddr[i]);
+
+    // 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(ret != BLE_STATUS_SUCCESS) {
+            return BLE_ERROR_OPERATION_NOT_PERMITTED;
+        }
+    } else {
+        return BLE_ERROR_OPERATION_NOT_PERMITTED;
     }
     
-    if(!isSetAddress) isSetAddress = true;
-    
     return BLE_ERROR_NONE;
 }
 
@@ -771,14 +796,20 @@
 /**************************************************************************/
 ble_error_t BlueNRGGap::getAddress(AddressType_t *typeP, Address_t address) 
 {
-    *typeP = addr_type;//Gap::ADDR_TYPE_PUBLIC;
-    
-    if(isSetAddress)
-    {
-        for(int i=0; i<BDADDR_SIZE; i++) {
-            address[i] = bdaddr[i];
-            //PRINTF("i[%d]:0x%x\n\r",i,bdaddr[i]);
-        }
+    uint8_t bdaddr[BDADDR_SIZE];
+    uint8_t data_len_out;
+
+    if(typeP != NULL) {
+        *typeP = addr_type;
+    }
+
+    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(address != NULL) {
+        memcpy(address, bdaddr, BDADDR_SIZE);
     }
         
     return BLE_ERROR_NONE;
@@ -802,7 +833,7 @@
     /* avoid compiler warnings about unused variables */
     (void)params;
 
-    return BLE_ERROR_NONE;
+    return BLE_ERROR_NOT_IMPLEMENTED;
 }
 
 
@@ -824,7 +855,7 @@
     /* avoid compiler warnings about unused variables */
     (void)params;
 
-    return BLE_ERROR_NONE;
+    return BLE_ERROR_NOT_IMPLEMENTED;
 }
 
 /**************************************************************************/
@@ -873,17 +904,14 @@
     tBleStatus ret;
     uint8_t nameLen = 0;     
     
-    DeviceName = (uint8_t *)deviceName;
-    //PRINTF("SetDeviceName=%s\n\r", DeviceName);
-    
-    nameLen = strlen((const char*)DeviceName);
-    //PRINTF("DeviceName Size=%d\n\r", nameLen); 
-    
-    ret = aci_gatt_update_char_value(g_gap_service_handle, 
-    g_device_name_char_handle, 
-    0, 
-    nameLen, 
-    (uint8_t *)DeviceName);
+    nameLen = strlen((const char*)deviceName);
+    PRINTF("DeviceName Size=%d\n\r", nameLen);
+
+    ret = aci_gatt_update_char_value(g_gap_service_handle,
+                                     g_device_name_char_handle,
+                                     0,
+                                     nameLen,
+                                     deviceName);
 
     if (BLE_STATUS_SUCCESS != ret){
         PRINTF("device set name failed (ret=0x%x)!!\n\r", ret) ;
@@ -925,18 +953,20 @@
     @endcode
 */
 /**************************************************************************/
-ble_error_t BlueNRGGap::getDeviceName(uint8_t *deviceName, unsigned *lengthP) 
-{   
-    if(DeviceName==NULL) 
-      return BLE_ERROR_UNSPECIFIED;
-    
-    strcpy((char*)deviceName, (const char*)DeviceName);
-    //PRINTF("GetDeviceName=%s\n\r", deviceName);
-    
-    *lengthP = strlen((const char*)DeviceName);
-    //PRINTF("DeviceName Size=%d\n\r", *lengthP); 
-    
-    return BLE_ERROR_NONE;
+ble_error_t BlueNRGGap::getDeviceName(uint8_t *deviceName, unsigned *lengthP)
+{
+    tBleStatus ret;
+
+    ret = aci_gatt_read_handle_value(g_device_name_char_handle+BlueNRGGattServer::CHAR_VALUE_HANDLE,
+                                     *lengthP,
+                                     (uint16_t *)lengthP,
+                                     deviceName);
+    PRINTF("getDeviceName ret=0x%02x (lengthP=%d)\n\r", ret, *lengthP);
+    if (ret == BLE_STATUS_SUCCESS) {
+        return BLE_ERROR_NONE;
+    } else {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
 }
 
 /**************************************************************************/
@@ -961,21 +991,19 @@
 ble_error_t BlueNRGGap::setAppearance(GapAdvertisingData::Appearance appearance)
 {
     tBleStatus ret;
+    uint8_t deviceAppearance[2];
 
-    /* 
-    Tested with GapAdvertisingData::GENERIC_PHONE. 
-    for other appearances BLE Scanner android app is not behaving properly 
-    */
-    //char deviceAppearance[2];   
     STORE_LE_16(deviceAppearance, appearance);                 
     PRINTF("input: incoming = %d deviceAppearance= 0x%x 0x%x\n\r", appearance, deviceAppearance[1], deviceAppearance[0]);
     
-    ret = aci_gatt_update_char_value(g_gap_service_handle, g_appearance_char_handle, 0, 2, (uint8_t *)deviceAppearance);
+    ret = aci_gatt_update_char_value(g_gap_service_handle,
+                                     g_appearance_char_handle,
+                                     0, 2, (uint8_t *)deviceAppearance);
     if (BLE_STATUS_SUCCESS == ret){
         return BLE_ERROR_NONE;
     }
 
-    PRINTF("setAppearance failed (ret=0x%x)!!\n\r", ret) ;
+    PRINTF("setAppearance failed (ret=0x%x)!!\n\r", ret);
     switch (ret) {
       case BLE_STATUS_INVALID_HANDLE:
       case BLE_STATUS_INVALID_PARAMETER:
@@ -1010,70 +1038,20 @@
 /**************************************************************************/
 ble_error_t BlueNRGGap::getAppearance(GapAdvertisingData::Appearance *appearanceP)
 {
-    uint16_t devP;
-    if(!appearanceP) return BLE_ERROR_PARAM_OUT_OF_RANGE;
-    devP = ((uint16_t)(0x0000|deviceAppearance[0])) | (((uint16_t)(0x0000|deviceAppearance[1]))<<8);
-    strcpy((char*)appearanceP, (const char*)&devP);
-    
-    return BLE_ERROR_NONE;    
-}
-
-/**************************************************************************/
-/*!
-    @brief  Gets the value of maximum advertising interval in ms
-
-    @returns    uint16_t
-
-    @retval     value of maximum advertising interval in ms
-                
-    @section EXAMPLE
-
-    @code
-
-    @endcode
-*/
-/**************************************************************************/
-uint16_t BlueNRGGap::getMaxAdvertisingInterval(void) const  {
-    return advtInterval;
-} 
-
-
-/**************************************************************************/
-/*!
-    @brief  Gets the value of minimum advertising interval in ms
+    tBleStatus ret;
+    uint16_t lengthP = 2;
 
-    @returns    uint16_t
-
-    @retval     value of minimum advertising interval in ms
-                
-    @section EXAMPLE
-
-    @code
-
-    @endcode
-*/
-/**************************************************************************/
-uint16_t BlueNRGGap::getMinAdvertisingInterval(void) const {    
-    return 0;    // minimum Advertising interval is 0
-}
+    ret = aci_gatt_read_handle_value(g_appearance_char_handle+BlueNRGGattServer::CHAR_VALUE_HANDLE,
+                                     lengthP,
+                                     &lengthP,
+                                     (uint8_t*)appearanceP);
+    PRINTF("getAppearance ret=0x%02x (lengthP=%d)\n\r", ret, lengthP);
+    if (ret == BLE_STATUS_SUCCESS) {
+        return BLE_ERROR_NONE;
+    } else {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
 
-/**************************************************************************/
-/*!
-    @brief  Gets the value of minimum non connectable advertising interval in ms
-
-    @returns    uint16_t
-
-    @retval     value of minimum non connectable advertising interval in ms
-                
-    @section EXAMPLE
-
-    @code
-
-    @endcode
-*/
-/**************************************************************************/
-uint16_t BlueNRGGap::getMinNonConnectableAdvertisingInterval(void) const {     
-    return BLE_GAP_ADV_NONCON_INTERVAL_MIN;    
 }
 
 GapScanningParams* BlueNRGGap::getScanningParams(void)
@@ -1081,35 +1059,40 @@
   return &_scanningParams;
 }
 
-static void radioScanning(void)
-{
-  GapScanningParams* scanningParams = BlueNRGGap::getInstance().getScanningParams();
-
-  BlueNRGGap::getInstance().startRadioScan(*scanningParams);
-}
-
 static void makeConnection(void)
 {
   BlueNRGGap::getInstance().createConnection();
 }
 
-// ANDREA
 void BlueNRGGap::Discovery_CB(Reason_t reason,
                               uint8_t adv_type,
-                              uint8_t *addr_type,
+                              uint8_t addr_type,
                               uint8_t *addr,
                               uint8_t *data_length,
                               uint8_t *data,
                               uint8_t *RSSI)
 {
-  /* avoid compiler warnings about unused variables */
-  (void)addr_type;
-
   switch (reason) {
   case DEVICE_FOUND:
     {
       GapAdvertisingParams::AdvertisingType_t type;
       bool isScanResponse = false;
+
+      /*
+       * Whitelisting (scan policy):
+       * SCAN_POLICY_FILTER_ALL_ADV (ADV packets only from devs in the White List) &&
+       * Private Random Address
+       * => scan_results = FALSE
+       * FIXME: the Security Manager should be implemented
+       */
+      ScanningPolicyMode_t mode = getScanningPolicyMode();
+      PRINTF("mode=%u addr_type=%u\n\r", mode, addr_type);
+      if(mode == Gap::SCAN_POLICY_FILTER_ALL_ADV ||
+         (addr_type == RESOLVABLE_PRIVATE_ADDR ||
+          addr_type == NON_RESOLVABLE_PRIVATE_ADDR)) {
+        return;
+      }
+
       switch(adv_type) {
       case ADV_IND:
         type = GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED;
@@ -1129,9 +1112,11 @@
         type = GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED;
       }
     
-      PRINTF("adv peerAddr[%02x %02x %02x %02x %02x %02x] \r\n",
-           addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
-      processAdvertisementReport(addr, *RSSI, isScanResponse, type, *data_length, data);
+      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) {
+        processAdvertisementReport(addr, *RSSI, isScanResponse, type, *data_length, data);
+      }
       PRINTF("!!!After processAdvertisementReport\n\r");
     }
     break;
@@ -1139,27 +1124,25 @@
   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
-    // then we start the device connection. Otherwise, we restart the scanning.
+    // then we start the device connection.
     PRINTF("DISCOVERY_COMPLETE\n\r");
     _scanning = false;
 
     // Since the DISCOVERY_COMPLETE event can be received during the scanning interval,
-    // we need to delay the starting of connection or re-scanning procedures
+    // we need to delay the starting of connection
     uint16_t delay = 2*(_scanningParams.getInterval());
+
 #ifdef AST_FOR_MBED_OS
     if(_connecting) {
       minar::Scheduler::postCallback(makeConnection).delay(minar::milliseconds(delay));
-    } else {
-      minar::Scheduler::postCallback(radioScanning).delay(minar::milliseconds(delay));
     }
 #else
     Clock_Wait(delay);
     if(_connecting) {
       makeConnection();
-    } else {
-      radioScanning();
     }
-#endif
+#endif /* AST_FOR_MBED_OS */
+
     break;
   }
 }
@@ -1168,41 +1151,64 @@
 {
   
   tBleStatus ret = BLE_STATUS_SUCCESS;
-  
-  PRINTF("Scanning...\n\r");
-  
-  // We received a start scan request from the application level.
-  // If we are on X-NUCLEO-IDB04A1 (playing a single role at time),
-  // we need to re-init our expansion board to specify the GAP CENTRAL ROLE
-  if(!btle_reinited) {
-    btle_init(isSetAddress, GAP_CENTRAL_ROLE_IDB04A1);
-    btle_reinited = true;
-    
-    PRINTF("BTLE re-init\n\r");
+
+  // Stop ADV before scanning
+  /*
+  if (state.advertising == 1) {
+    stopAdvertising();
+  }
+  */
+
+  /*
+   * Whitelisting (scan policy):
+   * SCAN_POLICY_FILTER_ALL_ADV (ADV packets only from devs in the White List) &&
+   * White List is empty
+   * => scan operation = FAILURE
+   * FIXME: the Security Manager should be implemented
+   */
+  ScanningPolicyMode_t mode = getScanningPolicyMode();
+  uint8_t whiteListSize = whitelistAddresses.size;
+  if(whiteListSize == 0 && mode == Gap::SCAN_POLICY_FILTER_ALL_ADV) {
+    return BLE_ERROR_OPERATION_NOT_PERMITTED;
   }
-  
-  ret = aci_gap_start_general_discovery_proc(scanningParams.getInterval(),
-					     scanningParams.getWindow(),
-					     addr_type,
-					     1); // 1 to filter duplicates
-  
-  if (ret != BLE_STATUS_SUCCESS) {
-    printf("Start Discovery Procedure failed (0x%02X)\n\r", ret);
-    return BLE_ERROR_UNSPECIFIED; 
-  } else {
-    PRINTF("Discovery Procedure Started\n");
+
+  ret = btleStartRadioScan(scanningParams.getActiveScanning(),
+                           scanningParams.getInterval(),
+                           scanningParams.getWindow(),
+                           addr_type);
+
+  PRINTF("Scanning...\n\r");
+  PRINTF("scanningParams.getInterval()=%u[msec]\r\n",(scanningParams.getInterval()*625)/1000);
+  PRINTF("scanningParams.getWindow()=%u[msec]\r\n",(scanningParams.getWindow()*625)/1000);
+  //PRINTF("_advParams.getInterval()=%u\r\n",_advParams.getInterval());
+  //PRINTF("CONN_P1=%u\r\n",(unsigned)CONN_P1);
+  //PRINTF("CONN_P2=%u\r\n",(unsigned)CONN_P2);
+  if (BLE_STATUS_SUCCESS == ret){
+    PRINTF("Observation Procedure Started\n");
     _scanning = true;
-    return BLE_ERROR_NONE; 
+    return BLE_ERROR_NONE;
   }
+
+  // Observer role is not supported by X-NUCLEO-IDB04A1, return BLE_ERROR_NOT_IMPLEMENTED
+  switch (ret) {
+    case BLE_STATUS_INVALID_CID:
+      PRINTF("Observation Procedure not implemented!!!\n\r");
+      return BLE_ERROR_NOT_IMPLEMENTED;
+    default:
+      PRINTF("Observation Procedure failed (0x%02X)\n\r", ret);
+      return BLE_ERROR_UNSPECIFIED;
+  }
+
 }
 
 ble_error_t BlueNRGGap::stopScan() {
   tBleStatus ret = BLE_STATUS_SUCCESS;
-  
-  ret = aci_gap_terminate_gap_procedure(GENERAL_DISCOVERY_PROCEDURE);
+
+  PRINTF("stopScan\n\r");
+  ret = aci_gap_terminate_gap_procedure(GAP_OBSERVATION_PROC);
   
   if (ret != BLE_STATUS_SUCCESS) {
-    printf("GAP Terminate Gap Procedure failed\n");
+    PRINTF("GAP Terminate Gap Procedure failed(ret=0x%x)\n", ret);
     return BLE_ERROR_UNSPECIFIED; 
   } else {
     PRINTF("Discovery Procedure Terminated\n");
@@ -1224,18 +1230,15 @@
     int8_t enHighPower = 0;
     int8_t paLevel = 0;
 
-    int8_t dbmActuallySet = getHighPowerAndPALevelValue(txPower, enHighPower, paLevel);
+    ret = getHighPowerAndPALevelValue(txPower, enHighPower, paLevel);
+    if(ret!=BLE_STATUS_SUCCESS) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
 
-#ifndef DEBUG
-    /* avoid compiler warnings about unused variables */
-    (void)dbmActuallySet;
-#endif
-    
-    PRINTF("txPower=%d, dbmActuallySet=%d\n\r", txPower, dbmActuallySet);
     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_UNSPECIFIED;
+      return BLE_ERROR_PARAM_OUT_OF_RANGE;
     }
 
     return BLE_ERROR_NONE;
@@ -1250,36 +1253,113 @@
 /**************************************************************************/
 void BlueNRGGap::getPermittedTxPowerValues(const int8_t **valueArrayPP, size_t *countP) {
     static const int8_t permittedTxValues[] = {
-        -18, -14, -11, -8, -4, -1, 1, 5, -15, -11, -8, -5, -2, 1, 4, 8
+        -18, -15, -14, -12, -11, -9, -8, -6, -5, -2, 0, 2, 4, 5, 8
     };
 
     *valueArrayPP = permittedTxValues;
     *countP = sizeof(permittedTxValues) / sizeof(int8_t);
 }
 
+/**************************************************************************/
+/*!
+    @brief  Set advertising parameters according to the current state
+            Parameters value is set taking into account guidelines of the BlueNRG
+            time slots allocation
+*/
+/**************************************************************************/
+void BlueNRGGap::setAdvParameters(void)
+{
+  uint32_t advIntMS;
+
+  if(state.connected == 1) {
+    advIntMS = (conn_min_interval*1.25)-GUARD_INT;
+    advInterval = _advParams.MSEC_TO_ADVERTISEMENT_DURATION_UNITS(advIntMS);
+  } else {
+    advInterval = _advParams.getIntervalInADVUnits();
+  }
+}
+
+/**************************************************************************/
+/*!
+    @brief  Set connection parameters according to the current state (ADV and/or SCAN)
+            Parameters value is set taking into account guidelines of the BlueNRG
+            time slots allocation
+*/
+/**************************************************************************/
+void BlueNRGGap::setConnectionParameters(void)
+{
+  if (state.advertising == 1) {
+
+    if (_scanningParams.getInterval() < advInterval) {
+      PRINTF("state.adv=1 scanInterval<advInterval\r\n");
+      scanInterval = advInterval;
+      scanWindow = advInterval;
+    } else {
+      PRINTF("state.adv=1 scanInterval>=advInterval\r\n");
+      scanInterval = _scanningParams.getInterval();
+      scanWindow = _scanningParams.getWindow();
+    }
+
+    if(advInterval>(MAX_INT_CONN-(GUARD_INT/1.25))) { //(4000-GUARD_INT)ms
+        conn_min_interval = MAX_INT_CONN;
+        conn_max_interval = MAX_INT_CONN;
+    } else {
+        conn_min_interval = (_advParams.ADVERTISEMENT_DURATION_UNITS_TO_MS(advInterval)+GUARD_INT)/1.25;
+        conn_max_interval = (_advParams.ADVERTISEMENT_DURATION_UNITS_TO_MS(advInterval)+GUARD_INT)/1.25;
+    }
+
+  } else {
+
+    PRINTF("state.adv = 0\r\n");
+
+    scanInterval = _scanningParams.getInterval();
+    scanWindow = _scanningParams.getWindow();
+    if(SCAN_DURATION_UNITS_TO_MSEC(scanInterval)>(MAX_INT_CONN*1.25) ||
+       SCAN_DURATION_UNITS_TO_MSEC(scanInterval)<(MIN_INT_CONN*1.25)) { //(4000)ms || (7.5)ms
+        conn_min_interval = DEF_INT_CONN;
+        conn_max_interval = DEF_INT_CONN;
+    } else {
+        conn_min_interval = SCAN_DURATION_UNITS_TO_MSEC(scanInterval)/1.25;
+        conn_max_interval = SCAN_DURATION_UNITS_TO_MSEC(scanInterval)/1.25;
+    }
+  }
+  PRINTF("scanInterval=%u[msec]\r\n",SCAN_DURATION_UNITS_TO_MSEC(scanInterval));
+  PRINTF("scanWindow()=%u[msec]\r\n",SCAN_DURATION_UNITS_TO_MSEC(scanWindow));
+  PRINTF("conn_min_interval=%u[msec]\r\n",(unsigned)(conn_min_interval*1.25));
+  PRINTF("conn_max_interval=%u[msec]\r\n",(unsigned)(conn_max_interval*1.25));
+
+}
+
 ble_error_t BlueNRGGap::createConnection ()
 {
   tBleStatus ret;
-  
+
+  /*
+     Before creating connection, set parameters according
+     to previous or current procedure (ADV and/or SCAN)
+   */
+  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    
   */
-  ret = aci_gap_create_connection(SCAN_P,
-				  SCAN_L,
-				  PUBLIC_ADDR,
+  ret = aci_gap_create_connection(scanInterval,
+				  scanWindow,
+				  _peerAddrType,
 				  (unsigned char*)_peerAddr,
-				  PUBLIC_ADDR,
-				  CONN_P1, CONN_P2, 0,
-				  SUPERV_TIMEOUT, CONN_L1 , CONN_L2);
+				  addr_type,
+				  conn_min_interval, conn_max_interval, 0,
+				  SUPERV_TIMEOUT, CONN_L1, CONN_L1);
 
-  _connecting = false;
+  //_connecting = false;
   
   if (ret != BLE_STATUS_SUCCESS) {
-    printf("Error while starting connection (ret=0x%02X).\n\r", ret);
+    PRINTF("Error while starting connection (ret=0x%02X).\n\r", ret);
     return BLE_ERROR_UNSPECIFIED;
   } else {
     PRINTF("Connection started.\n");
+    _connecting = false;
     return BLE_ERROR_NONE;
   }
 }
@@ -1290,14 +1370,14 @@
                                  const GapScanningParams *scanParams)
 {
   /* avoid compiler warnings about unused variables */
-  (void)peerAddrType;
   (void)connectionParams;
   (void)scanParams;
 
-    // Save the peer address
+  // Save the peer address
   for(int i=0; i<BDADDR_SIZE; i++) {
     _peerAddr[i] = peerAddr[i];
   }
+  _peerAddrType = peerAddrType;
 
   _connecting = true;
 
@@ -1310,3 +1390,103 @@
   
   return BLE_ERROR_NONE;
 }
+
+/**************************************************************************/
+/*!
+    @brief  Set the advertising policy filter mode that will be used in
+            the next call to startAdvertising().
+
+    @returns    \ref ble_errror_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly.
+
+                BLE_ERROR_NOT_IMPLEMENTED
+                This feature is currently note implemented.
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::setAdvertisingPolicyMode(Gap::AdvertisingPolicyMode_t mode)
+{
+   advertisingPolicyMode = mode;
+
+    return BLE_ERROR_NONE;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Set the scanning policy filter mode that will be used in
+            the next call to startAdvertising().
+
+    @returns    \ref ble_errror_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly.
+
+                BLE_ERROR_NOT_IMPLEMENTED
+                This feature is currently note implemented.
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::setScanningPolicyMode(Gap::ScanningPolicyMode_t mode)
+{
+    scanningPolicyMode = mode;
+
+    return BLE_ERROR_NONE;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Get the current advertising policy filter mode.
+
+    @returns    The advertising policy filter mode.
+*/
+/**************************************************************************/
+Gap::AdvertisingPolicyMode_t BlueNRGGap::getAdvertisingPolicyMode(void) const
+{
+    return advertisingPolicyMode;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Get the current scanning policy filter mode.
+
+    @returns    The scanning policy filter mode.
+
+*/
+/**************************************************************************/
+Gap::ScanningPolicyMode_t BlueNRGGap::getScanningPolicyMode(void) const
+{
+    return scanningPolicyMode;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Clear BlueNRGGap's state.
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::reset(void)
+{
+    /* Clear all state that is from the parent, including private members */
+    if (Gap::reset() != BLE_ERROR_NONE) {
+        return BLE_ERROR_INVALID_STATE;
+    }
+
+    /* 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;
+
+    return BLE_ERROR_NONE;
+}
+