17 files changed, 692 insertions(+), 87 deletions(-)

M .gitmodules
M changelog.md
M module-bluetooth/Bluetooth/BluetoothWorker.cpp
M module-bluetooth/Bluetooth/BluetoothWorker.hpp
M module-bluetooth/Bluetooth/Device.hpp
M module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DP.cpp
M module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DP.hpp
M module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DPImpl.hpp
A module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.cpp
A module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.hpp
A module-bluetooth/Bluetooth/interface/profiles/HSP/HSPImpl.hpp
A module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.cpp
A module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.hpp
M module-bluetooth/Bluetooth/interface/profiles/Profile.hpp
M module-bluetooth/CMakeLists.txt
M module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.cpp
M module-services/service-bluetooth/ServiceBluetooth.cpp

@@ 59,3 59,4 @@
 [submodule "module-utils/tinyexpr"]
 	path = module-utils/tinyexpr
 	url = https://github.com/codeplea/tinyexpr.git
+

@@ 9,6 9,8 @@
 * `[messages][db]` File Indexer db Agent
 * `[testing]` Added key release simulation through service-desktop developerMode
 * `[cellular]` Set pin functionality with Action mockup
+* `[bluetooth]` Added HSP playback and recording
+
 ### Changed
 
 * `[cellular]` Handled properly SIM READY and SIM PIN URC messages with Action mockup

@@ 4,8 4,8 @@
 #include "BluetoothWorker.hpp"
 #include "BtCommand.hpp"
 #include "log/log.hpp"
-#include "module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DP.hpp"
-
+#include "interface/profiles/A2DP/A2DP.hpp"
+#include "interface/profiles/HSP/HSP.hpp"
 extern "C"
 {
 #include "module-bluetooth/lib/btstack/src/btstack_util.h"


@@ 28,7 28,7 @@ const char *c_str(Bt::Error::Code code)
     return "";
 }
 
-BluetoothWorker::BluetoothWorker(sys::Service *service) : Worker(service), currentProfile(std::make_shared<Bt::A2DP>())
+BluetoothWorker::BluetoothWorker(sys::Service *service) : Worker(service), currentProfile(std::make_shared<Bt::HSP>())
 {
     init({
         {"qBtIO", sizeof(Bt::Message), 10},


@@ 86,18 86,18 @@ bool BluetoothWorker::scan()
     }
 }
 
-void BluetoothWorker::stop_scan()
+void BluetoothWorker::stopScan()
 {
     Bt::GAP::stop_scan();
 }
 
-bool BluetoothWorker::set_visible()
+bool BluetoothWorker::toggleVisibility()
 {
     static bool visibility = true;
     Bt::GAP::set_visibility(visibility);
     visibility = !visibility;
 
-    return false;
+    return visibility;
 }
 
 bool BluetoothWorker::start_pan()


@@ 110,16 110,6 @@ bool BluetoothWorker::start_pan()
     return false;
 }
 
-BluetoothWorker::Error BluetoothWorker::aud_init()
-{
-    LOG_INFO("%s", __PRETTY_FUNCTION__);
-    Error err = SuccessBt;
-    LOG_INFO("AUDIO - TODO");
-    // start GAVD
-    // &&  ASSIGN_CLASS_OF_DEVICE(ClassOfDevice, 0x28, 0x04, 0x10);
-    return err;
-}
-
 #include <sstream>
 
 bool BluetoothWorker::handleMessage(uint32_t queueID)


@@ 189,36 179,22 @@ bool BluetoothWorker::handleMessage(uint32_t queueID)
     return true;
 }
 
