16 files changed, 311 insertions(+), 233 deletions(-)

M module-apps/application-settings-new/windows/AddDeviceWindow.cpp
M module-apps/application-settings-new/windows/AllDevicesWindow.cpp
M module-bluetooth/Bluetooth/BluetoothWorker.cpp
M module-bluetooth/Bluetooth/BluetoothWorker.hpp
M module-bluetooth/Bluetooth/BtCommand.hpp
M module-bluetooth/Bluetooth/CommandHandler.cpp
M module-bluetooth/Bluetooth/CommandHandler.hpp
M module-bluetooth/Bluetooth/Device.hpp
M module-bluetooth/Bluetooth/WorkerController.cpp
M module-bluetooth/Bluetooth/interface/BluetoothDriver.cpp
M module-bluetooth/Bluetooth/interface/BluetoothDriver.hpp
R module-bluetooth/Bluetooth/interface/profiles/{ => GAP}/GAP.cpp
A module-bluetooth/Bluetooth/interface/profiles/GAP/GAP.hpp
M module-bluetooth/CMakeLists.txt
M module-bluetooth/lib/btstack.cmake
M module-bsp/board/rt1051/common/macros.h

@@ 1,4 1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "AddDeviceWindow.hpp"

@@ 1,4 1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "AllDevicesWindow.hpp"


