Diff: source/main.cpp

Revision:

16:4dd4ecbc8efb

Parent:

11:37872bf83624

Child:

21:59235cb6afd2

--- a/source/main.cpp	Fri Dec 14 13:15:25 2018 +0000
+++ b/source/main.cpp	Mon Jan 14 10:45:29 2019 +0000
@@ -1,5 +1,5 @@
 /* mbed Microcontroller Library
- * Copyright (c) 2006-2013 ARM Limited
+ * Copyright (c) 2006-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.
@@ -17,6 +17,9 @@
 #include <events/mbed_events.h>
 #include <mbed.h>
 #include "ble/BLE.h"
+#include "gap/Gap.h"
+#include "gap/AdvertisingDataParser.h"
+#include "pretty_printer.h"
 
 /** This example demonstrates all the basic setup required
  *  to advertise, scan and connect to other devices.
@@ -30,7 +33,7 @@
  *  a connection will be made to a connectable device upon its discovery.
  */
 
-static const uint8_t DEVICE_NAME[]        = "GAP_device";
+events::EventQueue event_queue;
 
 /* Duration of each mode in milliseconds */
 static const size_t MODE_DURATION_MS      = 6000;
@@ -39,168 +42,139 @@
 static const size_t TIME_BETWEEN_MODES_MS = 2000;
 
 /* how long to wait before disconnecting in milliseconds */
-static const size_t CONNECTION_DURATION = 3000;
+static const size_t CONNECTION_DURATION   = 3000;
+
+/* how many advertising sets we want to crate at once */
+static const uint8_t ADV_SET_NUMBER       = 2;
+
+static const uint16_t MAX_ADVERTISING_PAYLOAD_SIZE = 1000;
 
 typedef struct {
-    GapAdvertisingParams::AdvertisingType_t adv_type;
-    uint16_t interval;
-    uint16_t timeout;
-} AdvModeParam_t;
+    ble::advertising_type_t type;
+    ble::adv_interval_t min_interval;
+    ble::adv_interval_t max_interval;
+} DemoAdvParams_t;
 
 typedef struct {
-    uint16_t interval;
-    uint16_t window;
-    uint16_t timeout;
+    ble::scan_interval_t interval;
+    ble::scan_window_t   window;
+    ble::scan_duration_t duration;
     bool active;
-} ScanModeParam_t;
+} DemoScanParam_t;
 
 /** the entries in this array are used to configure our advertising
  *  parameters for each of the modes we use in our demo */