-void BluetoothWorker::initAudioBT()
-{}
-
-bool BluetoothWorker::play_audio()
+bool BluetoothWorker::establishAudioConnection()
 {
-    auto profile = dynamic_cast<Bt::A2DP *>(currentProfile.get());
-    if (profile == nullptr) {
+    currentProfile->setOwnerService(service);
+    if (currentProfile->init() != Bt::Error::Success) {
         return false;
     }
-
-    profile->init();
-    profile->setOwnerService(service);
-    profile->start();
+    currentProfile->connect();
     return true;
 }
-bool BluetoothWorker::stop_audio()
+bool BluetoothWorker::disconnectAudioConnection()
 {
-    auto profile = dynamic_cast<Bt::A2DP *>(currentProfile.get());
-    if (profile == nullptr) {
-        return false;
-    }
-
-    profile->stop();
+    currentProfile->disconnect();
     return true;
 }
-void BluetoothWorker::set_addr(bd_addr_t addr)
+void BluetoothWorker::setDeviceAddress(bd_addr_t addr)
 {
     Bt::GAP::do_pairing(addr);
-    auto profile = dynamic_cast<Bt::A2DP *>(currentProfile.get());
-    if (profile != nullptr) {
-        profile->setDeviceAddress(addr);
-    }
+    currentProfile->setDeviceAddress(addr);
 }

@@ 92,19 92,19 @@ class BluetoothWorker : private sys::Worker
 
     bool scan();
 
-    bool set_visible();
+    bool toggleVisibility();
 
     bool start_pan();
 
-    bool play_audio();
+    bool establishAudioConnection();
 
-    bool stop_audio();
+    bool disconnectAudioConnection();
 
     Error aud_init();
     /// bluetooth stack id in use
     unsigned long active_features;
-    void stop_scan();
-    void set_addr(bd_addr_t addr);
+    void stopScan();
+    void setDeviceAddress(bd_addr_t addr);
     void initAudioBT();
 
     std::shared_ptr<Bt::Profile> currentProfile;

@@ 53,4 53,5 @@ constexpr unsigned int DATA_BUFFER_SIZE = 256 * 2;
 struct AudioData_t
 {
     std::array<int16_t, DATA_BUFFER_SIZE> data;
+    uint16_t bytesSent;
 };

@@ 12,11 12,11 @@
 #include <Bluetooth/Device.hpp>
 #include <Bluetooth/Error.hpp>
 #include <log/log.hpp>
-#include <module-sys/Service/Bus.hpp>
+#include <Service/Bus.hpp>
 #include <service-bluetooth/messages/BluetoothMessage.hpp>
-#include <module-audio/Audio/AudioCommon.hpp>
-#include <module-services/service-audio/messages/AudioMessage.hpp>
-#include <module-services/service-evtmgr/Constants.hpp>
+#include <Audio/AudioCommon.hpp>
+#include <service-audio/messages/AudioMessage.hpp>
+#include <service-evtmgr/Constants.hpp>
 extern "C"
 {
 #include "module-bluetooth/lib/btstack/src/btstack.h"


@@ 55,32 55,37 @@ namespace Bt
         return *this;
     }
 
-    auto A2DP::init() -> Error
+    auto A2DP::init() -> Error::Code
     {
         return pimpl->init();
     }
-    void A2DP::start()
-    {
-        pimpl->start();
-    }
-    void A2DP::stop()
-    {
-        pimpl->stop();
-    }
+
     void A2DP::setDeviceAddress(uint8_t *addr)
     {
         pimpl->setDeviceAddress(addr);
     }
-    void A2DP::setOwnerService(sys::Service *service)
+
+    void A2DP::setOwnerService(const sys::Service *service)
     {
         pimpl->setOwnerService(service);
     }
+
     auto A2DP::getStreamData() -> std::shared_ptr<BluetoothStreamData>
     {
         return pimpl->getStreamData();
     }
 
-    sys::Service *A2DP::A2DPImpl::ownerService;
+    void A2DP::connect()
+    {
+        pimpl->start();
+    }
+
+    void A2DP::disconnect()
+    {
+        pimpl->stop();
+    }
+
+    const sys::Service *A2DP::A2DPImpl::ownerService;
     QueueHandle_t A2DP::A2DPImpl::sourceQueue = nullptr;
     QueueHandle_t A2DP::A2DPImpl::sinkQueue   = nullptr;
     bd_addr_t A2DP::A2DPImpl::deviceAddr;


@@ 95,7 100,7 @@ namespace Bt
 
     /* LISTING_START(MainConfiguration): Setup Audio Source and AVRCP Target services */
 
-    auto A2DP::A2DPImpl::init() -> Error
+    auto A2DP::A2DPImpl::init() -> Error::Code
     {
         // request role change on reconnecting headset to always use them in slave mode
         hci_set_master_slave_policy(0);


@@ 115,7 120,7 @@ namespace Bt
                                                AVDTP::sbcCodecConfiguration.size());
         if (local_stream_endpoint == nullptr) {
             LOG_INFO("A2DP Source: not enough memory to create local stream endpoint\n");
-            return Error(Bt::Error::SystemError);
+            return Bt::Error::SystemError;
         }
         AVRCP::mediaTracker.local_seid = avdtp_local_seid(local_stream_endpoint);
         avdtp_source_register_delay_reporting_category(AVRCP::mediaTracker.local_seid);


@@ 160,7 165,7 @@ namespace Bt
 
         LOG_INFO("Init done!");
 
-        return Error();
+        return Bt::Error::Success;
     }
 
     void A2DP::A2DPImpl::sendMediaPacket()


@@ 450,9 455,7 @@ namespace Bt
             sourceQueue = xQueueCreate(5, sizeof(AudioData_t));
             sinkQueue   = nullptr;
             if (sourceQueue != nullptr) {
-                auto event = std::make_shared<audio::Event>(audio::EventType::BTA2DPOn);
-                auto msg   = std::make_shared<AudioEventRequest>(std::move(event));
-                sys::Bus::SendUnicast(std::move(msg), service::name::evt_manager, ownerService);
+                sendAudioEvent(audio::EventType::BTA2DPOn);
             }
             else {
                 LOG_ERROR("failed to create queue!");


@@ 534,11 537,7 @@ namespace Bt
                 cid,
                 AVRCP::mediaTracker.local_seid,
                 local_seid);