@@ 28,6 28,8 @@ namespace gui
     void AllDevicesWindow::onBeforeShow(ShowMode mode, SwitchData *data)
     {
         clearOptions();
+        application->bus.sendUnicast(std::make_shared<BluetoothMessage>(BluetoothMessage::Request::StopScan),
+                                     "ServiceBluetooth");
         if (const auto newData = dynamic_cast<BondedDevicesData *>(data); newData != nullptr) {
             addOptions(allDevicesOptionsList(newData->getDevices()));
         }

@@ 48,7 48,7 @@ namespace
         [[nodiscard]] auto operator()()
         {
             bluetooth::KeyStorage::settings = settings;
-            bluetooth::GAP::register_scan();
+            bluetooth::GAP::registerScan();
 
             auto settingsName = std::get<std::string>(settings->getValue(bluetooth::Settings::DeviceName));
             if (settingsName.empty()) {


@@ 58,8 58,6 @@ namespace
                 settingsName = name;
             }
             bluetooth::set_name(settingsName);
-            bluetooth::GAP::set_visibility(
-                std::visit(bluetooth::BoolVisitor{}, settings->getValue(bluetooth::Settings::Visibility)));
 
             settings->onLinkKeyAdded = onLinkKeyAdded;
             return bluetooth::Error::Success;


@@ 76,8 74,9 @@ namespace
                                   std::shared_ptr<bluetooth::Profile> currentProfile,
                                   DeviceRegistration::OnLinkKeyAddedCallback &&onLinkKeyAddedCallback)
     {
-        auto driver         = std::make_unique<bluetooth::Driver>(loop->getRunLoopInstance());
-        auto commandHandler = std::make_unique<bluetooth::CommandHandler>(service, settings, std::move(currentProfile));
+        auto driver = std::make_shared<bluetooth::Driver>(loop->getRunLoopInstance(), service);
+        auto commandHandler =
+            std::make_unique<bluetooth::CommandHandler>(service, settings, std::move(currentProfile), driver);
         return std::make_unique<bluetooth::StatefulController>(
             std::move(driver),
             std::move(commandHandler),


@@ 109,7 108,8 @@ void BluetoothWorker::registerQueues()
 
 void BluetoothWorker::onLinkKeyAdded(const std::string &deviceAddress)
 {
-    for (auto &device : bluetooth::GAP::devices) {
+    auto devices = bluetooth::GAP::getDevicesList();
+    for (auto &device : devices) {
         if (bd_addr_to_str(device.address) == deviceAddress) {
             pairedDevices.emplace_back(device);
             settings->setValue(bluetooth::Settings::BondedDevices, SettingsSerializer::toString(pairedDevices));


@@ 249,7 249,7 @@ auto BluetoothWorker::handleMessage(uint32_t queueID) -> bool
 
 void BluetoothWorker::setDeviceAddress(bd_addr_t addr)
 {
-    bluetooth::GAP::do_pairing(addr);
+
     currentProfile->setDeviceAddress(addr);
 }
 

@@ 13,7 13,6 @@
 #include <vector>
 #include "service-bluetooth/SettingsHolder.hpp"
 #include "glucode/BluetoothRunLoop.hpp"
-#include "interface/BluetoothDriver.hpp"
 #include "WorkerController.hpp"
 struct HCI;
 

@@ 1,31 1,19 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
 #include <FreeRTOS.h>
 #include <task.h>
 
-#include "BluetoothWorker.hpp"
 #include "Error.hpp"
 #include <functional>
 #include <btstack_run_loop.h>
+#include <string>
 
 namespace bluetooth
 {
 
     auto set_name(std::string &name) -> Error;
-    namespace GAP
-    {
-        extern std::vector<Devicei> devices;
-        /// THIS have to be called prior to Bt system start!
-        auto register_scan() -> Error;
-        auto scan() -> Error;
-        void stop_scan();
-        auto set_visibility(bool visibility) -> Error;
-        auto do_pairing(uint8_t *addr) -> bool;
-
-        void setOwnerService(sys::Service *service);
-    }; // namespace GAP
     namespace PAN
     {
         auto bnep_start() -> Error;

@@ 3,8 3,10 @@
 
 #include "CommandHandler.hpp"
 
-#include "BtCommand.hpp"
+#include <utility>
+
 #include "Device.hpp"
+#include "BtCommand.hpp"
 
 namespace bluetooth
 {


@@ 18,8 20,10 @@ namespace bluetooth
 
     CommandHandler::CommandHandler(sys::Service *service,
                                    std::shared_ptr<bluetooth::SettingsHolder> settings,
-                                   std::shared_ptr<bluetooth::Profile> currentProfile)
-        : service{service}, settings{std::move(settings)}, currentProfile{std::move(currentProfile)}
+                                   std::shared_ptr<bluetooth::Profile> currentProfile,
+                                   std::shared_ptr<bluetooth::Driver> driver)
+        : service{service}, settings{std::move(settings)}, currentProfile{std::move(currentProfile)}, driver{std::move(
+                                                                                                          driver)}
     {}
 
     Error::Code CommandHandler::handle(Command command)


@@ 49,8 53,8 @@ namespace bluetooth
 
     Error::Code CommandHandler::scan()
     {
-        bluetooth::GAP::setOwnerService(service);
-        if (const auto ret = bluetooth::GAP::scan(); ret.err != bluetooth::Error::Success) {
+
+        if (const auto ret = driver->scan(); ret.err != bluetooth::Error::Success) {
             LOG_ERROR("Cant start scan!: %s %" PRIu32 "", toString(ret.err).c_str(), ret.lib_code);
             return ret.err;
         }


@@ 62,7 66,7 @@ namespace bluetooth
 
     Error::Code CommandHandler::stopScan()
     {
-        bluetooth::GAP::stop_scan();
+        driver->stopScan();
         return Error::Success;
     }
 


@@ 78,9 82,9 @@ namespace bluetooth
 
     Error::Code CommandHandler::setVisibility(bool visibility)
     {
-        const auto status = bluetooth::GAP::set_visibility(visibility);
+        driver->setVisibility(visibility);
         settings->setValue(bluetooth::Settings::Visibility, static_cast<int>(visibility));
-        return status.err;
+        return Error::Success;
     }
 
     Error::Code CommandHandler::establishAudioConnection()

@@ 5,6 5,7 @@
 
 #include "Error.hpp"
 #include "interface/profiles/Profile.hpp"
+#include "interface/BluetoothDriver.hpp"
 
 #include <service-bluetooth/SettingsHolder.hpp>
 #include <Service/Service.hpp>


@@ 39,7 40,8 @@ namespace bluetooth
       public:
         explicit CommandHandler(sys::Service *service,
                                 std::shared_ptr<bluetooth::SettingsHolder> settings,
-                                std::shared_ptr<bluetooth::Profile> currentProfile);
+                                std::shared_ptr<bluetooth::Profile> currentProfile,
+                                std::shared_ptr<bluetooth::Driver> driver);
 
         auto handle(Command command) -> Error::Code override;
 


@@ 54,5 56,6 @@ namespace bluetooth
         sys::Service *service;
         std::shared_ptr<bluetooth::SettingsHolder> settings;
         std::shared_ptr<bluetooth::Profile> currentProfile;
+        std::shared_ptr<Driver> driver;
     };
 } // namespace bluetooth

@@ 1,4 1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once


@@ 35,7 35,7 @@ struct Devicei : public Device
     Devicei(std::string name = "") : Device(std::move(name))
     {}
     ~Devicei() override = default;
-    void address_set(bd_addr_t *addr)
+    void setAddress(bd_addr_t *addr)
     {
         memcpy(&address, addr, sizeof address);
     }

@@ 7,6 7,7 @@
 #include "interface/profiles/Profile.hpp"
 
 #include <module-utils/log/log.hpp>
+#define BOOST_SML_CFG_DISABLE_MIN_SIZE // GCC10 fix
 #include <module-utils/sml/include/boost/sml.hpp>
 #include <module-utils/magic_enum/include/magic_enum.hpp>
 

@@ 53,7 53,8 @@ namespace bluetooth
     }
 #endif
 
-    Driver::Driver(const btstack_run_loop *runLoop) : runLoop{runLoop}
+    Driver::Driver(const btstack_run_loop *runLoop, sys::Service *ownerService)
+        : runLoop{runLoop}, gap{std::make_unique<bluetooth::GAP>(ownerService)}
     {}
 
     auto Driver::init() -> Error::Code


@@ 187,4 188,20 @@ namespace bluetooth
         hci_close();
         return ret != 0 ? Error::LibraryError : Error::Success;
     }
+    auto Driver::scan() -> Error
+    {
+        return gap->scan();
+    }
+    void Driver::stopScan()
+    {
+        gap->stopScan();
+    }
+    void Driver::setVisibility(bool visibility)
+    {
+        gap->setVisibility(visibility);
+    }
+    auto Driver::pair(uint8_t *addr, std::uint8_t protectionLevel) -> bool
+    {
+        return gap->pair(addr, protectionLevel);
+    }
 } // namespace bluetooth

@@ 5,6 5,7 @@
 
 #include <btstack.h>
 #include <functional>
+#include "GAP/GAP.hpp"
 #include "Error.hpp"
 
 namespace bluetooth


@@ 27,6 28,7 @@ namespace bluetooth
         static hci_transport_config_uart_t config;
         const btstack_run_loop *runLoop;
         btstack_packet_callback_registration_t hci_event_callback_registration;
+        std::unique_ptr<bluetooth::GAP> gap;
         static void hci_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
         static void local_version_information_handler(uint8_t *packet);
 #ifdef TARGET_RT1051


@@ 36,11 38,15 @@ namespace bluetooth
 #endif
 
       public:
-        explicit Driver(const btstack_run_loop *runLoop);
+        Driver(const btstack_run_loop *runLoop, sys::Service *ownerService);
 
         [[nodiscard]] auto init() -> Error::Code override;
         [[nodiscard]] auto run() -> Error::Code override;
         [[nodiscard]] auto stop() -> Error::Code override;
         void registerErrorCallback(const ErrorCallback &newCallback) override;
+        auto scan() -> Error;
+        void stopScan();
+        void setVisibility(bool visibility);
+        auto pair(uint8_t *addr, std::uint8_t protectionLevel = 0) -> bool;
     };
 } // namespace bluetooth

@@ 1,71 1,37 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
-#include "BluetoothWorker.hpp"
-#include "Device.hpp"
-#include <Bluetooth/Device.hpp>
-#include <Bluetooth/Error.hpp>
+#include "GAP.hpp"
+
 #include <log/log.hpp>
 #include <service-bluetooth/BluetoothMessage.hpp>
-#include <vector>
-#include <BtCommand.hpp>
 
 extern "C"
 {
 #include "btstack.h"
 #include "hci.h"
 };
-
-btstack_packet_callback_registration_t cb_handler;
-
-std::vector<Devicei> bluetooth::GAP::devices;
-static auto start_scan() -> int;
-static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
-
-static auto getDeviceIndexForAddress(std::vector<Devicei> &devs, bd_addr_t addr) -> int
+namespace bluetooth
 {
-    int j;
-    for (j = 0; j < devs.size(); j++) {
-        if (bd_addr_cmp(addr, devs[j].address) == 0) {
-            return j;
-        }
-    }
-    return -1;
-}
+    sys::Service *GAP::ownerService = nullptr;
+    std::vector<Devicei> GAP::devices;
+    btstack_packet_callback_registration_t GAP::cb_handler;
+    ScanState GAP::state;
 
-enum STATE
-{
-    INIT,
-    ACTIVE,
-    DONE
-};
-enum STATE state = INIT;
-
-namespace bluetooth::GAP
-{
-
-    static sys::Service *ownerService = nullptr;
-
-    void setOwnerService(sys::Service *service)
-    {
-        ownerService = service;
-    }
-
-    auto register_scan() -> Error
+    auto GAP::registerScan() -> Error
     {
         LOG_INFO("GAP register scan!");
         /// -> this have to be called prior to power on!
         hci_set_inquiry_mode(INQUIRY_MODE_RSSI_AND_EIR);
-        cb_handler.callback = &packet_handler;
+        cb_handler.callback = &packetHandler;
         hci_add_event_handler(&cb_handler);
         return Error();
     }
 
-    auto scan() -> Error
+    auto GAP::scan() -> Error
     {
-        LOG_INFO("Start scan if active: %d: %d", hci_get_state(), state);
         if (hci_get_state() == HCI_STATE_WORKING) {
-            if (int ret = start_scan(); ret != 0) {
+            if (auto ret = startScan(); ret != 0) {
                 LOG_ERROR("Start scan error!: 0x%X", ret);
                 return Error(Error::LibraryError, ret);
             }


@@ 76,192 42,230 @@ namespace bluetooth::GAP
         return Error();
     }
 
-    void stop_scan()
+    void GAP::stopScan()
     {
         gap_inquiry_force_stop();
+        devices.clear();
         LOG_INFO("Scan stopped!");
     }
 
-    auto set_visibility(bool visibility) -> Error
+    void GAP::setVisibility(bool visibility)
     {
-        gap_discoverable_control(static_cast<uint8_t>(visibility));
+        gap_discoverable_control(static_cast<std::uint8_t>(visibility));
         LOG_INFO("Visibility: %s", visibility ? "true" : "false");
-        return Error();
     }
 
-    auto do_pairing(uint8_t *addr) -> bool
+    auto GAP::pair(std::uint8_t *addr, std::uint8_t protectionLevel) -> bool
     {
         if (hci_get_state() == HCI_STATE_WORKING) {
-            gap_dedicated_bonding(addr, 0);
-            return true;
+            return gap_dedicated_bonding(addr, protectionLevel) == 0;
         }
         return false;
     }
 
-} // namespace bluetooth::GAP
-
-#define INQUIRY_INTERVAL 5
-static auto start_scan() -> int
-{
-    LOG_INFO("Starting inquiry scan..");
-    return gap_inquiry_start(INQUIRY_INTERVAL);
-}
+    auto getDeviceIndexForAddress(const std::vector<Devicei> &devs, bd_addr_t addr) -> int
+    {
+        auto result = std::find_if(std::begin(devs), std::end(devs), [addr](const Devicei &device) {
+            return bd_addr_cmp(addr, device.address) == 0;
+        });
 
-static auto has_more_remote_name_requests() -> int
-{
-    for (int i = 0; i < bluetooth::GAP::devices.size(); i++) {
-        if (bluetooth::GAP::devices[i].state == REMOTE_NAME_REQUEST) {
-            return 1;
+        if (result == std::end(devs)) {
+            return -1;
         }
-    }
-    return 0;
-}
-
-static void do_next_remote_name_request()
-{
 
-    for (auto &device : bluetooth::GAP::devices) {
-        // remote name request
-        if (device.state == REMOTE_NAME_REQUEST) {
-            device.state = REMOTE_NAME_INQUIRED;
-            LOG_INFO("Get remote name of %s...", bd_addr_to_str(device.address));
-            gap_remote_name_request(device.address, device.pageScanRepetitionMode, device.clockOffset | 0x8000);
-            return;
-        }
+        return std::distance(std::begin(devs), result);
     }
-}
 
-static void continue_remote_names()
-{
-    if (has_more_remote_name_requests() != 0) {
-        do_next_remote_name_request();
-        return;
+    void GAP::sendDevices()
+    {
+        auto msg = std::make_shared<BluetoothScanResultMessage>(devices);
+        ownerService->bus.sendUnicast(msg, "ApplicationSettings");
+        ownerService->bus.sendUnicast(std::move(msg), "ApplicationSettingsNew");
     }
-    start_scan();
-}
 
-static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
-{
-    UNUSED(channel);
-    UNUSED(size);
+    auto GAP::startScan() -> int
+    {
+        LOG_INFO("Starting inquiry scan..");
+        return gap_inquiry_start(inquiryIntervalSeconds);
+    }
 
-    bd_addr_t addr;
-    int i;
-    int index;
+    auto GAP::remoteNameToFetch() -> bool
+    {
+        auto result = std::find_if(std::begin(devices), std::end(devices), [](Devicei &device) {
+            return device.state == REMOTE_NAME_REQUEST;
+        });
 
-    if (packet_type != HCI_EVENT_PACKET) {
-        return;
+        return result != std::end(devices);
     }
 
-    uint8_t event = hci_event_packet_get_type(packet);
+    void GAP::fetchRemoteName()
+    {
+        for (auto &device : bluetooth::GAP::devices) {
+            if (device.state == REMOTE_NAME_REQUEST) {
+                device.state = REMOTE_NAME_INQUIRED;
+                LOG_INFO("Get remote name of %s...", bd_addr_to_str(device.address));
+                gap_remote_name_request(device.address, device.pageScanRepetitionMode, device.clockOffset | 0x8000);
+                return;
+            }
+        }
+    }
 
-    switch (state) {
-        /* @text In INIT, an inquiry  scan is started, and the application transits to
-         * ACTIVE state.
-         */
-    case INIT:
-        switch (event) {
-        case BTSTACK_EVENT_STATE:
-            if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING) {
-                state = ACTIVE;
+    void GAP::continueScanning()
+    {
+        if (remoteNameToFetch()) {
+            fetchRemoteName();
+            return;
+        }
+        startScan();
+    }
+    auto GAP::updateDeviceName(std::uint8_t *packet, bd_addr_t &addr) -> bool
+    {
+        reverse_bd_addr(&packet[3], addr);
+        auto index = getDeviceIndexForAddress(devices, addr);
+        if (index >= 0) {
+            if (packet[2] == 0) {
+                LOG_INFO("Name: '%s'", &packet[9]);
+                devices[index].state = REMOTE_NAME_FETCHED;
+                devices[index].name  = std::string{reinterpret_cast<const char *>(&packet[9])};
+                return true;
+            }
+            else {
+                LOG_INFO("Failed to get name: page timeout");
             }
-            break;
-        default:
-            break;
         }
-        break;
+        return false;
+    }
 
-        /* @text In ACTIVE, the following events are processed:
-         *  - GAP Inquiry result event: BTstack provides a unified inquiry result that contain
-         *    Class of Device (CoD), page scan mode, clock offset. RSSI and name (from EIR) are optional.
-         *  - Inquiry complete event: the remote name is requested for devices without a fetched
-         *    name. The state of a remote name can be one of the following:
-         *    REMOTE_NAME_REQUEST, REMOTE_NAME_INQUIRED, or REMOTE_NAME_FETCHED.
-         *  - Remote name request complete event: the remote name is stored in the table and the
-         *    state is updated to REMOTE_NAME_FETCHED. The query of remote names is continued.
-         */
-    case ACTIVE:
-        switch (event) {
+    void GAP::addNewDevice(std::uint8_t *packet, bd_addr_t &addr)
+    {
+        Devicei device;
+        device.setAddress(&addr);
+        device.pageScanRepetitionMode = gap_event_inquiry_result_get_page_scan_repetition_mode(packet);
+        device.clockOffset            = gap_event_inquiry_result_get_clock_offset(packet);
 
-        case GAP_EVENT_INQUIRY_RESULT: {
-            gap_event_inquiry_result_get_bd_addr(packet, addr);
-            index = getDeviceIndexForAddress(bluetooth::GAP::devices, addr);
-            if (index >= 0) {
-                break; // already in our list
-            }
+        LOG_INFO("Device found: %s ", bd_addr_to_str(addr));
+        LOG_INFO("with COD: 0x%06x, ", static_cast<unsigned int>(gap_event_inquiry_result_get_class_of_device(packet)));
+        LOG_INFO("pageScan %d, ", device.pageScanRepetitionMode);
+        LOG_INFO("clock offset 0x%04x", device.clockOffset);
+        if (gap_event_inquiry_result_get_rssi_available(packet) != 0u) {
+            LOG_INFO(", rssi %d dBm", static_cast<int8_t>(gap_event_inquiry_result_get_rssi(packet)));
+        }
+        if (gap_event_inquiry_result_get_name_available(packet) != 0u) {
+            auto name   = gap_event_inquiry_result_get_name(packet);
+            device.name = std::string{reinterpret_cast<const char *>(name)};
 
-            Devicei dev;
-            dev.address_set(&addr);
-            dev.pageScanRepetitionMode = gap_event_inquiry_result_get_page_scan_repetition_mode(packet);
-            dev.clockOffset            = gap_event_inquiry_result_get_clock_offset(packet);
-            // print info
-            LOG_INFO("Device found: %s ", bd_addr_to_str(addr));
-            LOG_INFO("with COD: 0x%06x, ", (unsigned int)gap_event_inquiry_result_get_class_of_device(packet));
-            LOG_INFO("pageScan %d, ", dev.pageScanRepetitionMode);
-            LOG_INFO("clock offset 0x%04x", dev.clockOffset);
-            if (gap_event_inquiry_result_get_rssi_available(packet) != 0u) {
-                LOG_INFO(", rssi %d dBm", (int8_t)gap_event_inquiry_result_get_rssi(packet));
-            }
-            if (gap_event_inquiry_result_get_name_available(packet) != 0u) {
-                char name_buffer[240];
-                int name_len = gap_event_inquiry_result_get_name_len(packet);
-                memcpy(name_buffer, gap_event_inquiry_result_get_name(packet), name_len);
-                name_buffer[name_len] = 0;
-                LOG_INFO(", name '%s'", name_buffer);
-                dev.name  = std::string{name_buffer};
-                dev.state = REMOTE_NAME_FETCHED;
-            }
-            else {
-                dev.state = REMOTE_NAME_REQUEST;
+            LOG_INFO(", name '%s'", device.name.c_str());
+            device.state = REMOTE_NAME_FETCHED;
+        }
+        else {
+            device.state = REMOTE_NAME_REQUEST;
+            device.name  = std::string{};
+        }
+
+        devices.emplace_back(std::move(device));
+    }
+
+    void GAP::processInquiryResult(std::uint8_t *packet, bd_addr_t &addr)
+    {
+        gap_event_inquiry_result_get_bd_addr(packet, addr);
+        auto index = getDeviceIndexForAddress(devices, addr);
+        if (index >= 0) {
+            return; // already in our list
+        }
+        addNewDevice(packet, addr);
+        sendDevices();
+    }
+    void GAP::processInquiryComplete()
+    {
+        for (auto &device : devices) {
+            // retry remote name request
+            if (device.state == REMOTE_NAME_INQUIRED) {
+                device.state = REMOTE_NAME_REQUEST;
             }
-            bluetooth::GAP::devices.push_back(dev);
-            auto msg = std::make_shared<BluetoothScanResultMessage>(bluetooth::GAP::devices);
-            bluetooth::GAP::ownerService->bus.sendUnicast(msg, "ApplicationSettings");
-            bluetooth::GAP::ownerService->bus.sendUnicast(msg, "ApplicationSettingsNew");
-        } break;
+        }
+        continueScanning();
+    }
+    void GAP::processNameRequestComplete(std::uint8_t *packet, bd_addr_t &addr)
+    {
+        if (updateDeviceName(packet, addr)) {
+            sendDevices();
+        }
+        continueScanning();
+    }
+    void GAP::processDedicatedBondingCompleted(std::uint8_t *packet)
+    {
+        auto result = packet[2];
+        auto msg    = std::make_shared<BluetoothPairResultMessage>(result == 0u);
+        ownerService->bus.sendUnicast(msg, "ApplicationSettings");
+        ownerService->bus.sendUnicast(std::move(msg), "ApplicationSettingsNew");
+    }
+    /* @text In ACTIVE, the following events are processed:
+     *  - GAP Inquiry result event: BTstack provides a unified inquiry result that contain
+     *    Class of Device (CoD), page scan mode, clock offset. RSSI and name (from EIR) are optional.
+     *  - Inquiry complete event: the remote name is requested for devices without a fetched
+     *    name. The state of a remote name can be one of the following:
+     *    REMOTE_NAME_REQUEST, REMOTE_NAME_INQUIRED, or REMOTE_NAME_FETCHED.
+     *  - Remote name request complete event: the remote name is stored in the table and the
+     *    state is updated to REMOTE_NAME_FETCHED. The query of remote names is continued.
+     */
+    void GAP::activeStateHandler(std::uint8_t eventType, std::uint8_t *packet, bd_addr_t &addr)
+    {
+        switch (eventType) {
+
+        case GAP_EVENT_INQUIRY_RESULT:
+            processInquiryResult(packet, addr);
+            break;
 
         case GAP_EVENT_INQUIRY_COMPLETE:
-            for (i = 0; i < bluetooth::GAP::devices.size(); i++) {
-                // retry remote name request
-                if (bluetooth::GAP::devices[i].state == REMOTE_NAME_INQUIRED)
-                    bluetooth::GAP::devices[i].state = REMOTE_NAME_REQUEST;
-            }
-            continue_remote_names();
+            processInquiryComplete();
             break;
 
-        case HCI_EVENT_REMOTE_NAME_REQUEST_COMPLETE: {
-            reverse_bd_addr(&packet[3], addr);
-            index = getDeviceIndexForAddress(bluetooth::GAP::devices, addr);
-            if (index >= 0) {
-                if (packet[2] == 0) {
-                    LOG_INFO("Name: '%s'", &packet[9]);
-                    bluetooth::GAP::devices[index].state = REMOTE_NAME_FETCHED;
-                    bluetooth::GAP::devices[index].name  = reinterpret_cast<char *>(&packet[9]);
-                }
-                else {
-                    LOG_INFO("Failed to get name: page timeout");
-                }
-            }
-            if (index + 1 == bluetooth::GAP::devices.size()) {
-                LOG_INFO("Scanned all");
-                state = DONE;
-                gap_inquiry_stop();
-                break;
-            }
-            continue_remote_names();
-        } break;
-        case GAP_EVENT_DEDICATED_BONDING_COMPLETED: {
-            auto result = packet[2];
-            auto msg    = std::make_shared<BluetoothPairResultMessage>(result == 0u);
-            bluetooth::GAP::ownerService->bus.sendUnicast(msg, "ApplicationSettings");
-            bluetooth::GAP::ownerService->bus.sendUnicast(msg, "ApplicationSettingsNew");
-        } break;
+        case HCI_EVENT_REMOTE_NAME_REQUEST_COMPLETE:
+            processNameRequestComplete(packet, addr);
+            break;
+        case GAP_EVENT_DEDICATED_BONDING_COMPLETED:
+            processDedicatedBondingCompleted(packet);
+            break;
         default:
             break;
         }
-        break;
+    }
+    void GAP::initStateHandler(std::uint8_t eventType, std::uint8_t *packet)
+    {
+        if (eventType == BTSTACK_EVENT_STATE) {
+            if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING) {
+                state = ScanState::active;
+            }
+        }
+    }
+    void GAP::packetHandler(std::uint8_t packet_type, std::uint16_t channel, std::uint8_t *packet, std::uint16_t size)
+    {
+        bd_addr_t addr;
+
+        if (packet_type != HCI_EVENT_PACKET) {
+            return;
+        }
 
-    default:
-        break;
+        const auto eventType = hci_event_packet_get_type(packet);
+        switch (state) {
+        case ScanState::init:
+            initStateHandler(eventType, packet);
+            break;
+        case ScanState::active:
+            activeStateHandler(eventType, packet, addr);
+            break;
+        default:
+            break;
+        }
+    }
+    GAP::GAP(sys::Service *owner)
+    {
+        ownerService = owner;
+        state        = ScanState::init;
     }
-}
+    auto GAP::getDevicesList() -> const std::vector<Devicei> &
+    {
+        return devices;
+    }
+
+} // namespace bluetooth

@@ 0,0 1,56 @@
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+#include <vector>
+#include <Bluetooth/Device.hpp>
+#include <Bluetooth/Error.hpp>
+#include <Service/Service.hpp>
+extern "C"
+{
+#include "btstack_defines.h"
+}
+namespace bluetooth
+{
+    enum ScanState
+    {
+        init,
+        active,
+        done
+    };
+    class GAP
+    {
+        static std::vector<Devicei> devices;
+        static sys::Service *ownerService;
+        static btstack_packet_callback_registration_t cb_handler;
+        static constexpr auto inquiryIntervalSeconds = 5;
+        static ScanState state;
+        static void sendDevices();
+        static auto startScan() -> int;
+        static auto remoteNameToFetch() -> bool;
+        static void fetchRemoteName();
+        static void continueScanning();
+        static auto updateDeviceName(std::uint8_t *packet, bd_addr_t &addr) -> bool;
+        static void addNewDevice(std::uint8_t *packet, bd_addr_t &addr);
+        static void activeStateHandler(std::uint8_t eventType, std::uint8_t *packet, bd_addr_t &addr);
+        static void packetHandler(std::uint8_t packet_type,
+                                  std::uint16_t channel,
+                                  std::uint8_t *packet,
+                                  std::uint16_t size);
+        static void processInquiryResult(std::uint8_t *packet, bd_addr_t &addr);
+        static void processInquiryComplete();
+        static void processNameRequestComplete(std::uint8_t *packet, bd_addr_t &addr);
+        static void processDedicatedBondingCompleted(std::uint8_t *packet);
+        static void initStateHandler(std::uint8_t eventType, std::uint8_t *packet);
+
+      public:
+        /// THIS have to be called prior to Bt system start!
+        static auto registerScan() -> Error;
+        auto scan() -> Error;
+        void stopScan();
+        void setVisibility(bool visibility);
+        auto pair(uint8_t *addr, std::uint8_t protectionLevel = 0) -> bool;
+        static auto getDevicesList() -> const std::vector<Devicei> &;
+        explicit GAP(sys::Service *owner);
+    };
+} // namespace bluetooth

@@ 11,6 11,7 @@ set(SOURCES
     ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/glucode/BluetoothRunLoop.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/interface/BluetoothDriver.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/BtKeysStorage.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/interface/profiles/GAP/GAP.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/interface/profiles/A2DP/A2DP.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/interface/profiles/A2DP/AVRCP.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/interface/profiles/A2DP/AVDTP.cpp

@@ 140,7 140,6 @@ list(APPEND TARGET_LIBRARIES_INCLUDES
     ${BT_STACK_ROOT}/platform/lwip
     )
 set(BOARD_DIR_SOURCES
-    ${BT_INT}/GAP.cpp
     ${BT_INT}/PAN.cpp
 
     ${BT_GLU}/bluetooth_init_cc2564C_1.0.c

@@ 10,8 10,6 @@
 #define _STRINGIFY(s) #s
 #define STRINGIFY(s)  _STRINGIFY(s)
 
-#define UNUSED(x) ((void)(x))
-
 #define ALIGN_(n) __attribute__((aligned(n)))
 
 #define CACHEABLE_SECTION_SDRAM(var) __attribute__((section(".sdram"))) var