Merge pull request #7899 from pan-/bt-phy

Bluetooth 5 Phy support

features/FEATURE_BLE/ble/BLETypes.h

@@ -572,6 +572,182 @@ struct peer_address_type_t :SafeEnum<peer_address_type_t, uint8_t> {
         SafeEnum<peer_address_type_t, uint8_t>(PUBLIC) { }
 };
 
+/**
+ * Type that describes a bluetooth PHY(sical) transport.
+ */
+struct phy_t : SafeEnum<phy_t, uint8_t> {
+    /** struct scoped enum wrapped by the class */
+    enum type {
+        /**
+         * 1Mbit/s LE.
+         *
+         * @note This physical transport was available since Bluetooth 4.0
+         */
+        LE_1M = 1,
+
+        /**
+         * 2Mbit/s LE.
+         *
+         * Modulation is similar to LE_1M but differs in rate. Therefore range
+         * performances are in the same ballpark as LE_1M while the increased rate
+         * minimize time spent to transfer or receive a packet which leads to a
+         * better power consumption and/or faster transfer.
+         *
+         * @note This transport has been introduced with the Bluetooth 5.
+         * @note When operating at 2Mbit/s range is not exactly identical to the
+         * range at 1Mbit/s due to a loss in sensitivity.
+         */
+        LE_2M = 2,
+
+        /**
+         * LE Coded PHY.
+         *
+         * This transport reuse the 1Mbit/s channel with different coding schemes.
+         * Either two (S=2) or eight (S=8) symbols can be used to represent a
+         * bit while the 1Mbit/s transport use 1 symbol to code 1 bit of data.
+         *
+         * Here is the data rate of the two coding schemes:
+         *   - S=2: 500kbit/s
+         *   - S=8: 125kbit/s
+         *
+         * The goal of the coded PHY is to increase the range of BLE devices.
+         * Of course given it takes more time to transfer data, transmission
+         * and reception last longer which leads to an increase in power
+         * consumption.
+         *
+         * @note This transport has been introduced with the Bluetooth 5.
+         */
+        LE_CODED
+    };
+
+    /**
+     * Construct a new instance of phy_t.
+     */
+    phy_t(type value) :
+        SafeEnum<phy_t, uint8_t>(value) { }
+};
+
+/**
+ * Type that describe a set of PHY(sical) transports. This is used to
+ * indicate preference for the PHY transports set within it.
+ */
+class phy_set_t {
+public:
+    enum PhysFlags_t {
+        PHY_SET_1M    = 0x01,
+        PHY_SET_2M    = 0x02,
+        PHY_SET_CODED = 0x04
+    };
+
+    /**
+     * Create set that indicates no preference.
+     */
+    phy_set_t() : _value(0) { }
+
+    /**
+     * Create a set based on the mask specified in the Bluetooth spec.
+     *
+     * @param value Octet containing the set of preferred PHYs
+     */
+    phy_set_t(uint8_t value) : _value(value) { }
+
+    /**
+     * Create a set based on individual settings.
+     *
+     * @param phy_1m Prefer LE 1M
+     * @param phy_2m Prefer LE 2M if avaiable
+     * @param phy_coded Prefer coded modulation if avaiable
+     */
+    phy_set_t(
+        bool phy_1m,
+        bool phy_2m,
+        bool phy_coded
+    ) {
+        set_1m(phy_1m);
+        set_2m(phy_2m);
+        set_coded(phy_coded);
+    }
+
+    /** Prefer 1M PHY. */
+    void set_1m(bool enabled = true) {
+        if (enabled) {
+            _value |= PHY_SET_1M;
+        } else {
+            _value &= ~PHY_SET_1M;
+        }
+    }
+
+    /** Prefer 2M PHY. */
+    void set_2m(bool enabled = true) {
+        if (enabled) {
+            _value |= PHY_SET_2M;
+        } else {
+            _value &= ~PHY_SET_2M;
+        }
+    }
+
+    /** Prefer coded PHY. */
+    void set_coded(bool enabled = true) {
+        if (enabled) {
+            _value |= PHY_SET_CODED;
+        } else {
+            _value &= ~PHY_SET_CODED;
+        }
+    }
+
+    bool get_1m() const {
+        return (_value & PHY_SET_1M);
+    }
+
+    bool get_2m() const {
+        return (_value & PHY_SET_2M);
+    }
+
+    bool get_coded() const {
+        return (_value & PHY_SET_CODED);
+    }
+
+    operator uint8_t() const {
+        return _value;
+    }
+
+    uint8_t value() const {
+        return _value;
+    }
+
+private:
+    uint8_t _value;
+};
+
+/**
+ * Type describing the number of symbols per bit in le coded PHY.
+ */
+struct coded_symbol_per_bit_t :SafeEnum<coded_symbol_per_bit_t, uint8_t> {
+    /** struct scoped enum wrapped by the class */
+    enum type {
+        /**
+         * The Number of symbol used to code a bit is undefined.
+         */
+        UNDEFINED,
+
+        /**
+         * Two symbols to code a bit.
+         */
+        S2,
+
+        /**
+         * Eight symbols to code a bit.
+         */
+        S8
+    };
+
+    /**
+     * Construct a new instance of coded_symbol_per_bit_t.
+     */
+    coded_symbol_per_bit_t(type value) :
+        SafeEnum<coded_symbol_per_bit_t, uint8_t>(value) { }
+};
+
 } // namespace ble
 
 /**

features/FEATURE_BLE/ble/Gap.h

@@ -276,6 +276,20 @@ class GapAdvertisingData;
  * gap.startScan(handle_advertising_packet);
  * @endcode
  *
+ * @par Changing the PHYsical transport of a connection
+ *
+ * Once a connection has been established, it is possible to change the physical
+ * transport used between the local and the distant device. Changing the transport
+ * can either increase the bandwidth or increase the communication range.
+ * An increased bandwidth equals a better power consumption but also a loss in
+ * sensibility and therefore a degraded range.
+ * Symmetrically an increased range means a lowered bandwith and a degraded power
+ * consumption.
+ *
+ * Applications can change the PHY used by calling the function setPhy. Once the
+ * update has been made the result is forwarded to the application by calling the
+ * function onPhyUpdateComplete of the event handler registered.
+ *
  * @par disconnection
  *
  * The application code initiates a disconnection when it calls the
@@ -285,6 +299,26 @@ class GapAdvertisingData;
  * controller/stack. To catch all disconnection events, application code may
  * set up an handler taking care of disconnection events by calling
  * onDisconnection().
+ *
+ * @par Modulation Schemes
+ *
+ * When supported by the host and controller you can select different modulation
+ * schemes (@see BLUETOOTH SPECIFICATION Version 5.0 | Vol 1, Part A - 1.2):
+ * - LE 1M PHY
+ * - LE 2M PHY
+ * - LE coded PHY
+ *
+ * You may set preferred PHYs (separately for RX and TX) using setPreferredPhys().
+ * You may also set the currently used PHYs on a selected connection using setPhy().
+ * Both of these settings are only advisory and the controller is allowed to make
+ * its own decision on the best PHY to use based on your request, the peer's
+ * supported features and the connection's physical conditions.
+ *
+ * You may query the currently used PHY using readPhy() which will return the
+ * result through a call to the registered event handler. You may register the
+ * handler with setEventHandler(). The events inform about the currently used
+ * PHY and of any changes to PHYs which may be triggered autonomously by the
+ * controller or by the peer.
  */
 class Gap {
     /*
@@ -533,6 +567,21 @@ public:
      */
     typedef ble::peer_address_type_t PeerAddressType_t;
 
+    /**
+     * Enumeration of BLE PHY
+     */
+    typedef ble::phy_t Phy_t;
+
+    /**
+     * Set of BLE PHYs
+     */
+    typedef ble::phy_set_t PhySet_t;
+
+    /**
+     * Enumeration of type of symbols that can be used with LE coded PHY.
+     */
+    typedef ble::coded_symbol_per_bit_t CodedSymbolPerBit_t;
+
     /**
      * Parameters of a BLE connection.
      */
@@ -1071,6 +1120,78 @@ public:
     typedef CallChainOfFunctionPointersWithContext<const Gap *>
         GapShutdownCallbackChain_t;
 
+
+    /**
+     * Definition of the general handler of Gap related events.
+     */
+    struct EventHandler {
+        /**
+         * Function invoked when the current transmitter and receiver PHY have
+         * been read for a given connection.
+         *
+         * @param status Status of the operation: BLE_ERROR_NONE in case of
+         * success or an appropriate error code.
+         *
+         * @param connectionHandle: The handle of the connection for which the
+         * PHYs have been read.
+         *
+         * @param txPhy PHY used by the transmitter.
+         *
+         * @param rxPhy PHY used by the receiver.
+         */
+        virtual void onReadPhy(
+            ble_error_t status,
+            Handle_t connectionHandle,
+            Phy_t txPhy,
+            Phy_t rxPhy
+        ) {
+            (void)status;
+            (void)connectionHandle;
+            (void)txPhy;
+            (void)rxPhy;
+        }
+
+        /**
+         * Function invoked when the update process of the PHY has been completed.
+         *
+         * The process can be initiated by a call to the function setPhy, the
+         * local bluetooth subsystem or the peer.
+         *
+         * @param status Status of the operation: BLE_ERROR_NONE in case of
+         * success or an appropriate error code.
+         *
+         * @param connectionHandle: The handle of the connection on which the
+         * operation was made.
+         *
+         * @param txPhy PHY used by the transmitter.
+         *
+         * @param rxPhy PHY used by the receiver.
+         *
+         * @note Success doesn't mean the PHY has been updated it means both
+         * ends have negociated the best phy according to their configuration and
+         * capabilities. The PHY currently used are present in the txPhy and
+         * rxPhy parameters.
+         */
+        virtual void onPhyUpdateComplete(
+            ble_error_t status,
+            Handle_t connectionHandle,
+            Phy_t txPhy,
+            Phy_t rxPhy
+        ) {
+            (void)status;
+            (void)connectionHandle;
+            (void)txPhy;
+            (void)rxPhy;
+        }
+
+    protected:
+        /**
+         * Prevent polymorphic deletion and avoid uncessery virtual destructor
+         * as the Gap class will never delete the instance it contains.
+         */
+        ~EventHandler() { }
+    };
+
     /*
      * The following functions are meant to be overridden in the platform-specific subclass.
      */
@@ -1331,6 +1452,72 @@ public:
         const GapScanningParams *scanParams
     );
 