-            {
-                auto event = std::make_shared<audio::Event>(audio::EventType::BTA2DPOff);
-                auto msg   = std::make_shared<AudioEventRequest>(std::move(event));
-                sys::Bus::SendUnicast(std::move(msg), service::name::evt_manager, ownerService);
-            }
+            sendAudioEvent(audio::EventType::BTA2DPOff);
             if (cid == AVRCP::mediaTracker.a2dp_cid) {
                 AVRCP::mediaTracker.stream_opened = 0;
                 LOG_INFO("A2DP Source: Stream released.\n");


@@ 581,13 580,22 @@ namespace Bt
         bd_addr_copy(deviceAddr, addr);
         LOG_INFO("Address set!");
     }
-    void A2DP::A2DPImpl::setOwnerService(sys::Service *service)
+
+    void A2DP::A2DPImpl::setOwnerService(const sys::Service *service)
     {
         ownerService = service;
     }
+
     auto A2DP::A2DPImpl::getStreamData() -> std::shared_ptr<BluetoothStreamData>
     {
         return std::make_shared<BluetoothStreamData>(sinkQueue, sourceQueue, metadata);
     }
 
+    void A2DP::A2DPImpl::sendAudioEvent(audio::EventType event)
+    {
+        auto evt = std::make_shared<audio::Event>(event);
+        auto msg = std::make_shared<AudioEventRequest>(std::move(evt));
+        sys::Bus::SendUnicast(std::move(msg), service::name::evt_manager, const_cast<sys::Service *>(ownerService));
+    }
+
 } // namespace Bt

@@ 8,7 8,7 @@
 #include "Service/Service.hpp"
 #include "queue.h"
 #include <memory>
-#include <module-services/service-bluetooth/ServiceBluetoothCommon.hpp>
+#include <service-bluetooth/ServiceBluetoothCommon.hpp>
 namespace Bt
 {
     class A2DP : public Profile


@@ 22,13 22,13 @@ namespace Bt
         A2DP(A2DP &&other) noexcept;
         auto operator=(A2DP &&other) noexcept -> A2DP &;
 
-        auto init() -> Error override;
+        auto init() -> Error::Code override;
         void setDeviceAddress(uint8_t *addr) override;
-        void setOwnerService(sys::Service *service);
-        auto getStreamData() -> std::shared_ptr<BluetoothStreamData> override;
+        void setOwnerService(const sys::Service *service) override;
+        [[nodiscard]] auto getStreamData() -> std::shared_ptr<BluetoothStreamData> override;
 
-        void start();
-        void stop();
+        void connect() override;
+        void disconnect() override;
 
       private:
         class A2DPImpl;

@@ 7,6 7,8 @@
 #include <Bluetooth/Error.hpp>
 #include <BtCommand.hpp>
 #include <log/log.hpp>
+#include <Audio/AudioCommon.hpp>
+
 extern "C"
 {
 #include <btstack.h>


@@ 53,7 55,7 @@ namespace Bt
         static bd_addr_t deviceAddr;
         static btstack_packet_callback_registration_t hciEventCallbackRegistration;
         static std::array<uint8_t, MEDIA_CAP_SIZE> mediaSbcCodecCapabilities;
-        static sys::Service *ownerService;
+        static const sys::Service *ownerService;
         static QueueHandle_t sourceQueue;
         static QueueHandle_t sinkQueue;
         static DeviceMetadata_t metadata;


@@ 65,13 67,14 @@ namespace Bt
         static void hciPacketHandler(uint8_t packetType, uint16_t channel, uint8_t *packet, uint16_t size);
         static void sendMediaPacket();
         static auto fillSbcAudioBuffer(MediaContext *context) -> int;
+        static void sendAudioEvent(audio::EventType event);
 
       public:
-        auto init() -> Error;
+        auto init() -> Error::Code;
         void start();
         void stop();
         void setDeviceAddress(bd_addr_t addr);
-        void setOwnerService(sys::Service *service);
+        void setOwnerService(const sys::Service *service);
         auto getStreamData() -> std::shared_ptr<BluetoothStreamData>;
     };
 } // namespace Bt

