Diff: shields/TARGET_ST_BLUENRG/source/BlueNRGGap.cpp

Revision:

0:29983394c6b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shields/TARGET_ST_BLUENRG/source/BlueNRGGap.cpp	Thu Apr 13 04:48:11 2017 +0000
@@ -0,0 +1,1447 @@
+/* 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.
+*/
+
+
+/**
+  ******************************************************************************
+  * @file    BlueNRGGap.cpp
+  * @author  STMicroelectronics
+  * @brief   Implementation of BLE_API Gap Class
+  ******************************************************************************
+  * @copy
+  *
+  * 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>
+  */
+
+/** @defgroup BlueNRGGap
+ *  @brief BlueNRG BLE_API GAP Adaptation
+ *  @{
+ */
+
+#include "BlueNRGDevice.h"
+#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();
+    }
+
+}
+
+/**************************************************************************/
+/*!
+    @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
+                The primary advertising data payload
+    @params[in] scanResponse
+                The optional Scan Response payload if the advertising
+                type is set to \ref GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED
+                in \ref GapAdveritinngParams
+
+    @returns    \ref ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @retval     BLE_ERROR_BUFFER_OVERFLOW
+                The proposed action would cause a buffer overflow.  All
+                advertising payloads must be <= 31 bytes, for example.
+
+    @retval     BLE_ERROR_NOT_IMPLEMENTED
+                A feature was requested that is not yet supported in the
+                nRF51 firmware or hardware.
+
+    @retval     BLE_ERROR_PARAM_OUT_OF_RANGE
+                One of the proposed values is outside the valid range.
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+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) {
+        PRINTF("Exceeded the advertising payload length\n\r");
+        return BLE_ERROR_BUFFER_OVERFLOW;
+    }
+
+    /* Make sure we have a payload! */
+    if (advData.getPayloadLen() != 0) {
+        PayloadPtr loadPtr(advData.getPayload(), advData.getPayloadLen());
+
+        /* 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()));
+
+            switch(*loadPtr.getUnitAtIndex(index).getAdTypePtr()) {
+            /**< TX Power Level (in dBm) */
+            case GapAdvertisingData::TX_POWER_LEVEL:
+                {
+                PRINTF("Advertising type: TX_POWER_LEVEL\n\r");
+                int8_t enHighPower = 0;
+                int8_t paLevel = 0;
+
+                int8_t dbm = *loadPtr.getUnitAtIndex(index).getDataPtr();
+                tBleStatus ret = getHighPowerAndPALevelValue(dbm, enHighPower, paLevel);
+#ifdef DEBUG
+                PRINTF("dbm=%d, ret=%d\n\r", dbm, ret);
+                PRINTF("enHighPower=%d, paLevel=%d\n\r", enHighPower, paLevel);
+#endif
+                if(ret == BLE_STATUS_SUCCESS) {
+                  aci_hal_set_tx_power_level(enHighPower, paLevel);
+                }
+                break;
+                }
+            /**< Appearance */
+            case GapAdvertisingData::APPEARANCE:
+                {
+                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;
+                }
+
+            } // end switch
+
+        } //end for
+
+    }
+
+    // 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;
+            }
+        }
+
+        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;
+}
+
+/*
+ * Utility to set ADV timeout flag
+ */
+void BlueNRGGap::setAdvToFlag(void) {
+    AdvToFlag = true;
+}
+
+/*
+ * ADV timeout callback
+ */
+#ifdef AST_FOR_MBED_OS
+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
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+
+ble_error_t BlueNRGGap::startAdvertising(const GapAdvertisingParams &params)
+{
+    tBleStatus ret;
+    int err;
+
+    /* Make sure we support the advertising type */
+    if (params.getAdvertisingType() == GapAdvertisingParams::ADV_CONNECTABLE_DIRECTED) {
+        /* ToDo: This requires a proper security implementation, etc. */
+        return BLE_ERROR_NOT_IMPLEMENTED;
+    }
+
+    /* Check interval range */
+    if (params.getAdvertisingType() == GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED) {
+        /* Min delay is slightly longer for unconnectable devices */
+        if ((params.getIntervalInADVUnits() < GapAdvertisingParams::GAP_ADV_PARAMS_INTERVAL_MIN_NONCON) ||
+                (params.getIntervalInADVUnits() > GapAdvertisingParams::GAP_ADV_PARAMS_INTERVAL_MAX)) {
+            return BLE_ERROR_PARAM_OUT_OF_RANGE;
+        }
+    } else {
+        if ((params.getIntervalInADVUnits() < GapAdvertisingParams::GAP_ADV_PARAMS_INTERVAL_MIN) ||
+                (params.getIntervalInADVUnits() > GapAdvertisingParams::GAP_ADV_PARAMS_INTERVAL_MAX)) {
+            return BLE_ERROR_PARAM_OUT_OF_RANGE;
+        }
+    }
+
+    /* Check timeout is zero for Connectable Directed */
+    if ((params.getAdvertisingType() == GapAdvertisingParams::ADV_CONNECTABLE_DIRECTED) && (params.getTimeout() != 0)) {
+        /* Timeout must be 0 with this type, although we'll never get here */
+        /* since this isn't implemented yet anyway */
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
+
+    /* Check timeout for other advertising types */
+    if ((params.getAdvertisingType() != GapAdvertisingParams::ADV_CONNECTABLE_DIRECTED) &&
+            (params.getTimeout() > GapAdvertisingParams::GAP_ADV_PARAMS_TIMEOUT_MAX)) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
+
+    /*
+     * 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;
+        }
+    }
+
+    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 (_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);
+            switch (ret) {
+              case BLE_STATUS_TIMEOUT:
+                return BLE_STACK_BUSY;
+              default:
+                return BLE_ERROR_UNSPECIFIED;
+            }
+        }
+    } else {
+        hci_le_set_scan_resp_data(0, NULL);
+    }
+
+    setAdvParameters();
+    PRINTF("advInterval=%d advType=%d\n\r", advInterval, params.getAdvertisingType());
+
+    err = hci_le_set_advertising_data(_advData.getPayloadLen(), _advData.getPayload());
+
+    if (err) {
+        PRINTF("error while setting the payload\r\n");
+        return BLE_ERROR_UNSPECIFIED;
+    }
+
+    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;
+    }
+
+    err = hci_le_set_advertise_enable(0x01);
+    if (err) {
+        PRINTF("impossible to start advertising\n\r");
+        return BLE_ERROR_UNSPECIFIED;
+    }
+
+    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;
+
+}
+
+
+/**************************************************************************/
+/*!
+    @brief  Stops the BLE HW and disconnects from any devices
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::stopAdvertising(void)
+{
+
+    if(state.advertising == 1) {
+
+        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;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Disconnects if we are connected to a central device
+
+    @param[in]  reason
+                Disconnection Reason
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::disconnect(Handle_t connectionHandle, Gap::DisconnectionReason_t reason)
+{
+    tBleStatus ret;
+
+    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;
+        }
+    }
+
+    return BLE_ERROR_NONE;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Disconnects if we are connected to a central device
+
+    @param[in]  reason
+                Disconnection Reason
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::disconnect(Gap::DisconnectionReason_t reason)
+{
+    return disconnect(m_connectionHandle, reason);
+}
+
+/**************************************************************************/
+/*!
+    @brief  Sets the 16-bit connection handle
+
+    @param[in]  conn_handle
+                Connection Handle which is set in the Gap Instance
+
+    @returns    void
+*/
+/**************************************************************************/
+void BlueNRGGap::setConnectionHandle(uint16_t conn_handle)
+{
+    m_connectionHandle = conn_handle;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the 16-bit connection handle
+
+    @param[in]  void
+
+    @returns    uint16_t
+                Connection Handle of the Gap Instance
+*/
+/**************************************************************************/
+uint16_t BlueNRGGap::getConnectionHandle(void)
+{
+    return m_connectionHandle;
+}
+
+/**************************************************************************/
+/*!
+    @brief      Sets the BLE device address. SetAddress will reset the BLE
+                device and re-initialize BTLE. Will not start advertising.
+
+    @param[in]  type
+                Type of Address
+
+    @param[in]  address[6]
+                Value of the Address to be set
+
+    @returns    ble_error_t
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::setAddress(AddressType_t type, const BLEProtocol::AddressBytes_t address)
+{
+    if (type > BLEProtocol::AddressType::RANDOM_PRIVATE_NON_RESOLVABLE) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
+
+    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 {
+        // 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;
+}
+
+/**************************************************************************/
+/*!
+    @brief      Returns boolean if the address of the device has been set
+                or not
+
+    @returns    bool
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+bool BlueNRGGap::getIsSetAddress()
+{
+    return isSetAddress;
+}
+
+/**************************************************************************/
+/*!
+    @brief      Returns the address of the device if set
+
+    @returns    Pointer to the address if Address is set else NULL
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::getAddress(BLEProtocol::AddressType_t *typeP, BLEProtocol::AddressBytes_t address)
+{
+    uint8_t bdaddr[BDADDR_SIZE];
+    uint8_t data_len_out;
+
+    // precondition, check that pointers in input are valid
+    if (typeP == NULL || address == NULL) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    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;
+    }
+
+    *typeP = addr_type;
+    memcpy(address, bdaddr, BDADDR_SIZE);
+
+    return BLE_ERROR_NONE;
+}
+
+/**************************************************************************/
+/*!
+    @brief      obtains preferred connection params
+
+    @returns    ble_error_t
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::getPreferredConnectionParams(ConnectionParams_t *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;
+
+    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;
+}
+
+
+/**************************************************************************/
+/*!
+    @brief      sets preferred connection params
+
+    @returns    ble_error_t
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::setPreferredConnectionParams(const ConnectionParams_t *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;
+    }
+
+    // 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;
+}
+
+/**************************************************************************/
+/*!
+    @brief      updates preferred connection params
+
+    @returns    ble_error_t
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::updateConnectionParams(Handle_t handle, const ConnectionParams_t *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
+
+    @param[in]  deviceName
+                pointer to device name to be set
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::setDeviceName(const uint8_t *deviceName)
+{
+    tBleStatus ret;
+    uint8_t nameLen = 0;
+
+    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) ;
+        switch (ret) {
+          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;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the Device Name Characteristic
+
+    @param[in]  deviceName
+                pointer to device name
+
+    @param[in]  lengthP
+                pointer to device name length
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+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;
+    }
+}
+
+/**************************************************************************/
+/*!
+    @brief  Sets the Device Appearance Characteristic
+
+    @param[in]  appearance
+                device appearance
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::setAppearance(GapAdvertisingData::Appearance appearance)
+{
+    tBleStatus ret;
+    uint8_t deviceAppearance[2];
+
+    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);
+    if (BLE_STATUS_SUCCESS == ret){
+        return BLE_ERROR_NONE;
+    }
+
+    PRINTF("setAppearance failed (ret=0x%x)!!\n\r", ret);
+    switch (ret) {
+      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;
+    }
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the Device Appearance Characteristic
+
+    @param[in]  appearance
+                pointer to device appearance value
+
+    @returns    ble_error_t
+
+    @retval     BLE_ERROR_NONE
+                Everything executed properly
+
+    @section EXAMPLE
+
+    @code
+
+    @endcode
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::getAppearance(GapAdvertisingData::Appearance *appearanceP)
+{
+    tBleStatus ret;
+    uint16_t lengthP = 2;
+
+    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;
+    }
+
+}
+
+GapScanningParams* BlueNRGGap::getScanningParams(void)
+{
+  return &_scanningParams;
+}
+
+static void makeConnection(void)
+{
+  BlueNRGGap::getInstance().createConnection();
+}
+
+void BlueNRGGap::Discovery_CB(Reason_t reason,
+                              uint8_t adv_type,
+                              uint8_t addr_type,
+                              uint8_t *addr,
+                              uint8_t *data_length,
+                              uint8_t *data,
+                              uint8_t *RSSI)
+{
+  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;
+        break;
+      case ADV_DIRECT_IND:
+        type = GapAdvertisingParams::ADV_CONNECTABLE_DIRECTED;
+        break;
+      case ADV_SCAN_IND:
+      case SCAN_RSP:
+        type = GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED;
+        isScanResponse = true;
+        break;
+      case ADV_NONCONN_IND:
+        type = GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED;
+        break;
+      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) {
+        processAdvertisementReport(addr, *RSSI, isScanResponse, type, *data_length, data);
+      }
+      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
+    // 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
+    uint16_t delay = 2*(_scanningParams.getInterval());
+
+#ifdef AST_FOR_MBED_OS
+    if(_connecting) {
+      minar::Scheduler::postCallback(makeConnection).delay(minar::milliseconds(delay));
+    }
+#else
+    Clock_Wait(delay);
+    if(_connecting) {
+      makeConnection();
+    }
+#endif /* AST_FOR_MBED_OS */
+
+    break;
+  }
+}
+
+ble_error_t BlueNRGGap::startRadioScan(const GapScanningParams &scanningParams)
+{
+
+  tBleStatus ret = BLE_STATUS_SUCCESS;
+
+  // 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 = 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;
+  }
+
+  // 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;
+
+  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;
+  } else {
+    PRINTF("Discovery Procedure Terminated\n");
+    return BLE_ERROR_NONE;
+  }
+}
+
+/**************************************************************************/
+/*!
+    @brief  set Tx power level
+    @param[in] txPower Transmission Power level
+    @returns    ble_error_t
+*/
+/**************************************************************************/
+ble_error_t BlueNRGGap::setTxPower(int8_t txPower)
+{
+    tBleStatus ret;
+
+    int8_t enHighPower = 0;
+    int8_t paLevel = 0;
+
+    ret = getHighPowerAndPALevelValue(txPower, enHighPower, paLevel);
+    if(ret!=BLE_STATUS_SUCCESS) {
+        return BLE_ERROR_PARAM_OUT_OF_RANGE;
+    }
+
+    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;
+    }
+
+    return BLE_ERROR_NONE;
+}
+
+/**************************************************************************/
+/*!
+    @brief  get permitted Tx power values
+    @param[in] values pointer to pointer to permitted power values
+    @param[in] num number of values
+*/
+/**************************************************************************/
+void BlueNRGGap::getPermittedTxPowerValues(const int8_t **valueArrayPP, size_t *countP) {
+    static const int8_t permittedTxValues[] = {
+        -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);
+
+    PRINTF("conn_min_interval is equal to %u\r\n", conn_min_interval);
+  } 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(scanInterval,
+				  scanWindow,
+				  _peerAddrType,
+				  (unsigned char*)_peerAddr,
+				  addr_type,
+				  conn_min_interval, conn_max_interval, 0,
+				  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;
+  } else {
+    PRINTF("Connection started.\n");
+    _connecting = false;
+    return BLE_ERROR_NONE;
+  }
+}
+
+ble_error_t BlueNRGGap::connect (const Gap::Address_t peerAddr,
+                                 Gap::AddressType_t peerAddrType,
+                                 const ConnectionParams_t *connectionParams,
+                                 const GapScanningParams *scanParams)
+{
+  /* avoid compiler warnings about unused variables */
+  (void)connectionParams;
+
+  setScanParams(scanParams->getInterval(),
+                scanParams->getWindow(),
+                scanParams->getTimeout(),
+                scanParams->getActiveScanning()
+               );
+
+  // Save the peer address
+  for(int i=0; i<BDADDR_SIZE; i++) {
+    _peerAddr[i] = peerAddr[i];
+  }
+  _peerAddrType = peerAddrType;
+
+  _connecting = true;
+
+  if(_scanning) {
+    stopScan();
+  } else {
+    PRINTF("Calling createConnection from connect()\n\r");
+    return createConnection();
+  }
+
+  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;
+
+    /* 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;
+}
+
+void BlueNRGGap::setConnectionInterval(uint16_t interval) {
+    conn_min_interval = interval;
+    conn_max_interval = interval;
+}
+
+void BlueNRGGap::setGapRole(Role_t role)
+{
+    gapRole = role;
+}