-static const AdvModeParam_t advertising_params[] = {
-    /*            advertising type                        interval  timeout */
-    { GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED,      40,/*ms*/ 3/*s*/},
-    { GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED,       100,       4     },
-    { GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED, 100,       0     }
+static const DemoAdvParams_t advertising_params[] = {
+/*    advertising type                                   | min interval - 0.625us  | max interval - 0.625us   */
+    { ble::advertising_type_t::CONNECTABLE_UNDIRECTED,      ble::adv_interval_t(40), ble::adv_interval_t(80)  },
+    { ble::advertising_type_t::SCANNABLE_UNDIRECTED,       ble::adv_interval_t(100), ble::adv_interval_t(200) },
+    { ble::advertising_type_t::NON_CONNECTABLE_UNDIRECTED, ble::adv_interval_t(100), ble::adv_interval_t(200) }
 };
 
 /* when we cycle through all our advertising modes we will move to scanning modes */
 
 /** the entries in this array are used to configure our scanning
  *  parameters for each of the modes we use in our demo */
-static const ScanModeParam_t scanning_params[] = {
-/* interval      window    timeout       active */
-    {   4,/*ms*/   4,/*ms*/   0,/*s*/    false },
-    { 160,       100,         3,         false },
-    { 160,        40,         0,         true  },
-    { 500,        10,         0,         false }
+static const DemoScanParam_t scanning_params[] = {
+/*                      interval                  window                   duration  active */
+/*                      0.625ms                  0.625ms                       10ms         */
+    {   ble::scan_interval_t(4),   ble::scan_window_t(4),   ble::scan_duration_t(0), false },
+    { ble::scan_interval_t(160), ble::scan_window_t(100), ble::scan_duration_t(300), false },
+    { ble::scan_interval_t(160),  ble::scan_window_t(40),   ble::scan_duration_t(0), true  },
+    { ble::scan_interval_t(500),  ble::scan_window_t(10),   ble::scan_duration_t(0), false }
 };
 
-/* parameters to use when attempting to connect to maximise speed of connection */
-static const GapScanningParams connection_scan_params(
-    GapScanningParams::SCAN_INTERVAL_MAX,
-    GapScanningParams::SCAN_WINDOW_MAX,
-    3,
-    false
-);
-
-/* get number of items in our arrays */
-static const size_t SCAN_PARAM_SET_MAX =
-    sizeof(scanning_params) / sizeof(GapScanningParams);
-static const size_t ADV_PARAM_SET_MAX  =
-    sizeof(advertising_params) / sizeof(GapAdvertisingParams);
-
-static const char* to_string(Gap::Phy_t phy) {
-    switch(phy.value()) {
-        case Gap::Phy_t::LE_1M:
-            return "LE 1M";
-        case Gap::Phy_t::LE_2M:
-            return "LE 2M";
-        case Gap::Phy_t::LE_CODED:
-            return "LE coded";
-        default:
-            return "invalid PHY";
-    }
+/* helper that gets the number of items in arrays */
+template<class T, size_t N>
+size_t size(const T (&)[N])
+{
+    return N;
 }
 
 /** Demonstrate advertising, scanning and connecting
  */
-class GAPDevice : private mbed::NonCopyable<GAPDevice>, public Gap::EventHandler
+class GapDemo : private mbed::NonCopyable<GapDemo>, public ble::Gap::EventHandler
 {
 public:
-    GAPDevice() :
-        _ble(BLE::Instance()),
+    GapDemo(BLE& ble, events::EventQueue& event_queue) :
+        _ble(ble),
+        _gap(ble.gap()),
+        _event_queue(event_queue),
         _led1(LED1, 0),
         _set_index(0),
-        _is_in_scanning_mode(false),
+        _is_in_scanning_mode(true),
         _is_connecting(false),
         _on_duration_end_id(0),
-        _scan_count(0) { };
+        _scan_count(0),
+        _blink_event(0) {
+        for (uint8_t i = 0; i < size(_adv_handles); ++i) {
+            _adv_handles[i] = ble::INVALID_ADVERTISING_HANDLE;
+        }
+    }
 
-    ~GAPDevice()
+    ~GapDemo()
     {
         if (_ble.hasInitialized()) {
             _ble.shutdown();
         }
-    };
+    }
 
     /** Start BLE interface initialisation */
     void run()
     {
-        ble_error_t error;
-
         if (_ble.hasInitialized()) {
             printf("Ble instance already initialised.\r\n");
             return;
         }
 
-        /* this will inform us off all events so we can schedule their handling
-         * using our event queue */
-        _ble.onEventsToProcess(
-            makeFunctionPointer(this, &GAPDevice::schedule_ble_events)
-        );
+        /* handle gap events */
+        _gap.setEventHandler(this);
 
-        /* handle timeouts, for example when connection attempts fail */
-        _ble.gap().onTimeout(
-            makeFunctionPointer(this, &GAPDevice::on_timeout)
-        );
-
-        /* handle gap events */
-        _ble.gap().setEventHandler(this);
-
-        error = _ble.init(this, &GAPDevice::on_init_complete);
-
+        ble_error_t error = _ble.init(this, &GapDemo::on_init_complete);
         if (error) {
-            printf("Error returned by BLE::init.\r\n");
+            print_error(error, "Error returned by BLE::init");
             return;
         }
 
         /* to show we're running we'll blink every 500ms */
-        _event_queue.call_every(500, this, &GAPDevice::blink);
+        _blink_event = _event_queue.call_every(500, this, &GapDemo::blink);
 
         /* this will not return until shutdown */
         _event_queue.dispatch_forever();
-    };
+    }
 
 private:
     /** This is called when BLE interface is initialised and starts the first mode */
     void on_init_complete(BLE::InitializationCompleteCallbackContext *event)
     {
         if (event->error) {
-            printf("Error during the initialisation\r\n");
+            print_error(event->error, "Error during the initialisation");
             return;
         }
 
-        /* print device address */
-        Gap::AddressType_t addr_type;
-        Gap::Address_t addr;
-        _ble.gap().getAddress(&addr_type, addr);
-        printf("Device address: %02x:%02x:%02x:%02x:%02x:%02x\r\n",
-               addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
+        print_mac_address();
 
         /* setup the default phy used in connection to 2M to reduce power consumption */
-        Gap::PhySet_t tx_phys(/* 1M */ false, /* 2M */ true, /* coded */ false);
-        Gap::PhySet_t rx_phys(/* 1M */ false, /* 2M */ true, /* coded */ false);
-        ble_error_t err = _ble.gap().setPreferredPhys(&tx_phys, &rx_phys);
-        if (err) {
-            printf("INFO: GAP::setPreferedPhys failed with error code %s", BLE::errorToString(err));
+        if (is_2m_phy_supported()) {
+            ble::phy_set_t phys(/* 1M */ false, /* 2M */ true, /* coded */ false);
+
+            ble_error_t error = _gap.setPreferredPhys(/* tx */&phys, /* rx */&phys);
+            if (error) {
+                print_error(error, "GAP::setPreferedPhys failed");
+            }
         }
 
         /* all calls are serialised on the user thread through the event queue */
-        _event_queue.call(this, &GAPDevice::demo_mode_start);
-    };
+        _event_queue.call(this, &GapDemo::demo_mode_start);
+    }
 
     /** queue up start of the current demo mode */
     void demo_mode_start()
     {
         if (_is_in_scanning_mode) {
-            /* when scanning we want to connect to a peer device so we need to
-             * attach callbacks that are used by Gap to notify us of events */
-            _ble.gap().onConnection(this, &GAPDevice::on_connect);
-            _ble.gap().onDisconnection(this, &GAPDevice::on_disconnect);
-
-            _event_queue.call(this, &GAPDevice::scan);
+            _event_queue.call(this, &GapDemo::scan);
         } else {
-            _event_queue.call(this, &GAPDevice::advertise);
+            _event_queue.call(this, &GapDemo::advertise);
         }
 
         /* for performance measurement keep track of duration of the demo mode */
@@ -210,7 +184,9 @@
 
         /* queue up next demo mode */
         _on_duration_end_id = _event_queue.call_in(
-            MODE_DURATION_MS, this, &GAPDevice::on_duration_end
+            MODE_DURATION_MS,
+            this,
+            &GapDemo::end_demo_mode
         );
 
         printf("\r\n");
@@ -219,107 +195,229 @@
     /** Set up and start advertising */
     void advertise()
     {
-        ble_error_t error;
-        GapAdvertisingData advertising_data;
-
-        /* add advertising flags */
-        advertising_data.addFlags(GapAdvertisingData::LE_GENERAL_DISCOVERABLE
-                                  | GapAdvertisingData::BREDR_NOT_SUPPORTED);
+        const DemoAdvParams_t &adv_params = advertising_params[_set_index];
 
-        /* add device name */
-        advertising_data.addData(
-            GapAdvertisingData::COMPLETE_LOCAL_NAME,
-            DEVICE_NAME,
-            sizeof(DEVICE_NAME)
+        /*
+         * Advertising parameters are mainly defined by an advertising type and
+         * and an interval between advertisements. lower interval increases the
+         * chances of being seen at the cost of more power.
+         * The Bluetooth controller may run concurrent operations with the radio;
+         * to help it, a minimum and maximum advertising interval should be
+         * provided.
+         *
+         * With Bluetooth 5; it is possible to advertise concurrently multiple
+         * payloads at different rate. The combination of payload and its associated
+         * parameters is named an advertising set. To refer to these advertising
+         * sets the Bluetooth system use an advertising set handle that needs to
+         * be created first.
+         * The only exception is the legacy advertising handle which is usable
+         * on Bluetooth 4 and Bluetooth 5 system. It is created at startup and
+         * its lifecycle is managed by the system.
+         */
+        ble_error_t error = _gap.setAdvertisingParameters(
+            ble::LEGACY_ADVERTISING_HANDLE,
+            ble::AdvertisingParameters(
+                adv_params.type,
+                adv_params.min_interval,
+                adv_params.max_interval
+            )
         );
-
-        error = _ble.gap().setAdvertisingPayload(advertising_data);
+        if (error) {
+            print_error(error, "Gap::setAdvertisingParameters() failed");
+            return;
+        }
 
+        /* Set payload for the set */
+        /* Use the simple builder to construct the payload; it fails at runtime
+         * if there is not enough space left in the buffer */
+        error = _gap.setAdvertisingPayload(
+            ble::LEGACY_ADVERTISING_HANDLE,
+            ble::AdvertisingDataSimpleBuilder<ble::LEGACY_ADVERTISING_MAX_SIZE>()
+                .setFlags()
+                .setName("Legacy advertiser")
+                .getAdvertisingData()
+        );
         if (error) {
-            printf("Error during Gap::setAdvertisingPayload\r\n");
+            print_error(error, "Gap::setAdvertisingPayload() failed");
+            return;
+        }
+
+        /* Start advertising the set */
+        error = _gap.startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
+        if (error) {
+            print_error(error, "Gap::startAdvertising() failed");
             return;
         }
 
-        /* set the advertising parameters according to currently selected set,
-         * see @AdvertisingType_t for explanation of modes */
-        GapAdvertisingParams::AdvertisingType_t adv_type =
-            advertising_params[_set_index].adv_type;
+        printf("Advertising started (type: 0x%x, interval: [%d : %d]ms)\r\n",
+            adv_params.type.value(),
+            adv_params.min_interval.valueInMs(), adv_params.max_interval.valueInMs() );
 
-        /* how many milliseconds between advertisements, lower interval
-         * increases the chances of being seen at the cost of more power */
-        uint16_t interval = advertising_params[_set_index].interval;
+        if (is_extended_advertising_supported()) {
+            advertise_extended();
+        }
+    }
 
-        /* advertising will continue for this many seconds or until connected */
-        uint16_t timeout = advertising_params[_set_index].timeout;
+    void advertise_extended()
+    {
+        const DemoAdvParams_t &adv_params = advertising_params[_set_index];
+
+        /* this is the memory backing for the payload */
+        uint8_t adv_buffer[MAX_ADVERTISING_PAYLOAD_SIZE];
 
-        _ble.gap().setAdvertisingType(adv_type);
-        _ble.gap().setAdvertisingInterval(interval);
-        _ble.gap().setAdvertisingTimeout(timeout);
+        /* how many sets */
+        uint8_t max_adv_set = std::min(
+            _gap.getMaxAdvertisingSetNumber(),
+            (uint8_t) size(_adv_handles)
+        );
 
-        error = _ble.gap().startAdvertising();
-
-        if (error) {
-            printf("Error during Gap::startAdvertising.\r\n");
+        /* one advertising set is reserved for legacy advertising */
+        if (max_adv_set < 2) {
             return;
         }
 
-        printf("Advertising started (type: 0x%x, interval: %dms, timeout: %ds)\r\n",
-               adv_type, interval, timeout);
-    };
+        /* how much payload in a set */
+        uint16_t max_adv_size = std::min(
+            (uint16_t) _gap.getMaxAdvertisingDataLength(),
+            MAX_ADVERTISING_PAYLOAD_SIZE
+        );
+
+        /* create and start all requested (and possible) advertising sets */
+        for (uint8_t i = 0; i < (max_adv_set - 1); ++i) {
+            /* create the advertising set with its parameter */
+            /* this time we do not use legacy PDUs */
+            ble_error_t error = _gap.createAdvertisingSet(
+                &_adv_handles[i],
+                ble::AdvertisingParameters(
+                    adv_params.type,
+                    adv_params.min_interval,
+                    adv_params.max_interval
+                ).setUseLegacyPDU(false)
+            );
+            if (error) {
+                print_error(error, "Gap::createAdvertisingSet() failed");
+                return;
+            }
+
+            /* use the helper to build the payload */
+            ble::AdvertisingDataBuilder adv_data_builder(
+                adv_buffer,
+                max_adv_size
+            );
+
+            /* set the flags */
+            error = adv_data_builder.setFlags();
+            if (error) {
+                print_error(error, "AdvertisingDataBuilder::setFlags() failed");
+                return;
+            }
+
+            /* set different name for each set */
+            MBED_ASSERT(i < 9);
+            char device_name[] = "Advertiser x";
+            snprintf(device_name, size(device_name), "Advertiser %d", i%10);
+            error = adv_data_builder.setName(device_name);
+            if (error) {
+                print_error(error, "AdvertisingDataBuilder::setName() failed");
+                return;
+            }
+
+            /* Set payload for the set */
+            error = _gap.setAdvertisingPayload(
+                _adv_handles[i],
+                adv_data_builder.getAdvertisingData()
+            );
+            if (error) {
+                print_error(error, "Gap::setAdvertisingPayload() failed");
+                return;
+            }
+
+            /* Start advertising the set */
+            error = _gap.startAdvertising(_adv_handles[i]);
+            if (error) {
+                print_error(error, "Gap::startAdvertising() failed");
+                return;
+            }
+
+            printf("Advertising started (type: 0x%x, interval: [%d : %d]ms)\r\n",
+                adv_params.type.value(),
+                adv_params.min_interval.valueInMs(), adv_params.max_interval.valueInMs() );
+        }
+    }
 
     /** Set up and start scanning */
     void scan()
     {
-        ble_error_t error;
-
-        /* scanning happens repeatedly, interval is the number of milliseconds
-         * between each cycle of scanning */
-        uint16_t interval = scanning_params[_set_index].interval;
-
-        /* number of milliseconds we scan for each time we enter
-         * the scanning cycle after the interval set above */
-        uint16_t window = scanning_params[_set_index].window;
+        const DemoScanParam_t &scan_params = scanning_params[_set_index];
 
-        /* how long to repeat the cycles of scanning in seconds */
-        uint16_t timeout = scanning_params[_set_index].timeout;
-
-        /* active scanning will send a scan request to any scanable devices that
-         * we see advertising */
-        bool active = scanning_params[_set_index].active;
-
-        /* set the scanning parameters according to currently selected set */
-        error = _ble.gap().setScanParams(interval, window, timeout, active);
-
+        /*
+         * Scanning happens repeatedly and is defined by:
+         *  - The scan interval which is the time (in 0.625us) between each scan cycle.
+         *  - The scan window which is the scanning time (in 0.625us) during a cycle.
+         * If the scanning process is active, the local device sends scan requests
+         * to discovered peer to get additional data.
+         */
+        ble_error_t error = _gap.setScanParameters(
+            ble::ScanParameters(
+                ble::phy_t::LE_1M,   // scan on the 1M PHY
+                scan_params.interval,
+                scan_params.window,
+                scan_params.active
+            )
+        );
         if (error) {
-            printf("Error during Gap::setScanParams\r\n");
+            print_error(error, "Error caused by Gap::setScanParameters");
             return;
         }
 
         /* start scanning and attach a callback that will handle advertisements
          * and scan requests responses */
-        error = _ble.gap().startScan(this, &GAPDevice::on_scan);
-
+        error = _gap.startScan(scan_params.duration);
         if (error) {
-            printf("Error during Gap::startScan\r\n");
+            print_error(error, "Error caused by Gap::startScan");
             return;
         }
 
-        printf("Scanning started (interval: %dms, window: %dms, timeout: %ds).\r\n",
-               interval, window, timeout);
-    };
+        printf("Scanning started (interval: %dms, window: %dms, timeout: %dms).\r\n",
+               scan_params.interval.valueInMs(), scan_params.window.valueInMs(), scan_params.duration.valueInMs());
+    }
+
+    /** Finish the mode by shutting down advertising or scanning and move to the next mode. */
+    void end_demo_mode()
+    {
+        if (_is_in_scanning_mode) {
+            end_scanning_mode();
+        } else {
+            end_advertising_mode();
+        }
+
+        /* alloted time has elapsed or be connected, move to next demo mode */
+        _event_queue.call(this, &GapDemo::next_demo_mode);
+    }
 
-    /** After a set duration this cycles to the next demo mode
-     *  unless a connection happened first */
-    void on_duration_end()
+    /** Execute the disconnection */
+    void do_disconnect(ble::connection_handle_t handle)
+    {
+        printf("Disconnecting\r\n");
+        _gap.disconnect(handle, ble::local_disconnection_reason_t::USER_TERMINATION);
+    }
+
+    bool is_2m_phy_supported()
     {
-        print_performance();
+        return _gap.isFeatureSupported(ble::controller_supported_features_t::LE_2M_PHY);
+    }
 
-        /* alloted time has elapsed, move to next demo mode */
-        _event_queue.call(this, &GAPDevice::demo_mode_end);
-    };
+    bool is_extended_advertising_supported()
+    {
+        return _gap.isFeatureSupported(ble::controller_supported_features_t::LE_EXTENDED_ADVERTISING);
+    }
+
+
+private:
+    /* Gap::EventHandler */
 
     /** Look at scan payload to find a peer device and connect to it */
-    void on_scan(const Gap::AdvertisementCallbackParams_t *params)
+    virtual void onAdvertisingReport(const ble::AdvertisingReportEvent &event)
     {
         /* keep track of scan events for performance reporting */
         _scan_count++;
@@ -329,86 +427,105 @@
             return;
         }
 
+        /* only look at events from devices at a close range */
+        if (event.getRssi() < -65) {
+            return;
+        }
+
+        ble::AdvertisingDataParser adv_parser(event.getPayload());
+
         /* parse the advertising payload, looking for a discoverable device */
-        for (uint8_t i = 0; i < params->advertisingDataLen; ++i) {
-            /* The advertising payload is a collection of key/value records where
-             * byte 0: length of the record excluding this byte
-             * byte 1: The key, it is the type of the data
-             * byte [2..N] The value. N is equal to byte0 - 1 */
-            const uint8_t record_length = params->advertisingData[i];
-            if (record_length == 0) {
+        while (adv_parser.hasNext()) {
+            ble::AdvertisingDataParser::element_t field = adv_parser.next();
+
+            /* skip non discoverable device */
+            if (field.type != ble::adv_data_type_t::FLAGS ||
+                field.value.size() != 1 ||
+                !(field.value[0] & GapAdvertisingData::LE_GENERAL_DISCOVERABLE)) {
                 continue;
             }
-            const uint8_t type = params->advertisingData[i + 1];
-            const uint8_t *value = params->advertisingData + i + 2;
 
             /* connect to a discoverable device */
-            if ((type == GapAdvertisingData::FLAGS)
-                && (*value & GapAdvertisingData::LE_GENERAL_DISCOVERABLE)) {
 
-                /* abort timeout as the mode will end on disconnection */
-                _event_queue.cancel(_on_duration_end_id);
-
-                printf("We found a connectable device\r\n");
-
-                ble_error_t error = _ble.gap().connect(
-                    params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC,
-                    NULL, &connection_scan_params
-                );
+            /* abort timeout as the mode will end on disconnection */
+            _event_queue.cancel(_on_duration_end_id);
 
-                if (error) {
-                    printf("Error during Gap::connect\r\n");
-                    /* since no connection will be attempted end the mode */
-                    _event_queue.call(this, &GAPDevice::demo_mode_end);
-                    return;
-                }
-
-                /* we may have already scan events waiting
-                 * to be processed so we need to remember
-                 * that we are already connecting and ignore them */
-                _is_connecting = true;
-
+            printf("We found a connectable device\r\n");
+            ble_error_t error = _gap.connect(
+                event.getPeerAddressType(),
+                event.getPeerAddress(),
+                ble::ConnectionParameters() // use the default connection parameters
+            );
+            if (error) {
+                print_error(error, "Error caused by Gap::connect");
+                /* since no connection will be attempted end the mode */
+                _event_queue.call(this, &GapDemo::end_demo_mode);
                 return;
             }
 
-            i += record_length;
+            /* we may have already scan events waiting
+             * to be processed so we need to remember
+             * that we are already connecting and ignore them */
+            _is_connecting = true;
+            return;
         }
-    };
+    }
+
+    virtual void onAdvertisingEnd(const ble::AdvertisingEndEvent &event)
+    {
+        if (event.isConnected()) {
+            printf("Stopped advertising early due to connection\r\n");
+        }
+    }
+
+    virtual void onScanTimeout(const ble::ScanTimeoutEvent&)
+    {
+        printf("Stopped scanning early due to timeout parameter\r\n");
+        _demo_duration.stop();
+    }
 
     /** This is called by Gap to notify the application we connected,
      *  in our case it immediately disconnects */
-    void on_connect(const Gap::ConnectionCallbackParams_t *connection_event)
+    virtual void onConnectionComplete(const ble::ConnectionCompleteEvent &event)
     {
-        print_performance();
+        _demo_duration.stop();
 
-        printf("Connected in %dms\r\n", _demo_duration.read_ms());
+        if (event.getStatus() == BLE_ERROR_NONE) {
+            printf("Connected in %dms\r\n", _demo_duration.read_ms());
+
+            /* cancel the connect timeout since we connected */
+            _event_queue.cancel(_on_duration_end_id);
 
-        /* cancel the connect timeout since we connected */
-        _event_queue.cancel(_on_duration_end_id);
-
-        _event_queue.call_in(
-            CONNECTION_DURATION, &_ble.gap(), &Gap::disconnect, Gap::REMOTE_USER_TERMINATED_CONNECTION
-        );
-    };
+            _event_queue.call_in(
+                CONNECTION_DURATION,
+                this,
+                &GapDemo::do_disconnect,
+                event.getConnectionHandle()
+            );
+        } else {
+            printf("Failed to connect after scanning %d advertisements\r\n", _scan_count);
+            _event_queue.call(this, &GapDemo::end_demo_mode);
+        }
+    }
 
     /** This is called by Gap to notify the application we disconnected,
-     *  in our case it calls demo_mode_end() to progress the demo */
-    void on_disconnect(const Gap::DisconnectionCallbackParams_t *event)
+     *  in our case it calls next_demo_mode() to progress the demo */
+    virtual void onDisconnectionComplete(const ble::DisconnectionCompleteEvent &event)
     {
         printf("Disconnected\r\n");
 
         /* we have successfully disconnected ending the demo, move to next mode */
-        _event_queue.call(this, &GAPDevice::demo_mode_end);
-    };
+        _event_queue.call(this, &GapDemo::end_demo_mode);
+    }
 
     /**
      * Implementation of Gap::EventHandler::onReadPhy
      */
     virtual void onReadPhy(
         ble_error_t error,
-        Gap::Handle_t connectionHandle,
-        Gap::Phy_t txPhy,
-        Gap::Phy_t rxPhy
+        ble::connection_handle_t connectionHandle,
+        ble::phy_t txPhy,
+        ble::phy_t rxPhy
     ) {
         if (error) {
             printf(
@@ -420,8 +537,8 @@
             printf(
                 "Phy read on connection %d - Tx Phy: %s, Rx Phy: %s\r\n",
                 connectionHandle,
-                to_string(txPhy),
-                to_string(rxPhy)
+                phy_to_string(txPhy),
+                phy_to_string(rxPhy)
             );
         }
     }
@@ -431,9 +548,9 @@
      */
     virtual void onPhyUpdateComplete(
         ble_error_t error,
-        Gap::Handle_t connectionHandle,
-        Gap::Phy_t txPhy,
-        Gap::Phy_t rxPhy
+        ble::connection_handle_t connectionHandle,
+        ble::phy_t txPhy,
+        ble::phy_t rxPhy
     ) {
         if (error) {
             printf(
@@ -445,38 +562,16 @@
             printf(
                 "Phy update on connection %d - Tx Phy: %s, Rx Phy: %s\r\n",
                 connectionHandle,
-                to_string(txPhy),
-                to_string(rxPhy)
+                phy_to_string(txPhy),
+                phy_to_string(rxPhy)
             );
         }
     }
 
-    /** called if timeout is reached during advertising, scanning
-     *  or connection initiation */
-    void on_timeout(const Gap::TimeoutSource_t source)
-    {
-        _demo_duration.stop();
+private:
 
-        switch (source) {
-            case Gap::TIMEOUT_SRC_ADVERTISING:
-                printf("Stopped advertising early due to timeout parameter\r\n");
-                break;
-            case Gap::TIMEOUT_SRC_SCAN:
-                printf("Stopped scanning early due to timeout parameter\r\n");
-                break;
-            case Gap::TIMEOUT_SRC_CONN:
-                printf("Failed to connect after scanning %d advertisements\r\n", _scan_count);
-                _event_queue.call(this, &GAPDevice::print_performance);
-                _event_queue.call(this, &GAPDevice::demo_mode_end);
-                break;
-            default:
-                printf("Unexpected timeout\r\n");
-                break;
-        }
-    };
-
-    /** clean up after last run, cycle to the next mode and launch it */
-    void demo_mode_end()
+    /** Clean up internal state after last run, cycle to the next mode and launch it */
+    void next_demo_mode()
     {
         /* reset the demo ready for the next mode */
         _scan_count = 0;
@@ -488,87 +583,137 @@
 
         /* switch between advertising and scanning when we go
          * through all the params in the array */
-        if (_set_index >= (_is_in_scanning_mode? SCAN_PARAM_SET_MAX : ADV_PARAM_SET_MAX)) {
+        if (_set_index >= (_is_in_scanning_mode ? size(scanning_params) : size(advertising_params))) {
             _set_index = 0;
             _is_in_scanning_mode = !_is_in_scanning_mode;
         }
 
         _ble.shutdown();
+        _event_queue.cancel(_blink_event);
         _event_queue.break_dispatch();
-    };
+    }
+
+    /** Finish the mode by shutting down advertising or scanning and move to the next mode. */
+    void end_scanning_mode()
+    {
+        print_scanning_performance();
+        ble_error_t error = _gap.stopScan();
+
+        if (error) {
+            print_error(error, "Error caused by Gap::stopScan");
+        }
+    }
+
+    void end_advertising_mode()
+    {
+        print_advertising_performance();
+
+        _gap.stopAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
+
+        if (is_extended_advertising_supported()) {
+            end_extended_advertising();
+        }
+    }
+
+    void end_extended_advertising()
+    {
+        /* iterate over the advertising handles */
+        for (uint8_t i = 0; i < size(_adv_handles); ++i) {
+            /* check if the set has been sucesfully created */
+            if (_adv_handles[i] == ble::INVALID_ADVERTISING_HANDLE) {
+                continue;
+            }
+
+            /* if it's still active, stop it */
+            if (_gap.isAdvertisingActive(_adv_handles[i])) {
+                ble_error_t error = _gap.stopAdvertising(_adv_handles[i]);
+                if (error) {
+                    print_error(error, "Error caused by Gap::stopAdvertising");
+                    continue;
+                }
+            }
+
+            ble_error_t error = _gap.destroyAdvertisingSet(_adv_handles[i]);
+            if (error) {
+                print_error(error, "Error caused by Gap::destroyAdvertisingSet");
+                continue;
+            }
+
+            _adv_handles[i] = ble::INVALID_ADVERTISING_HANDLE;
+        }
+    }
 
     /** print some information about our radio activity */
-    void print_performance()
+    void print_scanning_performance()
     {
         /* measure time from mode start, may have been stopped by timeout */
-        uint16_t duration = _demo_duration.read_ms();
+        uint16_t duration_ms = _demo_duration.read_ms();
 
-        if (_is_in_scanning_mode) {
-            /* convert ms into timeslots for accurate calculation as internally
-             * all durations are in timeslots (0.625ms) */
-            uint16_t interval_ts = GapScanningParams::MSEC_TO_SCAN_DURATION_UNITS(
-                scanning_params[_set_index].interval
-            );
-            uint16_t window_ts = GapScanningParams::MSEC_TO_SCAN_DURATION_UNITS(
-                scanning_params[_set_index].window
-            );
-            uint16_t duration_ts = GapScanningParams::MSEC_TO_SCAN_DURATION_UNITS(
-                duration
-            );
-            /* this is how long we scanned for in timeslots */
-            uint16_t rx_ts = (duration_ts / interval_ts) * window_ts;
-            /* convert to milliseconds */
-            uint16_t rx_ms = (rx_ts * GapScanningParams::UNIT_0_625_MS) / 1000;
+        /* convert ms into timeslots for accurate calculation as internally
+         * all durations are in timeslots (0.625ms) */
+        uint16_t duration_ts = ble::scan_interval_t(ble::millisecond_t(duration_ms)).value();
+        uint16_t interval_ts = scanning_params[_set_index].interval.value();
+        uint16_t window_ts = scanning_params[_set_index].window.value();
+        /* this is how long we scanned for in timeslots */
+        uint16_t rx_ts = (duration_ts / interval_ts) * window_ts;
+        /* convert to milliseconds */
+        uint16_t rx_ms = ble::scan_interval_t(rx_ts).valueInMs();
 
-            printf("We have scanned for %dms with an interval of %d"
-                    " timeslot and a window of %d timeslots\r\n",
-                    duration, interval_ts, window_ts);
-
-            printf("We have been listening on the radio for at least %dms\r\n", rx_ms);
+        printf("We have scanned for %dms with an interval of %d"
+               " timeslots and a window of %d timeslots\r\n",
+            duration_ms, interval_ts, window_ts);
 
-        } else /* advertising */ {
+        printf("We have been listening on the radio for at least %dms\r\n", rx_ms);
+    }
 
-            /* convert ms into timeslots for accurate calculation as internally
-             * all durations are in timeslots (0.625ms) */
-            uint16_t interval_ts = GapAdvertisingParams::MSEC_TO_ADVERTISEMENT_DURATION_UNITS(
-                advertising_params[_set_index].interval
-            );
-            uint16_t duration_ts = GapAdvertisingParams::MSEC_TO_ADVERTISEMENT_DURATION_UNITS(
-                duration
-            );
-            /* this is how many times we advertised */
-            uint16_t events = duration_ts / interval_ts;
+    /** print some information about our radio activity */
+    void print_advertising_performance()
+    {
+        /* measure time from mode start, may have been stopped by timeout */
+        uint16_t duration_ms = _demo_duration.read_ms();
+        uint8_t number_of_active_sets = 0;
 
-            printf("We have advertised for %dms"
-                   " with an interval of %d timeslots\r\n",
-                   duration, interval_ts);
-
-            /* non-scannable and non-connectable advertising
-             * skips rx events saving on power consumption */
-            if (advertising_params[_set_index].adv_type
-                == GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED) {
-                printf("We created at least %d tx events\r\n", events);
-            } else {
-                printf("We created at least %d tx and rx events\r\n", events);
+        for (uint8_t i = 0; i < size(_adv_handles); ++i) {
+            if (_adv_handles[i] != ble::INVALID_ADVERTISING_HANDLE) {
+                if (_gap.isAdvertisingActive(_adv_handles[i])) {
+                    number_of_active_sets++;
+                }
             }
         }
-    };
+
+        /* convert ms into timeslots for accurate calculation as internally
+         * all durations are in timeslots (0.625ms) */
+        uint16_t duration_ts = ble::adv_interval_t(ble::millisecond_t(duration_ms)).value();
+        uint16_t interval_ts = advertising_params[_set_index].max_interval.value();
+        /* this is how many times we advertised */
+        uint16_t events = (duration_ts / interval_ts) * number_of_active_sets;
+
+        printf("We have advertised for %dms with an interval of at least %d timeslots\r\n",
+            duration_ms, interval_ts);
 
-    /** Schedule processing of events from the BLE middleware in the event queue. */
-    void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context)
-    {
-        _event_queue.call(mbed::callback(&context->ble, &BLE::processEvents));
-    };
+        if (number_of_active_sets > 1) {
+            printf("We had %d active advertising sets\r\n", number_of_active_sets);
+        }
+
+        /* non-scannable and non-connectable advertising
+         * skips rx events saving on power consumption */
+        if (advertising_params[_set_index].type == ble::advertising_type_t::NON_CONNECTABLE_UNDIRECTED) {
+            printf("We created at least %d tx events\r\n", events);
+        } else {
+            printf("We created at least %d tx and rx events\r\n", events);
+        }
+    }
 
     /** Blink LED to show we're running */
     void blink(void)
     {
         _led1 = !_led1;
-    };
+    }
 
 private:
     BLE                &_ble;
-    events::EventQueue  _event_queue;
+    ble::Gap           &_gap;
+    events::EventQueue &_event_queue;
     DigitalOut          _led1;
 
     /* Keep track of our progress through demo modes */
@@ -583,14 +728,29 @@
     /* Measure performance of our advertising/scanning */
     Timer               _demo_duration;
     size_t              _scan_count;
+
+    int                 _blink_event;
+
+    ble::advertising_handle_t _adv_handles[ADV_SET_NUMBER];
 };
 
+/** Schedule processing of events from the BLE middleware in the event queue. */
+void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) {
+    event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents));
+}
+
 int main()
 {
-    GAPDevice gap_device;
+    BLE &ble = BLE::Instance();
+
+    /* this will inform us off all events so we can schedule their handling
+     * using our event queue */
+    ble.onEventsToProcess(schedule_ble_events);
+
+    GapDemo demo(ble, event_queue);
 
     while (1) {
-        gap_device.run();
+        demo.run();
         wait_ms(TIME_BETWEEN_MODES_MS);
         printf("\r\nStarting next GAP demo mode\r\n");
     };