@@ 0,0 1,237 @@
+// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "HSPImpl.hpp"
+#include "HSP.hpp"
+
+#include <Bluetooth/Error.hpp>
+#include <log/log.hpp>
+#include <service-evtmgr/Constants.hpp>
+#include <service-audio/messages/AudioMessage.hpp>
+
+extern "C"
+{
+#include "btstack.h"
+#include "btstack_run_loop_freertos.h"
+#include "btstack_stdin.h"
+#include <btstack_defines.h>
+}
+
+namespace Bt
+{
+    HSP::HSP() : pimpl(std::make_unique<HSPImpl>(HSPImpl()))
+    {}
+
+    HSP::HSP(HSP &&other) noexcept : pimpl(std::move(other.pimpl))
+    {}
+
+    auto HSP::operator=(HSP &&other) noexcept -> HSP &
+    {
+        if (&other == this) {
+            return *this;
+        }
+        pimpl = std::move(other.pimpl);
+        return *this;
+    }
+
+    auto HSP::init() -> Error::Code
+    {
+        return pimpl->init();
+    }
+
+    void HSP::setDeviceAddress(uint8_t *addr)
+    {
+        pimpl->setDeviceAddress(addr);
+    }
+
+    void HSP::setOwnerService(const sys::Service *service)
+    {
+        ownerService = service;
+        pimpl->setOwnerService(service);
+    }
+
+    auto HSP::getStreamData() -> std::shared_ptr<BluetoothStreamData>
+    {
+        return pimpl->getStreamData();
+    }
+
+    void HSP::connect()
+    {
+        pimpl->start();
+    }
+
+    void HSP::disconnect()
+    {
+        pimpl->stop();
+    }
+
+    HSP::~HSP() = default;
+
+    uint16_t HSP::HSPImpl::scoHandle = HCI_CON_HANDLE_INVALID;
+    bd_addr_t HSP::HSPImpl::deviceAddr;
+    std::array<char, commandBufferLength> HSP::HSPImpl::ATcommandBuffer;
+    std::array<uint8_t, serviceBufferLength> HSP::HSPImpl::serviceBuffer;
+    std::unique_ptr<SCO> HSP::HSPImpl::sco;
+    const sys::Service *HSP::HSPImpl::ownerService;
+    std::string HSP::HSPImpl::agServiceName = "PurePhone HSP";
+
+    void HSP::HSPImpl::sendAudioEvent(audio::EventType event)
+    {
+        auto evt = std::make_shared<audio::Event>(event);
+        auto msg = std::make_shared<AudioEventRequest>(std::move(evt));
+        sys::Bus::SendUnicast(std::move(msg), service::name::evt_manager, const_cast<sys::Service *>(ownerService));
+    }
+
+    void HSP::HSPImpl::packetHandler(uint8_t packetType, uint16_t channel, uint8_t *event, uint16_t eventSize)
+    {
+        switch (packetType) {
+        case HCI_SCO_DATA_PACKET:
+            if (READ_SCO_CONNECTION_HANDLE(event) != scoHandle) {
+                break;
+            }
+            sco->receive(event, eventSize);
+            break;
+
+        case HCI_EVENT_PACKET:
+            processHCIEvent(event);
+            break;
+        default:
+            break;
+        }
+    }
+
+    void HSP::HSPImpl::processHCIEvent(uint8_t *event)
+    {
+        switch (hci_event_packet_get_type(event)) {
+        case HCI_EVENT_SCO_CAN_SEND_NOW:
+            sco->send(scoHandle);
+            break;
+        case HCI_EVENT_HSP_META:
+            processHSPEvent(event);
+            break;
+        default:
+            break;
+        }
+    }
+
+    void HSP::HSPImpl::processHSPEvent(uint8_t *event)
+    {
+        auto eventDescriptor = event[2];
+        switch (eventDescriptor) {
+        case HSP_SUBEVENT_RFCOMM_CONNECTION_COMPLETE:
+            if (hsp_subevent_rfcomm_connection_complete_get_status(event) != 0u) {
+                LOG_DEBUG("RFCOMM connection establishement failed with status %u\n",
+                          hsp_subevent_rfcomm_connection_complete_get_status(event));
+                break;
+            }
+            LOG_DEBUG("RFCOMM connection established.\n");
+            LOG_DEBUG("Establish Audio connection to %s...\n", bd_addr_to_str(deviceAddr));
+            hsp_ag_establish_audio_connection();
+            break;
+        case HSP_SUBEVENT_RFCOMM_DISCONNECTION_COMPLETE:
+            if (hsp_subevent_rfcomm_disconnection_complete_get_status(event) != 0u) {
+                LOG_DEBUG("RFCOMM disconnection failed with status %u.\n",
+                          hsp_subevent_rfcomm_disconnection_complete_get_status(event));
+            }
+            else {
+                LOG_DEBUG("RFCOMM disconnected.\n");
+                sendAudioEvent(audio::EventType::BTHeadsetOff);
+            }
+            break;
+        case HSP_SUBEVENT_AUDIO_CONNECTION_COMPLETE:
+            if (hsp_subevent_audio_connection_complete_get_status(event) != 0u) {
+                LOG_DEBUG("Audio connection establishment failed with status %u\n",
+                          hsp_subevent_audio_connection_complete_get_status(event));
+            }
+            else {
+                scoHandle = hsp_subevent_audio_connection_complete_get_handle(event);
+                LOG_DEBUG("Audio connection established with SCO handle 0x%04x.\n", scoHandle);
+                hci_request_sco_can_send_now_event();
+                btstack_run_loop_freertos_trigger();
+            }
+            break;
+        case HSP_SUBEVENT_AUDIO_DISCONNECTION_COMPLETE:
+            LOG_DEBUG("Audio connection released.\n\n");
+            sendAudioEvent(audio::EventType::BTHeadsetOff);
+            scoHandle = HCI_CON_HANDLE_INVALID;
+            break;
+        case HSP_SUBEVENT_MICROPHONE_GAIN_CHANGED:
+            LOG_DEBUG("Received microphone gain change %d\n", hsp_subevent_microphone_gain_changed_get_gain(event));
+            break;
+        case HSP_SUBEVENT_SPEAKER_GAIN_CHANGED:
+            LOG_DEBUG("Received speaker gain change %d\n", hsp_subevent_speaker_gain_changed_get_gain(event));
+            break;
+        case HSP_SUBEVENT_HS_COMMAND: {
+            ATcommandBuffer.fill(0);
+            auto cmd_length   = hsp_subevent_hs_command_get_value_length(event);
+            auto size         = cmd_length <= ATcommandBuffer.size() ? cmd_length : ATcommandBuffer.size();
+            auto commandValue = hsp_subevent_hs_command_get_value(event);
+            memcpy(ATcommandBuffer.data(), commandValue, size - 1);
+            LOG_DEBUG("Received custom command: \"%s\". \nExit code or call hsp_ag_send_result.\n",
+                      ATcommandBuffer.data());
+            break;
+        }
+        default:
+            LOG_DEBUG("event not handled %u\n", event[2]);
+            break;
+        }
+    }
+
+    auto HSP::HSPImpl::init() -> Error::Code
+    {
+        sco = std::make_unique<SCO>();
+        sco->setOwnerService(ownerService);
+        sco->init();
+
+        l2cap_init();
+        sdp_init();
+
+        serviceBuffer.fill(0);
+        hsp_ag_create_sdp_record(serviceBuffer.data(), 0x10001, rfcommChannelNr, agServiceName.c_str());
+        sdp_register_service(serviceBuffer.data());
+
+        rfcomm_init();
+
+        hsp_ag_init(rfcommChannelNr);
+        hsp_ag_register_packet_handler(&packetHandler);
+
+        // register for SCO packets
+        hci_register_sco_packet_handler(&packetHandler);
+
+        gap_set_local_name("PurePhone");
+        gap_discoverable_control(1);
+        gap_set_class_of_device(CLASS_OF_DEVICE);
+
+        LOG_INFO("HSP init done!");
+
+        return Bt::Error::Success;
+    }
+
+    void HSP::HSPImpl::start()
+    {
+        hsp_ag_connect(deviceAddr);
+    }
+
+    void HSP::HSPImpl::stop()
+    {
+        hsp_ag_release_audio_connection();
+        hsp_ag_disconnect();
+    }
+
+    void HSP::HSPImpl::setDeviceAddress(bd_addr_t addr)
+    {
+        bd_addr_copy(deviceAddr, addr);
+        LOG_INFO("Address set!");
+    }
+
+    void HSP::HSPImpl::setOwnerService(const sys::Service *service)
+    {
+        ownerService = service;
+    }
+
+    auto HSP::HSPImpl::getStreamData() -> std::shared_ptr<BluetoothStreamData>
+    {
+        return sco->getStreamData();
+    }
+
+} // namespace Bt