+    /**
+     * Read the PHY used by the transmitter and the receiver on a connection.
+     *
+     * Once the PHY has been read, it is reported back via the function onPhyRead
+     * of the event handler registered by the application.
+     *
+     * @param connection Handle of the connection for which the PHY being used is
+     * queried.
+     *
+     * @return BLE_ERROR_NONE if the read PHY procedure has been started or an
+     * appropriate error code.
+     */
+    virtual ble_error_t readPhy(Handle_t connection) {
+        return BLE_ERROR_NOT_IMPLEMENTED;
+    }
+
+    /**
+     * Set the preferred PHYs to use in a connection.
+     *
+     * @param txPhys: Set of PHYs preferred for tx operations. If NULL then no
+     * preferred PHYs are set and the default value of the subsytem is used.
+     *
+     * @param rxPhys: Set of PHYs preferred for rx operations. If NULL then no
+     * preferred PHYs are set and the default value of the subsytem is used.
+     *
+     * @return BLE_ERROR_NONE if the preferences have been set or an appropriate
+     * error code.
+     */
+    virtual ble_error_t setPreferredPhys(
+        const PhySet_t* txPhys,
+        const PhySet_t* rxPhys
+    ) {
+        return BLE_ERROR_NOT_IMPLEMENTED;
+    }
+
+    /**
+     * Update the PHY used by a connection.
+     *
+     * Once the update process has been completed, it is reported back to the
+     * application via the function onPhyUpdateComplete of the event handler
+     * registered by the application.
+     *
+     * @param connection Handle of the connection to update.
+     *
+     * @param txPhys Set of PHYs preferred for tx operations. If NULL then the
+     * choice is up to the Bluetooth subsystem.
+     *
+     * @param rxPhys Set of PHYs preferred for rx operations. If NULL then the
+     * choice is up to the Bluetooth subsystem.
+     *
+     * @param codedSymbol Number of symbols used to code a bit when le coded is
+     * used. If the value is UNDEFINED then the choice is up to the Bluetooth
+     * subsystem.
+     *
+     * @return BLE_ERROR_NONE if the update PHY procedure has been successfully
+     * started or an error code.
+     */
+    virtual ble_error_t setPhy(
+        Handle_t connection,
+        const PhySet_t* txPhys,
+        const PhySet_t* rxPhys,
+        CodedSymbolPerBit_t codedSymbol
+    ) {
+        return BLE_ERROR_NOT_IMPLEMENTED;
+    }
+
     /**
      * Initiate a disconnection procedure.
      *
@@ -2446,6 +2633,17 @@ public:
 
     /* Event handlers. */
 public:
+
+    /**
+     * Assign the event handler implementation that will be used by the gap
+     * module to signal events back to the application.
+     *
+     * @param handler Application implementation of an Eventhandler.
+     */
+    void setEventHandler(EventHandler* handler) {
+        _eventHandler = handler;
+    }
+
     /**
      * Register a callback handling timeout events.
      *
@@ -2674,6 +2872,7 @@ public:
         disconnectionCallChain.clear();
         radioNotificationCallback = NULL;
         onAdvertisementReport     = NULL;
+        _eventHandler = NULL;
 
         return BLE_ERROR_NONE;
     }
@@ -2694,7 +2893,8 @@ protected:
         radioNotificationCallback(),
         onAdvertisementReport(),
         connectionCallChain(),
-        disconnectionCallChain() {
+        disconnectionCallChain(),
+        _eventHandler(NULL) {
         _advPayload.clear();
         _scanResponse.clear();
     }
@@ -2938,6 +3138,11 @@ protected:
      */
     DisconnectionEventCallbackChain_t disconnectionCallChain;
 
+    /**
+     * Event handler provided by the application.
+     */
+    EventHandler* _eventHandler;
+
 private:
     /**
      * Callchain containing all registered callback handlers for shutdown

features/FEATURE_BLE/ble/generic/GenericGap.h

@@ -43,7 +43,8 @@ namespace generic {
  * @attention: Not part of the public interface of BLE API.
  */
 class GenericGap : public ::Gap,