@@ 0,0 1,39 @@
+// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "Profile.hpp"
+#include <service-bluetooth/messages/BluetoothMessage.hpp>
+
+namespace Bt
+{
+
+    class HSP : public Profile
+    {
+        static constexpr auto CLASS_OF_DEVICE = 0x400204;
+        // Service class: Telephony, Major device class: Phone, Minor device class: Cellular
+      public:
+        HSP();
+        ~HSP() override;
+
+        HSP(const HSP &other) = delete;
+        auto operator=(const HSP &rhs) -> HSP & = delete;
+        HSP(HSP &&other) noexcept;
+        auto operator=(HSP &&other) noexcept -> HSP &;
+
+        auto init() -> Error::Code override;
+        void setDeviceAddress(uint8_t *addr) override;
+        auto getStreamData() -> std::shared_ptr<BluetoothStreamData> override;
+        void setOwnerService(const sys::Service *service) override;
+
+        void connect() override;
+        void disconnect() override;
+
+      private:
+        class HSPImpl;
+        std::unique_ptr<HSPImpl> pimpl;
+        const sys::Service *ownerService;
+    };
+
+} // namespace Bt

@@ 0,0 1,39 @@
+// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+#include "HSP.hpp"
+#include "Error.hpp"
+#include "SCO.hpp"
+#include <Audio/AudioCommon.hpp>
+
+namespace Bt
+{
+    static constexpr int serviceBufferLength = 150;
+    static constexpr int commandBufferLength = 150;
+
+    class HSP::HSPImpl
+    {
+      public:
+        static void packetHandler(uint8_t packetType, uint16_t channel, uint8_t *event, uint16_t eventSize);
+        auto init() -> Error::Code;
+        void start();
+        void stop();
+        void setDeviceAddress(bd_addr_t addr);
+        void setOwnerService(const sys::Service *service);
+        auto getStreamData() -> std::shared_ptr<BluetoothStreamData>;
+
+      private:
+        static void sendAudioEvent(audio::EventType event);
+        static void processHCIEvent(uint8_t *event);
+        static void processHSPEvent(uint8_t *event);
+        static std::array<uint8_t, serviceBufferLength> serviceBuffer;
+        static constexpr uint8_t rfcommChannelNr = 1;
+        static std::string agServiceName;
+        static uint16_t scoHandle;
+        static std::unique_ptr<SCO> sco;
+        static std::array<char, commandBufferLength> ATcommandBuffer;
+        static bd_addr_t deviceAddr;
+        static const sys::Service *ownerService;
+    };
+} // namespace Bt

@@ 0,0 1,249 @@
+// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "SCO.hpp"
+#include <cstdio>
+#include <Audio/AudioCommon.hpp>
+#include <service-evtmgr/Constants.hpp>
+#include <service-audio/messages/AudioMessage.hpp>
+
+extern "C"
+{
+#include "btstack_audio.h"
+#include "btstack_debug.h"
+#include "btstack_ring_buffer.h"
+#include "classic/btstack_cvsd_plc.h"
+#include "classic/btstack_sbc.h"
+#include "classic/hfp.h"
+#include "classic/hfp_msbc.h"
+#include "hci.h"
+}
+
+namespace Bt
+{
+
+    class SCO::SCOImpl
+    {
+      public:
+        static void init();
+        static void send(hci_con_handle_t scoHandle);
+        static void receive(uint8_t *packet, uint16_t size);
+        void setOwnerService(const sys::Service *service);
+        auto getStreamData() -> std::shared_ptr<BluetoothStreamData>;
+
+      private:
+        static constexpr auto CVSD_SAMPLE_RATE    = 8000;
+        static constexpr auto BYTES_PER_FRAME     = 2;
+        static constexpr auto ALL_GOOD_MASK       = 0x30;
+        static constexpr auto AUDIO_BUFFER_LENGTH = 128;
+        static constexpr auto PACKET_DATA_OFFSET  = 3;
+        static constexpr auto VOICE_SETTING_CVSD  = 0x60; // linear, unsigned, 16-bit, CVSD
+
+        static btstack_cvsd_plc_state_t cvsdPlcState;
+
+        static QueueHandle_t sinkQueue;
+        static QueueHandle_t sourceQueue;
+        static DeviceMetadata_t metadata;
+        static const sys::Service *ownerService;
+
+        static auto audioInitialize(int sampleRate) -> Error;
+        static void initCvsd();
+        static void receiveCvsd(uint8_t *packet, uint16_t size);
+        static void writeToHostEndian(int16_t *buffer, uint8_t *packet, int length);
+        static void sendEvent(audio::EventType event);
+        static void flipEndianess(uint16_t *data, size_t length);
+    };
+
+    SCO::SCO() : pimpl(std::make_unique<SCOImpl>(SCOImpl()))
+    {}
+
+    SCO::SCO(SCO &&other) noexcept : pimpl(std::move(other.pimpl))
+    {}
+    auto SCO::operator=(SCO &&other) noexcept -> SCO &
+    {
+        if (&other == this) {
+            return *this;
+        }
+
+        pimpl       = std::move(other.pimpl);
+        other.pimpl = nullptr;
+        return *this;
+    }
+
+    void SCO::init()
+    {
+        pimpl->init();
+    }
+    void SCO::send(hci_con_handle_t sco_handle)
+    {
+        pimpl->send(sco_handle);
+    }
+    void SCO::receive(uint8_t *packet, uint16_t size)
+    {
+        pimpl->receive(packet, size);
+    }
+    [[nodiscard]] auto SCO::getStreamData() const -> std::shared_ptr<BluetoothStreamData>
+    {
+        return pimpl->getStreamData();
+    }
+    void SCO::setOwnerService(const sys::Service *service)
+    {
+        pimpl->setOwnerService(service);
+    }
+
+    SCO::~SCO() = default;
+} // namespace Bt
+
+using namespace Bt;
+
+btstack_cvsd_plc_state_t SCO::SCOImpl::cvsdPlcState;
+QueueHandle_t SCO::SCOImpl::sinkQueue;
+QueueHandle_t SCO::SCOImpl::sourceQueue;
+const sys::Service *SCO::SCOImpl::ownerService = nullptr;
+DeviceMetadata_t SCO::SCOImpl::metadata;
+
+void SCO::SCOImpl::sendEvent(audio::EventType event)
+{
+    auto evt = std::make_shared<audio::Event>(event);
+    auto msg = std::make_shared<AudioEventRequest>(std::move(evt));
+    sys::Bus::SendUnicast(std::move(msg), service::name::evt_manager, const_cast<sys::Service *>(ownerService));
+}
+auto SCO::SCOImpl::audioInitialize(int sampleRate) -> Error
+{
+    sourceQueue = xQueueCreate(5, sizeof(AudioData_t));
+    sinkQueue   = xQueueCreate(5, sizeof(AudioData_t));
+
+    int scoPayloadLength            = hci_get_sco_packet_length() - PACKET_DATA_OFFSET;
+    const int audioSamplesPerPacket = scoPayloadLength;
+
+    metadata.sampleRate      = static_cast<unsigned int>(sampleRate);
+    metadata.channels        = 1;
+    metadata.samplesPerFrame = audioSamplesPerPacket;
+
+    if (sourceQueue != nullptr && sinkQueue != nullptr) {
+        sendEvent(audio::EventType::BTHeadsetOn);
+    }
+    else {
+        LOG_ERROR("failed to create queue!");
+        return Error(Error::SystemError);
+    }
+
+    LOG_INFO("Init done!");
+    return Error(Error::Success);
+}
+
+void SCO::SCOImpl::initCvsd()
+{
+    btstack_cvsd_plc_init(&cvsdPlcState);
+    auto ret = audioInitialize(CVSD_SAMPLE_RATE);
+    if (ret.err == Error::Success) {
+        LOG_INFO("CVSD init done!");
+    }
+}
+void SCO::SCOImpl::writeToHostEndian(int16_t *buffer, uint8_t *packet, int length)
+{
+    for (int i = 0; i < length; i++) {
+        buffer[i] = little_endian_read_16(packet, PACKET_DATA_OFFSET + i * 2);
+    }
+}
+void SCO::SCOImpl::receiveCvsd(uint8_t *packet, uint16_t size)
+{
+
+    std::array<int16_t, AUDIO_BUFFER_LENGTH> audioFrameOut;
+
+    if (size > audioFrameOut.size()) {
+        LOG_WARN("SCO packet larger than local output buffer - dropping data.");
+        return;
+    }
+
+    const int audioBytesRead = size - PACKET_DATA_OFFSET;
+    const int numSamples     = audioBytesRead / BYTES_PER_FRAME;
+
+    std::array<int16_t, AUDIO_BUFFER_LENGTH> audioFrameIn;
+    writeToHostEndian(audioFrameIn.data(), packet, numSamples);
+
+    // treat packet as bad frame if controller does not report 'all good'
+    bool badFrame         = false;
+    auto packetStatusByte = packet[1];
+    if ((packetStatusByte & ALL_GOOD_MASK) != 0) {
+        badFrame = true;
+    }
+
+    btstack_cvsd_plc_process_data(&cvsdPlcState, badFrame, audioFrameIn.data(), numSamples, audioFrameOut.data());
+
+    // Samples in CVSD SCO packet are in little endian
+    AudioData_t audioData;
+    std::copy_n(std::begin(audioFrameOut), audioBytesRead, std::begin(audioData.data));
+    audioData.bytesSent = audioBytesRead;
+
+    if (sinkQueue != nullptr) {
+        xQueueSend(sinkQueue, &audioData, 5);
+    }
+    else {
+        LOG_ERROR("queue is nullptr!");
+    }
+}
+
+void SCO::SCOImpl::init()
+{
+    hci_set_sco_voice_setting(VOICE_SETTING_CVSD);
+    initCvsd();
+}
+
+void SCO::SCOImpl::flipEndianess(uint16_t *data, size_t length)
+{
+    for (size_t i = 0; i < length; i++) {
+        data[i] = __builtin_bswap16(data[i]);
+    }
+}
+void SCO::SCOImpl::send(hci_con_handle_t scoHandle)
+{
+    if (scoHandle == HCI_CON_HANDLE_INVALID) {
+        return;
+    }
+    assert(sourceQueue != nullptr);
+
+    int scoPacketLength  = hci_get_sco_packet_length();
+    int scoPayloadLength = scoPacketLength - PACKET_DATA_OFFSET;
+
+    hci_reserve_packet_buffer();
+    auto scoPacket  = hci_get_outgoing_packet_buffer();
+    auto sampleData = &scoPacket[3];
+    AudioData_t audioData;
+
+    if (xQueueReceive(sourceQueue, &audioData, 1) != pdPASS) {
+        auto rangeStart = static_cast<uint8_t *>(sampleData);
+        auto rangeEnd   = rangeStart + scoPayloadLength;
+        std::fill(rangeStart, rangeEnd, 0);
+    }
+    else {
+        auto dest = static_cast<std::uint8_t *>(sampleData);
+        std::copy(std::begin(audioData.data), std::begin(audioData.data) + scoPayloadLength, dest);
+
+        if (btstack_is_big_endian()) {
+            flipEndianess(reinterpret_cast<uint16_t *>(sampleData), scoPayloadLength / 2);
+        }
+    }
+
+    // set handle + flags
+    little_endian_store_16(scoPacket, 0, scoHandle);
+    // set length
+    scoPacket[2] = scoPayloadLength;
+    // finally send packet
+    hci_send_sco_packet_buffer(scoPacketLength);
+    // request another send event
+    hci_request_sco_can_send_now_event();
+}
+
+void SCO::SCOImpl::receive(uint8_t *packet, uint16_t size)
+{
+    receiveCvsd(packet, size);
+}
+auto SCO::SCOImpl::getStreamData() -> std::shared_ptr<BluetoothStreamData>
+{
+    return std::make_shared<BluetoothStreamData>(sinkQueue, sourceQueue, metadata);
+}
+void SCO::SCOImpl::setOwnerService(const sys::Service *service)
+{
+    ownerService = service;
+}