-                   public pal::ConnectionEventMonitor {
+                   public pal::ConnectionEventMonitor,
+                   public pal::Gap::EventHandler {
 
 public:
     /**
@@ -133,6 +134,29 @@ public:
         const GapScanningParams *scanParams
     );
 
+    /**
+     * @see Gap::readPhy
+     */
+    virtual ble_error_t readPhy(Handle_t connection);
+
+    /**
+    * @see Gap::setPreferredPhys
+    */
+    virtual ble_error_t setPreferredPhys(
+       const phy_set_t* txPhys,
+       const phy_set_t* rxPhys
+    );
+
+    /**
+    * @see Gap::setPhy
+    */
+    virtual ble_error_t setPhy(
+       Handle_t connection,
+       const phy_set_t* txPhys,
+       const phy_set_t* rxPhys,
+       CodedSymbolPerBit_t codedSymbol
+    );
+
     /**
      * @see Gap::disconnect
      */
@@ -371,6 +395,23 @@ private:
 
     void on_address_rotation_timeout();
 
+    /* implements pal::Gap::EventHandler */
+private:
+    virtual void on_read_phy(
+        pal::hci_error_code_t hci_status,
+        Handle_t connection_handle,
+        ble::phy_t tx_phy,
+        ble::phy_t rx_phy
+    );
+
+    virtual void on_phy_update_complete(
+        pal::hci_error_code_t hci_status,
+        Handle_t connection_handle,
+        ble::phy_t tx_phy,
+        ble::phy_t rx_phy
+    );
+
+private:
     pal::EventQueue& _event_queue;
     pal::Gap &_pal_gap;
     pal::GenericAccessService &_gap_service;

features/FEATURE_BLE/ble/pal/PalGap.h

@@ -18,8 +18,9 @@
 #define BLE_PAL_GAP_H_
 
 #include "platform/Callback.h"
-#include "GapTypes.h"
+#include "ble/pal/GapTypes.h"
 #include "GapEvents.h"
+#include "blecommon.h"
 
 namespace ble {
 namespace pal {
@@ -32,6 +33,55 @@ namespace pal {
  * by that layer.
  */
 struct Gap {
+    /** @see BLUETOOTH SPECIFICATION Version 5.0 | Vol 6, Part B - 4.6 */
+    struct ControllerSupportedFeatures_t : SafeEnum<ControllerSupportedFeatures_t, uint8_t> {
+        enum type {
+            LE_ENCRYPTION = 0,
+            CONNECTION_PARAMETERS_REQUEST_PROCEDURE,
+            EXTENDED_REJECT_INDICATION,
+            SLAVE_INITIATED_FEATURES_EXCHANGE,
+            LE_PING,
+            LE_DATA_PACKET_LENGTH_EXTENSION,
+            LL_PRIVACY,
+            EXTENDED_SCANNER_FILTER_POLICIES,
+            LE_2M_PHY,
+            STABLE_MODULATION_INDEX_TRANSMITTER,
+            STABLE_MODULATION_INDEX_RECEIVER,
+            LE_CODED_PHY,
+            LE_EXTENDED_ADVERTISING,
+            LE_PERIODIC_ADVERTISING,
+            CHANNEL_SELECTION_ALGORITHM_2,
+            LE_POWER_CLASS
+        };
+
+        /**
+         * Construct a new instance of ControllerSupportedFeatures_t.
+         */
+        ControllerSupportedFeatures_t(type value) : SafeEnum<ControllerSupportedFeatures_t, uint8_t>(value) { }
+    };
+
+    struct EventHandler {
+        /**
+         * @copydoc Gap::EventHandler::onReadPhy
+         */
+        virtual void on_read_phy(
+            pal::hci_error_code_t status,
+            connection_handle_t connectionHandle,
+            ble::phy_t tx_phy,
+            ble::phy_t rx_phy
+        ) = 0;
+
+        /**
+         * @copydoc Gap::EventHandler::onPhyUpdateComplete
+         */
+        virtual void on_phy_update_complete(
+            pal::hci_error_code_t status,
+            connection_handle_t connection_handle,
+            ble::phy_t tx_phy,
+            ble::phy_t rx_phy
+        ) = 0;
+    };
+
     /**
      * Initialisation of the instance. An implementation can use this function
      * to initialise the subsystems needed to realize the operations of this
@@ -670,6 +720,39 @@ struct Gap {
         bool enable
     ) = 0;
 
+    /**
+     * Checked support for a feature in the link controller.
+     *
+     * @param feature feature to be checked.
+     * @return TRUE if feature is supported.
+     */
+    virtual bool is_feature_supported(
+        ControllerSupportedFeatures_t feature
+    ) = 0;
+     
+    /**
+    * @see Gap::readPhy
+    */
+    virtual ble_error_t read_phy(connection_handle_t connection) = 0;
+
+    /**
+    * @see Gap::setPreferredPhys
+    */
+    virtual ble_error_t set_preferred_phys(
+       const phy_set_t& tx_phys,
+       const phy_set_t& rx_phys
+    ) = 0;
+
+    /**
+    * @see Gap::setPhy
+    */
+    virtual ble_error_t set_phy(
+       connection_handle_t connection,
+       const phy_set_t& tx_phys,
+       const phy_set_t& rx_phys,
+       coded_symbol_per_bit_t coded_symbol
+    ) = 0;
+
     /**
      * Register a callback which will handle Gap events.
      *
@@ -682,8 +765,27 @@ struct Gap {
         _gap_event_cb = cb;
     }
 
+public:
+    /**
+    * Sets the event handler that us called by the PAL porters to notify the stack of events
+    * which will in turn be passed onto the user application when appropriate.
+    *
+    * @param[in] event_handler the new event handler interface implementation. Memory
+    * owned by caller who is responsible for updating this pointer if interface changes.
+    */
+    void set_event_handler(EventHandler *event_handler) {
+        _pal_event_handler = event_handler;
+    }
+
+    EventHandler* get_event_handler() {
+        return _pal_event_handler;
+    }
+
 protected:
-    Gap() { }
+    EventHandler *_pal_event_handler;
+
+protected:
+    Gap() : _pal_event_handler(NULL) { }
 
     virtual ~Gap() { }
 
@@ -700,12 +802,34 @@ protected:
         }
     }
 
+public:
+    /**
+     * Create an ALL_PHYS parameter used in LE Set PHY Command
+     * and LE Set Default PHY Command.
+     * @see BLUETOOTH SPECIFICATION Version 5.0 | Vol 2, Part E - 7.8.49
+     */
+    static uint8_t create_all_phys_value(
+        const phy_set_t& tx_phys,
+        const phy_set_t& rx_phys
+    ) {
+        /* if phy set is empty set corresponding all_phys bit to 1 */
+        uint8_t all_phys = 0;
+        if (tx_phys.value() == 0) {
+            all_phys |= 0x01;
+        }
+        if (rx_phys.value() == 0) {
+            all_phys |= 0x02;
+        }
+        return all_phys;
+    }
+
 private:
     /**
      * Callback called when an event is emitted by the LE subsystem.
      */
     mbed::Callback<void(const GapEvent&)> _gap_event_cb;
 
+private:
     // Disallow copy construction and copy assignment.
     Gap(const Gap&);
     Gap& operator=(const Gap&);

features/FEATURE_BLE/source/generic/GenericGap.cpp

@@ -417,12 +417,16 @@ GenericGap::GenericGap(
     _scan_timeout(),
     _connection_event_handler(NULL)
 {
+    _pal_gap.initialize();
+
     _pal_gap.when_gap_event_received(
         mbed::callback(this, &GenericGap::on_gap_event_received)
     );
 
     // Recover static random identity
     _random_static_identity_address = _pal_gap.get_random_address();
+
+    _pal_gap.set_event_handler(this);
 }
 
 GenericGap::~GenericGap()
@@ -585,6 +589,63 @@ ble_error_t GenericGap::connect(
     );
 }
 
+ble_error_t GenericGap::readPhy(Handle_t connection) {
+    return _pal_gap.read_phy(connection);
+}
+
+ble_error_t GenericGap::setPreferredPhys(
+    const phy_set_t* txPhys,
+    const phy_set_t* rxPhys
+) {
+    phy_set_t tx_phys(txPhys? txPhys->value() : 0);
+    phy_set_t rx_phys(rxPhys? rxPhys->value() : 0);
+    return _pal_gap.set_preferred_phys(tx_phys, rx_phys);
+}
+
+ble_error_t GenericGap::setPhy(
+    Handle_t connection,
+    const phy_set_t* txPhys,
+    const phy_set_t* rxPhys,
+    CodedSymbolPerBit_t codedSymbol
+) {
+    phy_set_t tx_phys(txPhys? txPhys->value() : 0);
+    phy_set_t rx_phys(rxPhys? rxPhys->value() : 0);
+    return _pal_gap.set_phy(connection, tx_phys, rx_phys, codedSymbol);
+}
+
+
+void GenericGap::on_read_phy(
+    pal::hci_error_code_t hci_status,
+    Handle_t connection_handle,
+    ble::phy_t tx_phy,
+    ble::phy_t rx_phy
+) {
+    ble_error_t status = BLE_ERROR_NONE;
+    if (pal::hci_error_code_t::SUCCESS != hci_status) {
+        status = BLE_ERROR_UNSPECIFIED;
+    }
+
+    if (_eventHandler) {
+        _eventHandler->onPhyUpdateComplete(status, connection_handle, tx_phy, rx_phy);
+    }
+}
+
+void GenericGap::on_phy_update_complete(
+    pal::hci_error_code_t hci_status,
+    Handle_t connection_handle,
+    ble::phy_t tx_phy,
+    ble::phy_t rx_phy
+) {
+    ble_error_t status = BLE_ERROR_NONE;
+    if (pal::hci_error_code_t::SUCCESS != hci_status) {
+        status = BLE_ERROR_UNSPECIFIED;
+    }
+
+    if (_eventHandler) {
+        _eventHandler->onPhyUpdateComplete(status, connection_handle, tx_phy, rx_phy);
+    }
+}
+
 ble_error_t GenericGap::disconnect(Handle_t connectionHandle, DisconnectionReason_t reason)
 {
     if (is_disconnection_reason_valid(reason) == false) {

features/FEATURE_BLE/targets/TARGET_CORDIO/CordioPalGap.h

@@ -13,29 +13,20 @@ namespace cordio {
  * Implementation of ble::pal::Gap for the Cordio stack.
  */
 class Gap : public ::ble::pal::Gap {
-
 public:
-    virtual ble_error_t initialize() {
+    virtual bool is_feature_supported(
-        return BLE_ERROR_NONE;
+        Gap::ControllerSupportedFeatures_t feature
-    }
+    );
 
-    virtual ble_error_t terminate() {
+    virtual ble_error_t initialize();
-        return BLE_ERROR_NONE;
-    }
 
-    virtual address_t get_device_address() {
+    virtual ble_error_t terminate();
-        return address_t(HciGetBdAddr());
-    }
 
-    virtual address_t get_random_address() {
+    virtual address_t get_device_address();
-        return device_random_address;
-    }
 
-    virtual ble_error_t set_random_address(const address_t& address) {
+    virtual address_t get_random_address();
-        device_random_address = address;
+
-        DmDevSetRandAddr(const_cast<uint8_t*>(address.data()));
+    virtual ble_error_t set_random_address(const address_t& address);
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t set_advertising_parameters(
         uint16_t advertising_interval_min,
@@ -46,75 +37,19 @@ public:
         const address_t& peer_address,
         advertising_channel_map_t advertising_channel_map,
         advertising_filter_policy_t advertising_filter_policy
-    ) {
-        DmAdvSetInterval(
-            DM_ADV_HANDLE_DEFAULT,
-            advertising_interval_min,
-            advertising_interval_max
     );
 
-        DmAdvSetAddrType(own_address_type.value());
-
-        DmAdvSetChannelMap(
-            DM_ADV_HANDLE_DEFAULT,
-            advertising_channel_map.value()
-        );
-
-        DmDevSetFilterPolicy(
-            DM_FILT_POLICY_MODE_ADV,
-            advertising_filter_policy.value()
-        );
-
-        DmAdvConfig(
-            DM_ADV_HANDLE_DEFAULT,
-            advertising_type.value(),
-            peer_address_type.value(),
-            const_cast<uint8_t*>(peer_address.data())
-        );
-
-        return BLE_ERROR_NONE;
-    }
-
     virtual ble_error_t set_advertising_data(
         uint8_t advertising_data_length,
         const advertising_data_t& advertising_data
-    ) {
-        DmAdvSetData(
-            DM_ADV_HANDLE_DEFAULT,
-            HCI_ADV_DATA_OP_COMP_FRAG,
-            DM_DATA_LOC_ADV,
-            advertising_data_length,
-            const_cast<uint8_t*>(advertising_data.data())
     );
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t set_scan_response_data(
         uint8_t scan_response_data_length,
         const advertising_data_t& scan_response_data
-    ) {
-        DmAdvSetData(
-            DM_ADV_HANDLE_DEFAULT,
-            HCI_ADV_DATA_OP_COMP_FRAG,
-            DM_DATA_LOC_SCAN,
-            scan_response_data_length,
-            const_cast<uint8_t*>(scan_response_data.data())
     );
-        return BLE_ERROR_NONE;
-    }
 
-    virtual ble_error_t advertising_enable(bool enable) {
+    virtual ble_error_t advertising_enable(bool enable);
-        if (enable) {
-            uint8_t adv_handles[] = { DM_ADV_HANDLE_DEFAULT };
-            uint16_t adv_durations[] = { /* infinite */ 0 };
-            uint8_t max_ea_events[] = { 0 };
-            DmAdvStart(1, adv_handles, adv_durations, max_ea_events);
-        } else {
-            uint8_t adv_handles[] = { DM_ADV_HANDLE_DEFAULT };
-            DmAdvStop(1, adv_handles);
-        }
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t set_scan_parameters(
         bool active_scanning,
@@ -122,36 +57,12 @@ public:
         uint16_t scan_window,
         own_address_type_t own_address_type,
         scanning_filter_policy_t filter_policy
-    ) {
-        use_active_scanning = active_scanning;
-        DmScanSetInterval(HCI_INIT_PHY_LE_1M_BIT, &scan_interval, &scan_window);
-        DmScanSetAddrType(own_address_type.value());
-        DmDevSetFilterPolicy(
-            DM_FILT_POLICY_MODE_SCAN,
-            filter_policy.value()
     );
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t scan_enable(
         bool enable,
         bool filter_duplicates
-    ) {
-        if (enable) {
-            uint8_t scanType = use_active_scanning ? DM_SCAN_TYPE_ACTIVE : DM_SCAN_TYPE_PASSIVE;
-            DmScanStart(
-                HCI_SCAN_PHY_LE_1M_BIT,
-                DM_DISC_MODE_NONE,
-                &scanType,
-                filter_duplicates,
-                0,
-                0
     );
-        } else {
-            DmScanStop();
-        }
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t create_connection(
         uint16_t scan_interval,
@@ -166,74 +77,23 @@ public:
         uint16_t supervision_timeout,
         uint16_t minimum_connection_event_length,
         uint16_t maximum_connection_event_length
-    ) {
-        DmConnSetScanInterval(scan_interval, scan_window);
-        DmDevSetFilterPolicy(DM_FILT_POLICY_MODE_INIT, initiator_policy.value());
-        DmConnSetAddrType(own_address_type.value());
-
-        hciConnSpec_t conn_spec = {
-            connection_interval_min,
-            connection_interval_max,
-            connection_latency,
-            supervision_timeout,
-            minimum_connection_event_length,
-            maximum_connection_event_length
-        };
-        DmConnSetConnSpec(&conn_spec);
-
-        dmConnId_t connection_id = DmConnOpen(
-            DM_CLIENT_ID_APP,
-            HCI_INIT_PHY_LE_1M_BIT,
-            peer_address_type.value(),
-            const_cast<uint8_t*>(peer_address.data())
     );
 
-        if (connection_id == DM_CONN_ID_NONE) {
+    virtual ble_error_t cancel_connection_creation();
-            return BLE_ERROR_INTERNAL_STACK_FAILURE;
-        }
 
-        return BLE_ERROR_NONE;
+    virtual uint8_t read_white_list_capacity();
-    }
 
-    virtual ble_error_t cancel_connection_creation() {
+    virtual ble_error_t clear_whitelist();
-        DmConnClose(
-            DM_CLIENT_ID_APP,
-            /* connection handle - invalid */ DM_CONN_ID_NONE,
-            /* reason - invalid (use success) */ 0x00
-        );
-        return BLE_ERROR_NONE;
-    }
-
-    virtual uint8_t read_white_list_capacity() {
-        return HciGetWhiteListSize();
-    }
-
-    virtual ble_error_t clear_whitelist() {
-        DmDevWhiteListClear();
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t add_device_to_whitelist(
         whitelist_address_type_t address_type,
         address_t address
-    ) {
-        DmDevWhiteListAdd(
-            address_type.value(),
-            const_cast<uint8_t*>(address.data())
     );
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t remove_device_from_whitelist(
         whitelist_address_type_t address_type,
         address_t address
-    ) {
-        DmDevWhiteListRemove(
-            address_type.value(),
-            const_cast<uint8_t*>(address.data())
     );
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t connection_parameters_update(
         connection_handle_t connection,
@@ -243,27 +103,8 @@ public:
         uint16_t supervision_timeout,
         uint16_t minimum_connection_event_length,
         uint16_t maximum_connection_event_length
-    ) {
-        if (DmConnCheckIdle(connection) != 0) {
-            return BLE_ERROR_INVALID_STATE;
-        }
-
-        hciConnSpec_t connection_spec = {
-            connection_interval_min,
-            connection_interval_max,
-            connection_latency,
-            supervision_timeout,
-            minimum_connection_event_length,
-            maximum_connection_event_length
-        };
-        DmConnUpdate(
-            connection,
-            &connection_spec
     );
 
-        return BLE_ERROR_NONE;
-    }
-
     virtual ble_error_t accept_connection_parameter_request(
         connection_handle_t connection_handle,
         uint16_t interval_min,
@@ -272,104 +113,52 @@ public:
         uint16_t supervision_timeout,
         uint16_t minimum_connection_event_length,
         uint16_t maximum_connection_event_length
-    ) {
+    );
-        hciConnSpec_t connection_spec = {
-            interval_min,
-            interval_max,
-            latency,
-            supervision_timeout,
-            minimum_connection_event_length,
-            maximum_connection_event_length
-        };
-        DmRemoteConnParamReqReply(connection_handle, &connection_spec);
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t reject_connection_parameter_request(
         connection_handle_t connection_handle,
         hci_error_code_t rejection_reason
-    ) {
-        DmRemoteConnParamReqNegReply(
-            connection_handle,
-            rejection_reason.value()
     );
-        return BLE_ERROR_NONE;
-    }
 
     virtual ble_error_t disconnect(
         connection_handle_t connection,
         disconnection_reason_t disconnection_reason
-    ) {
-        DmConnClose(
-            DM_CLIENT_ID_APP,
-            connection,
-            disconnection_reason.value()
     );
-        return BLE_ERROR_NONE;
-    }
 
-    virtual bool is_privacy_supported() {
+    virtual bool is_privacy_supported();
-        // We only support controller-based privacy, so return whether the controller supports it
-        return HciLlPrivacySupported();
-    }
     
     virtual ble_error_t set_address_resolution(
         bool enable
-    ) {
+    );
-        DmPrivSetAddrResEnable(enable);
+
-        return BLE_ERROR_NONE;
+    virtual ble_error_t read_phy(connection_handle_t connection);
-    }
+
+    virtual ble_error_t set_preferred_phys(
+        const phy_set_t& tx_phys,
+        const phy_set_t& rx_phys
+    );
+
+    virtual ble_error_t set_phy(
+        connection_handle_t connection,
+        const phy_set_t& tx_phys,
+        const phy_set_t& rx_phys,
+        coded_symbol_per_bit_t coded_symbol
+    );
 
     // singleton of the ARM Cordio client
-    static Gap& get_gap() {
+    static Gap& get_gap();
-        static Gap _gap;
-        return _gap;
-    }
 
-private:
-    typedef bool (*event_handler_t)(const wsfMsgHdr_t* msg);
-
-public:
     /**
      * Callback which handle wsfMsgHdr_t and forward them to emit_gap_event.
      */
-    static void gap_handler(const wsfMsgHdr_t* msg) {
+    static void gap_handler(const wsfMsgHdr_t* msg);
-        if (msg == NULL) {
-            return;
-        }
-
-        // all handlers are stored in a static array
-        static const event_handler_t handlers[] = {
-            &event_handler<ConnectionCompleteMessageConverter>,
-            &event_handler<GapAdvertisingReportMessageConverter>,
-            &event_handler<DisconnectionMessageConverter>,
-            &event_handler<ConnectionUpdateMessageConverter>,
-            &event_handler<RemoteConnectionParameterRequestMessageConverter>
-        };
-
-        // event->hdr.param: connection handle
-
-        // traverse all handlers and execute them with the event in input.
-        // exit if an handler has handled the event.
-        for(size_t i = 0; i < (sizeof(handlers)/sizeof(handlers[0])); ++i) {
-            if (handlers[i](msg)) {
-                return;
-            }
-        }
-    }
 
 private:
     /**
      * T shall define a can_convert and convert function and a type
      */
     template<typename T>
-    static bool event_handler(const wsfMsgHdr_t* msg) {
+    static bool event_handler(const wsfMsgHdr_t* msg);
-        if (T::can_convert(msg)) {
-            get_gap().emit_gap_event(T::convert((const typename T::type*)msg));
-            return true;
-        }
-        return false;
-    }
 
     /**
      * Traits defining can_convert for events.

features/FEATURE_BLE/targets/TARGET_CORDIO/source/CordioBLE.cpp

@@ -338,6 +338,7 @@ void BLE::stack_setup()
     DmConnMasterInit();
     DmConnSlaveInit();
     DmSecInit();
+    DmPhyInit();
 
     // Note: enable once security is supported
     DmSecLescInit();

features/FEATURE_BLE/targets/TARGET_CORDIO/source/CordioPalGap.cpp

@@ -0,0 +1,461 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2017-2018 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "CordioPalGap.h"
+#include "hci_api.h"
+
+namespace ble {
+namespace pal {
+namespace vendor {
+namespace cordio {
+
+bool Gap::is_feature_supported(
+    Gap::ControllerSupportedFeatures_t feature
+) {
+    return (HciGetLeSupFeat() & (1 << feature.value()));
+}
+
+ble_error_t Gap::initialize() {
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::terminate() {
+    return BLE_ERROR_NONE;
+}
+
+address_t Gap::get_device_address() {
+    return address_t(HciGetBdAddr());
+}
+
+address_t Gap::get_random_address() {
+    return device_random_address;
+}
+
+ble_error_t Gap::set_random_address(const address_t& address) {
+    device_random_address = address;
+    DmDevSetRandAddr(const_cast<uint8_t*>(address.data()));
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::set_advertising_parameters(
+    uint16_t advertising_interval_min,
+    uint16_t advertising_interval_max,
+    advertising_type_t advertising_type,
+    own_address_type_t own_address_type,
+    advertising_peer_address_type_t peer_address_type,
+    const address_t& peer_address,
+    advertising_channel_map_t advertising_channel_map,
+    advertising_filter_policy_t advertising_filter_policy
+) {
+    DmAdvSetInterval(
+        DM_ADV_HANDLE_DEFAULT,
+        advertising_interval_min,
+        advertising_interval_max
+    );
+
+    DmAdvSetAddrType(own_address_type.value());
+
+    DmAdvSetChannelMap(
+        DM_ADV_HANDLE_DEFAULT,
+        advertising_channel_map.value()
+    );
+
+    DmDevSetFilterPolicy(
+        DM_FILT_POLICY_MODE_ADV,
+        advertising_filter_policy.value()
+    );
+
+    DmAdvConfig(
+        DM_ADV_HANDLE_DEFAULT,
+        advertising_type.value(),
+        peer_address_type.value(),
+        const_cast<uint8_t*>(peer_address.data())
+    );
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::set_advertising_data(
+    uint8_t advertising_data_length,
+    const advertising_data_t& advertising_data
+) {
+    DmAdvSetData(
+        DM_ADV_HANDLE_DEFAULT,
+        HCI_ADV_DATA_OP_COMP_FRAG,
+        DM_DATA_LOC_ADV,
+        advertising_data_length,
+        const_cast<uint8_t*>(advertising_data.data())
+    );
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::set_scan_response_data(
+    uint8_t scan_response_data_length,
+    const advertising_data_t& scan_response_data
+) {
+    DmAdvSetData(
+        DM_ADV_HANDLE_DEFAULT,
+        HCI_ADV_DATA_OP_COMP_FRAG,
+        DM_DATA_LOC_SCAN,
+        scan_response_data_length,
+        const_cast<uint8_t*>(scan_response_data.data())
+    );
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::advertising_enable(bool enable) {
+    if (enable) {
+        uint8_t adv_handles[] = { DM_ADV_HANDLE_DEFAULT };
+        uint16_t adv_durations[] = { /* infinite */ 0 };
+        uint8_t max_ea_events[] = { 0 };
+        DmAdvStart(1, adv_handles, adv_durations, max_ea_events);
+    } else {
+        uint8_t adv_handles[] = { DM_ADV_HANDLE_DEFAULT };
+        DmAdvStop(1, adv_handles);
+    }
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::set_scan_parameters(
+    bool active_scanning,
+    uint16_t scan_interval,
+    uint16_t scan_window,
+    own_address_type_t own_address_type,
+    scanning_filter_policy_t filter_policy
+) {
+    use_active_scanning = active_scanning;
+    DmScanSetInterval(HCI_INIT_PHY_LE_1M_BIT, &scan_interval, &scan_window);
+    DmScanSetAddrType(own_address_type.value());
+    DmDevSetFilterPolicy(
+        DM_FILT_POLICY_MODE_SCAN,
+        filter_policy.value()
+    );
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::scan_enable(
+    bool enable,
+    bool filter_duplicates
+) {
+    if (enable) {
+        uint8_t scanType = use_active_scanning ? DM_SCAN_TYPE_ACTIVE : DM_SCAN_TYPE_PASSIVE;
+        DmScanStart(
+            HCI_SCAN_PHY_LE_1M_BIT,
+            DM_DISC_MODE_NONE,
+            &scanType,
+            filter_duplicates,
+            0,
+            0
+        );
+    } else {
+        DmScanStop();
+    }
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::create_connection(
+    uint16_t scan_interval,
+    uint16_t scan_window,
+    initiator_policy_t initiator_policy,
+    connection_peer_address_type_t peer_address_type,
+    const address_t& peer_address,
+    own_address_type_t own_address_type,
+    uint16_t connection_interval_min,
+    uint16_t connection_interval_max,
+    uint16_t connection_latency,
+    uint16_t supervision_timeout,
+    uint16_t minimum_connection_event_length,
+    uint16_t maximum_connection_event_length
+) {
+    DmConnSetScanInterval(scan_interval, scan_window);
+    DmDevSetFilterPolicy(DM_FILT_POLICY_MODE_INIT, initiator_policy.value());
+    DmConnSetAddrType(own_address_type.value());
+
+    hciConnSpec_t conn_spec = {
+        connection_interval_min,
+        connection_interval_max,
+        connection_latency,
+        supervision_timeout,
+        minimum_connection_event_length,
+        maximum_connection_event_length
+    };
+    DmConnSetConnSpec(&conn_spec);
+
+    dmConnId_t connection_id = DmConnOpen(
+        DM_CLIENT_ID_APP,
+        HCI_INIT_PHY_LE_1M_BIT,
+        peer_address_type.value(),
+        const_cast<uint8_t*>(peer_address.data())
+    );
+
+    if (connection_id == DM_CONN_ID_NONE) {
+        return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    }
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::cancel_connection_creation() {
+    DmConnClose(
+        DM_CLIENT_ID_APP,
+        /* connection handle - invalid */ DM_CONN_ID_NONE,
+        /* reason - invalid (use success) */ 0x00
+    );
+    return BLE_ERROR_NONE;
+}
+
+uint8_t Gap::read_white_list_capacity() {
+    return HciGetWhiteListSize();
+}
+
+ble_error_t Gap::clear_whitelist() {
+    DmDevWhiteListClear();
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::add_device_to_whitelist(
+    whitelist_address_type_t address_type,
+    address_t address
+) {
+    DmDevWhiteListAdd(
+        address_type.value(),
+        const_cast<uint8_t*>(address.data())
+    );
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::remove_device_from_whitelist(
+    whitelist_address_type_t address_type,
+    address_t address
+) {
+    DmDevWhiteListRemove(
+        address_type.value(),
+        const_cast<uint8_t*>(address.data())
+    );
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::connection_parameters_update(
+    connection_handle_t connection,
+    uint16_t connection_interval_min,
+    uint16_t connection_interval_max,
+    uint16_t connection_latency,
+    uint16_t supervision_timeout,
+    uint16_t minimum_connection_event_length,
+    uint16_t maximum_connection_event_length
+) {
+    if (DmConnCheckIdle(connection) != 0) {
+        return BLE_ERROR_INVALID_STATE;
+    }
+
+    hciConnSpec_t connection_spec = {
+        connection_interval_min,
+        connection_interval_max,
+        connection_latency,
+        supervision_timeout,
+        minimum_connection_event_length,
+        maximum_connection_event_length
+    };
+    DmConnUpdate(
+        connection,
+        &connection_spec
+    );
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::accept_connection_parameter_request(
+    connection_handle_t connection_handle,
+    uint16_t interval_min,
+    uint16_t interval_max,
+    uint16_t latency,
+    uint16_t supervision_timeout,
+    uint16_t minimum_connection_event_length,
+    uint16_t maximum_connection_event_length
+) {
+    hciConnSpec_t connection_spec = {
+        interval_min,
+        interval_max,
+        latency,
+        supervision_timeout,
+        minimum_connection_event_length,
+        maximum_connection_event_length
+    };
+    DmRemoteConnParamReqReply(connection_handle, &connection_spec);
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::reject_connection_parameter_request(
+    connection_handle_t connection_handle,
+    hci_error_code_t rejection_reason
+) {
+    DmRemoteConnParamReqNegReply(
+        connection_handle,
+        rejection_reason.value()
+    );
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::disconnect(
+    connection_handle_t connection,
+    disconnection_reason_t disconnection_reason
+) {
+    DmConnClose(
+        DM_CLIENT_ID_APP,
+        connection,
+        disconnection_reason.value()
+    );
+    return BLE_ERROR_NONE;
+}
+
+bool Gap::is_privacy_supported() {
+    // We only support controller-based privacy, so return whether the controller supports it
+    return HciLlPrivacySupported();
+}
+
+ble_error_t Gap::set_address_resolution(
+    bool enable
+) {
+    DmPrivSetAddrResEnable(enable);
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::read_phy(connection_handle_t connection) {
+    DmReadPhy(connection);
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::set_preferred_phys(
+    const phy_set_t& tx_phys,
+    const phy_set_t& rx_phys
+) {
+    DmSetDefaultPhy(
+        create_all_phys_value(tx_phys, rx_phys),
+        tx_phys.value(),
+        rx_phys.value()
+    );
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t Gap::set_phy(
+    connection_handle_t connection,
+    const phy_set_t& tx_phys,
+    const phy_set_t& rx_phys,
+    coded_symbol_per_bit_t coded_symbol
+) {
+    /* if phy set is empty set corresponding all_phys bit to 1 */
+    uint8_t all_phys = 0;
+    if (tx_phys.value() == 0) {
+        all_phys |= 0x01;
+    }
+    if (rx_phys.value() == 0) {
+        all_phys |= 0x02;
+    }
+
+    DmSetPhy(
+        connection,
+        create_all_phys_value(tx_phys, rx_phys),
+        tx_phys.value(),
+        rx_phys.value(),
+        coded_symbol.value()
+    );
+
+    return BLE_ERROR_NONE;
+}
+
+// singleton of the ARM Cordio client
+Gap& Gap::get_gap() {
+    static Gap _gap;
+    return _gap;
+}
+
+/**
+ * Callback which handle wsfMsgHdr_t and forward them to emit_gap_event.
+ */
+void Gap::gap_handler(const wsfMsgHdr_t* msg) {
+    typedef bool (*event_handler_t)(const wsfMsgHdr_t* msg);
+
+    if (msg == NULL) {
+        return;
+    }
+
+    connection_handle_t handle = (connection_handle_t)msg->param;
+
+    switch(msg->event) {
+        case DM_PHY_READ_IND:
+            if (get_gap()._pal_event_handler) {
+                const hciLeReadPhyCmdCmplEvt_t* evt = (const hciLeReadPhyCmdCmplEvt_t*)msg;
+
+                get_gap()._pal_event_handler->on_read_phy(
+                    (hci_error_code_t::type)msg->status,
+                    handle,
+                    (ble::phy_t::type)evt->txPhy,
+                    (ble::phy_t::type)evt->rxPhy
+                );
+            }
+            break;
+        case DM_PHY_UPDATE_IND:
+            if (get_gap()._pal_event_handler) {
+                const hciLePhyUpdateEvt_t* evt = (const hciLePhyUpdateEvt_t*)msg;
+
+                get_gap()._pal_event_handler->on_phy_update_complete(
+                    (hci_error_code_t::type)msg->status,
+                    handle,
+                    (ble::phy_t::type)evt->txPhy,
+                    (ble::phy_t::type)evt->rxPhy
+                );
+            }
+            break;
+    }
+
+    // all handlers are stored in a static array
+    static const event_handler_t handlers[] = {
+        &event_handler<ConnectionCompleteMessageConverter>,
+        &event_handler<GapAdvertisingReportMessageConverter>,
+        &event_handler<DisconnectionMessageConverter>,
+        &event_handler<ConnectionUpdateMessageConverter>,
+        &event_handler<RemoteConnectionParameterRequestMessageConverter>
+    };
+
+    // event->hdr.param: connection handle
+
+    // traverse all handlers and execute them with the event in input.
+    // exit if an handler has handled the event.
+    for(size_t i = 0; i < (sizeof(handlers)/sizeof(handlers[0])); ++i) {
+        if (handlers[i](msg)) {
+            return;
+        }
+    }
+}
+
+/**
+ * T shall define a can_convert and convert function and a type
+ */
+template<typename T>
+bool Gap::event_handler(const wsfMsgHdr_t* msg) {
+    if (T::can_convert(msg)) {
+        get_gap().emit_gap_event(T::convert((const typename T::type*)msg));
+        return true;
+    }
+    return false;
+}
+
+
+} // cordio
+} // vendor
+} // pal
+} // ble

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF52/source/btle/btle.cpp

@@ -335,16 +335,18 @@ void btle_handler(const ble_evt_t *p_ble_evt)
 #ifndef S140
         // Handle PHY upgrade request
         case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
-        {
+            gap.on_phy_update_request(
-            ble_gap_phys_t const phys =
+                p_ble_evt->evt.gap_evt.conn_handle,
-            {
+                p_ble_evt->evt.gap_evt.params.phy_update_request
-                /* rx_phys */ BLE_GAP_PHY_AUTO,
+            );
-                /* tx_phys */ BLE_GAP_PHY_AUTO,
-            };
-            ASSERT_STATUS_RET_VOID( sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys) );
             break;
-        }
 #endif
+        case BLE_GAP_EVT_PHY_UPDATE:
+            gap.on_phy_update(
+                p_ble_evt->evt.gap_evt.conn_handle,
+                p_ble_evt->evt.gap_evt.params.phy_update
+            );
+            break;
 
         	// Handle Data length negotiation request
         case BLE_GAP_EVT_DATA_LENGTH_UPDATE_REQUEST:

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF52/source/nRF5xGap.cpp

@@ -115,6 +115,11 @@ peer_address_type_t convert_identity_address(advertising_peer_address_type_t add
     }
 }
 
+// FIXME: update when SD 5 (not alpha!) or more is used for 52840.
+#ifndef BLE_GAP_PHY_AUTO
+#define BLE_GAP_PHY_AUTO 0
+#endif
+
 } // namespace
 
 void radioNotificationStaticCallback(bool param) {
@@ -134,6 +139,8 @@ nRF5xGap::nRF5xGap() : Gap(),
     _peripheral_privacy_configuration(default_peripheral_privacy_configuration),
     _central_privacy_configuration(default_central_privacy_configuration),
     _non_private_address_type(LegacyAddressType::RANDOM_STATIC),
+    _preferred_tx_phys(BLE_GAP_PHY_AUTO),
+    _preferred_rx_phys(BLE_GAP_PHY_AUTO),
     _connections_role()
 {
     m_connectionHandle = BLE_CONN_HANDLE_INVALID;
@@ -654,6 +661,87 @@ ble_error_t nRF5xGap::connect(
     }
 }
 
+ble_error_t nRF5xGap::readPhy(Handle_t connection) {
+    /*
+     * This function is not implemented as it is not possible with current
+     * nordic API to know which phy was used to establish the connection.
+     *
+     * TODO: Update when Nordic provides the API to do the job.
+     */
+    return BLE_ERROR_NOT_IMPLEMENTED;
+}
+
+ble_error_t nRF5xGap::setPreferredPhys(
+    const ble::phy_set_t* txPhys,
+    const ble::phy_set_t* rxPhys
+) {
+    uint8_t preferred_tx_phys = txPhys? txPhys->value() : 0;
+    uint8_t preferred_rx_phys = rxPhys? rxPhys->value() : 0;
+
+#ifdef S140
+    ble_opt_t opt = { 0 };
+    opt.gap_opt.preferred_phys.tx_phys = preferred_tx_phys;
+    opt.gap_opt.preferred_phys.rx_phys = preferred_rx_phys;
+
+    uint32_t err = sd_ble_opt_set(BLE_GAP_OPT_PREFERRED_PHYS_SET, &opt);
+
+    switch (err) {
+        case NRF_SUCCESS:
+            break;
+        case NRF_ERROR_INVALID_ADDR:
+        case BLE_ERROR_INVALID_CONN_HANDLE:
+        case NRF_ERROR_INVALID_PARAM:
+            return BLE_ERROR_INVALID_PARAM;
+        case NRF_ERROR_INVALID_STATE:
+            return BLE_ERROR_INVALID_STATE;
+        case NRF_ERROR_BUSY:
+            return BLE_STACK_BUSY;
+        default:
+            return BLE_ERROR_UNSPECIFIED;
+    }
+
+#endif
+
+    _preferred_tx_phys = preferred_tx_phys;
+    _preferred_rx_phys = preferred_rx_phys;
+
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t nRF5xGap::setPhy(
+    Handle_t connection,
+    const ble::phy_set_t* txPhys,
+    const ble::phy_set_t* rxPhys,
+    CodedSymbolPerBit_t codedSymbol
+) {
+#ifdef S140
+    return BLE_ERROR_NOT_IMPLEMENTED;
+#else
+    // TODO handle coded symbol once supported by the softdevice.
+    ble_gap_phys_t gap_phys = {
+        txPhys? txPhys->value() : 0,
+        rxPhys? rxPhys->value() : 0
+    };
+
+    uint32_t err = sd_ble_gap_phy_update(connection, &gap_phys);
+
+    switch (err) {
+        case NRF_SUCCESS:
+            return BLE_ERROR_NONE;
+        case NRF_ERROR_INVALID_ADDR:
+        case BLE_ERROR_INVALID_CONN_HANDLE:
+        case NRF_ERROR_INVALID_PARAM:
+            return BLE_ERROR_INVALID_PARAM;
+        case NRF_ERROR_INVALID_STATE:
+            return BLE_ERROR_INVALID_STATE;
+        case NRF_ERROR_BUSY:
+            return BLE_STACK_BUSY;
+        default:
+            return BLE_ERROR_UNSPECIFIED;
+    }
+#endif
+}
+
 ble_error_t nRF5xGap::disconnect(Handle_t connectionHandle, DisconnectionReason_t reason)
 {
     uint8_t code = BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION;
@@ -1507,6 +1595,67 @@ void nRF5xGap::release_all_connections_role() {
         _connections_role[i].is_allocated = false;
     }
 }
+void nRF5xGap::on_phy_update(
+    Handle_t connection,
+    const ble_gap_evt_phy_update_t& evt
+) {
+    if (!_eventHandler) {
+        return;
+    }
+
+    ble_error_t status;
+    switch (evt.status) {
+        case BLE_HCI_STATUS_CODE_SUCCESS:
+            status = BLE_ERROR_NONE;
+            break;
+
+        case BLE_HCI_UNSUPPORTED_REMOTE_FEATURE:
+            status = BLE_ERROR_OPERATION_NOT_PERMITTED;
+            break;
+
+        case BLE_HCI_STATUS_CODE_LMP_ERROR_TRANSACTION_COLLISION:
+        case BLE_HCI_DIFFERENT_TRANSACTION_COLLISION:
+            status = BLE_ERROR_INVALID_STATE;
+            break;
+
+        case BLE_HCI_STATUS_CODE_INVALID_LMP_PARAMETERS:
+            status = BLE_ERROR_INVALID_PARAM;
+            break;
+
+        default:
+            status = BLE_ERROR_UNSPECIFIED;
+            break;
+    }
+
+    _eventHandler->onPhyUpdateComplete(
+        status,
+        connection,
+        Phy_t::LE_1M,
+        Phy_t::LE_1M
+    );
+}
+
+#ifndef S140
+void nRF5xGap::on_phy_update_request(
+    Handle_t connection,
+    const ble_gap_evt_phy_update_request_t& evt
+) {
+    ble_gap_phys_t phys = {
+        _preferred_tx_phys & evt.peer_preferred_phys.tx_phys,
+        _preferred_rx_phys & evt.peer_preferred_phys.rx_phys
+    };
+
+    if (!phys.tx_phys) {
+        phys.tx_phys = BLE_GAP_PHY_AUTO;
+    }
+
+    if (!phys.rx_phys) {
+        phys.rx_phys = BLE_GAP_PHY_AUTO;
+    }
+
+    sd_ble_gap_phy_update(connection, &phys);
+}
+#endif
 
 
 

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF52/source/nRF5xGap.h

@@ -89,6 +89,19 @@ public:
     virtual ble_error_t connect(const Address_t, ble::peer_address_type_t peerAddrType, const ConnectionParams_t *connectionParams, const GapScanningParams *scanParams);
     virtual ble_error_t connect(const Address_t, BLEProtocol::AddressType_t peerAddrType, const ConnectionParams_t *connectionParams, const GapScanningParams *scanParams);
             ble_error_t connect(const Address_t, BLEProtocol::AddressType_t peerAddrType, const ConnectionParams_t *connectionParams, const GapScanningParams *scanParams, bool identity);
+
+    virtual ble_error_t readPhy(Handle_t connection);
+    virtual ble_error_t setPreferredPhys(
+        const ble::phy_set_t* txPhys,
+        const ble::phy_set_t* rxPhys
+    );
+    virtual ble_error_t setPhy(
+        Handle_t connection,
+        const ble::phy_set_t* txPhys,
+        const ble::phy_set_t* rxPhys,
+        CodedSymbolPerBit_t codedSymbol
+    );
+
     virtual ble_error_t disconnect(Handle_t connectionHandle, DisconnectionReason_t reason);
     virtual ble_error_t disconnect(DisconnectionReason_t reason);
 
@@ -287,6 +300,12 @@ private:
     void release_connection_role(ble::connection_handle_t);
     void release_all_connections_role();
 
+    void on_phy_update(Handle_t connection, const ble_gap_evt_phy_update_t& evt);
+    // FIXME: remove guard when S140 updated
+    #ifndef S140
+    void on_phy_update_request(Handle_t connection, const ble_gap_evt_phy_update_request_t& evt);
+    #endif
+
     uint16_t m_connectionHandle;
     ConnectionEventMonitor::EventHandler* _connection_event_handler;
 
@@ -295,6 +314,8 @@ private:
     CentralPrivacyConfiguration_t _central_privacy_configuration;
     AddressType_t _non_private_address_type;
     Address_t _non_private_address;
+    uint8_t _preferred_tx_phys;
+    uint8_t _preferred_rx_phys;
 
     struct connection_role_t {
         connection_role_t() :