@@ 0,0 1,38 @@
+// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "Profile.hpp"
+#include <memory>
+#include <module-sys/Service/Bus.hpp>
+
+extern "C"
+{
+#include <module-bluetooth/lib/btstack/src/bluetooth.h>
+};
+namespace Bt
+{
+
+    class SCO
+    {
+      public:
+        SCO();
+        ~SCO();
+
+        SCO(const SCO &other) = delete;
+        auto operator=(SCO rhs) -> SCO & = delete;
+        SCO(SCO &&other) noexcept;
+        auto operator=(SCO &&other) noexcept -> SCO &;
+
+        void init();
+        void send(hci_con_handle_t sco_handle);
+        void receive(uint8_t *packet, uint16_t size);
+        [[nodiscard]] auto getStreamData() const -> std::shared_ptr<BluetoothStreamData>;
+        void setOwnerService(const sys::Service *service);
+
+      private:
+        class SCOImpl;
+        std::unique_ptr<SCOImpl> pimpl;
+    };
+} // namespace Bt

@@ 4,6 4,7 @@
 #pragma once
 
 #include "Error.hpp"
+#include <Service/Message.hpp>
 #include <module-services/service-bluetooth/ServiceBluetoothCommon.hpp>
 #include <memory>
 namespace Bt


@@ 12,9 13,12 @@ namespace Bt
     {
       public:
         virtual ~Profile()                           = default;
-        virtual auto init() -> Error                 = 0;
+        virtual auto init() -> Error::Code                                   = 0;
         virtual void setDeviceAddress(uint8_t *addr) = 0;
+        virtual void setOwnerService(const sys::Service *service)            = 0;
         virtual auto getStreamData() -> std::shared_ptr<BluetoothStreamData> = 0;
+        virtual void connect()                                               = 0;
+        virtual void disconnect()                                            = 0;
     };
 
 } // namespace Bt

@@ 11,6 11,8 @@ set(SOURCES
     ${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
+    ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/interface/profiles/HSP/HSP.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/Bluetooth/interface/profiles/HSP/SCO.cpp
     )
 
 message("Build with BlueKitchen")


@@ 60,5 62,6 @@ target_link_libraries(
     module-utils
     module-vfs
     module-sys
+    module-audio
     ${BOARD_DIR_LIBRARIES}
     )

@@ 46,13 46,18 @@ namespace bsp
     {
         return true;
     }
-
     void outBluetoothAudioWorkerTask(void *pvp)
     {
         auto *inst    = static_cast<RT1051BluetoothAudio *>(pvp);
         auto dataSize = inst->metadata.samplesPerFrame;
+        auto fatalError = false;
+
+        if (inst->sourceQueue == nullptr) {
+            LOG_FATAL("sourceQueue nullptr");
+            fatalError = true;
+        }
 
-        while (true) {
+        while (!fatalError) {
             auto framesFetched = inst->GetAudioCallback()(nullptr, inst->audioData.data.data(), dataSize);
             if (framesFetched == 0) {
                 break;

@@ 62,7 62,7 @@ sys::Message_t ServiceBluetooth::DataReceivedHandler(sys::DataMessage *msg, sys:
                     return std::make_shared<sys::ResponseMessage>(sys::ReturnCodes::Failure);
                 }
             case BluetoothMessage::StopScan:
-                worker->stop_scan();
+                worker->stopScan();
                 break;
             case BluetoothMessage::PAN: {
                 /// TODO request lwip first...


@@ 79,14 79,14 @@ sys::Message_t ServiceBluetooth::DataReceivedHandler(sys::DataMessage *msg, sys:
                 //                    }
             } break;
             case BluetoothMessage::Visible:
-                worker->set_visible();
+                worker->toggleVisibility();
                 break;
 
             case BluetoothMessage::Play:
-                worker->play_audio();
+                worker->establishAudioConnection();
                 break;
             case BluetoothMessage::Stop:
-                worker->stop_audio();
+                worker->disconnectAudioConnection();
                 break;
 
             default:


@@ 96,7 96,7 @@ sys::Message_t ServiceBluetooth::DataReceivedHandler(sys::DataMessage *msg, sys:
         }
         case MessageType::BluetoothAddrResult: {
             auto addrMsg = static_cast<BluetoothAddrMessage *>(msg);
-            worker->set_addr(addrMsg->addr);
+            worker->setDeviceAddress(addrMsg->addr);
         } break;
         case MessageType::BluetoothRequestStream: {
             auto result =