From ef3f840a4dfa76f16dd009c536782a8b28fec6e9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Marcin=20Smoczy=C5=84ski?= <smoczynski.marcin@gmail.com>
Date: Thu, 29 Apr 2021 11:53:51 +0200
Subject: [PATCH] [EGD-6049] Add voice call over HSP
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Add support for voice calls over HSP.

Signed-off-by: Marcin Smoczyński <smoczynski.marcin@gmail.com>
---
 module-audio/Audio/AudioDevice.hpp            |   3 +-
 .../Profiles/ProfilePlaybackBluetoothA2DP.hpp |   2 +-
 .../Profiles/ProfileRecordingBluetoothHSP.hpp |   2 +-
 .../Profiles/ProfileRoutingBluetoothHSP.hpp   |   2 +-
 .../Audio/transcode/InputTranscodeProxy.cpp   |   5 +
 .../Audio/transcode/InputTranscodeProxy.hpp   |   1 +
 .../board/rt1051/RT1051DeviceFactory.cpp      |   8 +-
 .../Bluetooth/audio/BluetoothAudioDevice.cpp  | 179 ++++++++++++++++--
 .../Bluetooth/audio/BluetoothAudioDevice.hpp  |  84 ++++++--
 .../interface/profiles/A2DP/A2DP.cpp          |   2 +-
 .../interface/profiles/A2DP/A2DPImpl.hpp      |   1 -
 .../interface/profiles/AudioProfile.hpp       |  15 ++
 .../Bluetooth/interface/profiles/HSP/HSP.cpp  |  20 +-
 .../Bluetooth/interface/profiles/HSP/HSP.hpp  |   3 +-
 .../interface/profiles/HSP/HSPImpl.hpp        |   4 +-
 .../Bluetooth/interface/profiles/HSP/SCO.cpp  |   1 -
 .../Bluetooth/interface/profiles/HSP/SCO.hpp  |   4 +-
 .../interface/profiles/ProfileManager.cpp     |  10 +-
 .../interface/profiles/ProfileManager.hpp     |   9 +-
 .../service-audio/ServiceAudio.cpp            |  44 +++--
 .../service-audio/ServiceAudio.hpp            |   3 +-
 21 files changed, 327 insertions(+), 75 deletions(-)
 create mode 100644 module-bluetooth/Bluetooth/interface/profiles/AudioProfile.hpp

diff --git a/module-audio/Audio/AudioDevice.hpp b/module-audio/Audio/AudioDevice.hpp
index f27d38b5414ce423bc0bffd8ffc1b18ad747b90f..3dd991753a51249b1e9d5c130a94270fad3ec1da 100644
--- a/module-audio/Audio/AudioDevice.hpp
+++ b/module-audio/Audio/AudioDevice.hpp
@@ -27,7 +27,8 @@ namespace audio
             None,
             Audiocodec,
             Cellular,
-            Bluetooth
+            BluetoothA2DP,
+            BluetoothHSP
         };
 
         virtual ~AudioDevice() = default;
diff --git a/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp b/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp
index 9b5e01700082f74897e0ebf336026c2883ac32b9..546199c3d562bddbdb9d5aa661542a39fa87df60 100644
--- a/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp
+++ b/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp
@@ -21,7 +21,7 @@ namespace audio
                                                   .inputGain     = 0,
                                                   .inputPath     = audio::codec::InputPath::None,
                                                   .outputPath    = audio::codec::OutputPath::None},
-                      AudioDevice::Type::Bluetooth)
+                      AudioDevice::Type::BluetoothA2DP)
         {}
     };
 
diff --git a/module-audio/Audio/Profiles/ProfileRecordingBluetoothHSP.hpp b/module-audio/Audio/Profiles/ProfileRecordingBluetoothHSP.hpp
index 7850fdc9584e58aad22aeb5ab4fa31ef46f82f1f..4234ac8ea5c4e8ce864e6124c53e81d11b0365f2 100644
--- a/module-audio/Audio/Profiles/ProfileRecordingBluetoothHSP.hpp
+++ b/module-audio/Audio/Profiles/ProfileRecordingBluetoothHSP.hpp
@@ -23,7 +23,7 @@ namespace audio
                                               .inputGain    = static_cast<float>(gain),
                                               .inputPath    = audio::codec::InputPath::None,
                                               .outputPath   = audio::codec::OutputPath::None},
-                  AudioDevice::Type::Bluetooth)
+                  AudioDevice::Type::BluetoothHSP)
         {}
     };
 
diff --git a/module-audio/Audio/Profiles/ProfileRoutingBluetoothHSP.hpp b/module-audio/Audio/Profiles/ProfileRoutingBluetoothHSP.hpp
index d48f57aa0c740ff2b04480067abde66d3d159265..f70e847efcbc6ccd7d3dff330f50720c17dad067 100644
--- a/module-audio/Audio/Profiles/ProfileRoutingBluetoothHSP.hpp
+++ b/module-audio/Audio/Profiles/ProfileRoutingBluetoothHSP.hpp
@@ -23,7 +23,7 @@ namespace audio
                           .inputGain    = static_cast<float>(gain),
                           .inputPath    = audio::codec::InputPath::None,
                           .outputPath   = audio::codec::OutputPath::None},
-                      AudioDevice::Type::Bluetooth)
+                      AudioDevice::Type::BluetoothHSP)
         {}
     };
 
diff --git a/module-audio/Audio/transcode/InputTranscodeProxy.cpp b/module-audio/Audio/transcode/InputTranscodeProxy.cpp
index 36f181c7afcf2f8f2fd83bb70e93d324b6e47fc0..1d88525a88e6cf6c605ada2f0cf12e7cc132fc39 100644
--- a/module-audio/Audio/transcode/InputTranscodeProxy.cpp
+++ b/module-audio/Audio/transcode/InputTranscodeProxy.cpp
@@ -23,6 +23,11 @@ bool InputTranscodeProxy::push(const Span &span)
     return getWrappedStream().push(transform->transform(span, transcodingSpaceSpan));
 }
 
+bool InputTranscodeProxy::push(void *data, std::size_t dataSize)
+{
+    return push(Span{.data = reinterpret_cast<std::uint8_t *>(data), .dataSize = dataSize});
+}
+
 void InputTranscodeProxy::commit()
 {
     if (isReserved) {
diff --git a/module-audio/Audio/transcode/InputTranscodeProxy.hpp b/module-audio/Audio/transcode/InputTranscodeProxy.hpp
index dfb2d23c41ed02e393db6c4ea3ddbaf9d1132a58..46903418f82a08b097f79fe912a0ac155d8be6c6 100644
--- a/module-audio/Audio/transcode/InputTranscodeProxy.hpp
+++ b/module-audio/Audio/transcode/InputTranscodeProxy.hpp
@@ -27,6 +27,7 @@ namespace audio::transcode
                                      std::shared_ptr<Transform> transform) noexcept;
 
         bool push(const Span &span) override;
+        bool push(void *data, std::size_t dataSize);
         void commit() override;
         bool reserve(Span &span) override;
         [[nodiscard]] auto getInputTraits() const noexcept -> Traits override;
diff --git a/module-audio/board/rt1051/RT1051DeviceFactory.cpp b/module-audio/board/rt1051/RT1051DeviceFactory.cpp
index a3b29a0202f9d4502d9e015b4b82fa597e79c1be..b7baa642b38df4f8f822e6982d04a69d0f59a21a 100644
--- a/module-audio/board/rt1051/RT1051DeviceFactory.cpp
+++ b/module-audio/board/rt1051/RT1051DeviceFactory.cpp
@@ -21,8 +21,12 @@ std::shared_ptr<AudioDevice> RT1051DeviceFactory::getDevice(const audio::Profile
         device = std::make_shared<RT1051AudioCodec>(profile.GetAudioConfiguration());
     } break;
 
-    case AudioDevice::Type::Bluetooth: {
-        device = std::make_shared<bluetooth::BluetoothAudioDevice>();
+    case AudioDevice::Type::BluetoothA2DP: {
+        device = std::make_shared<bluetooth::A2DPAudioDevice>();
+    } break;
+
+    case AudioDevice::Type::BluetoothHSP: {
+        device = std::make_shared<bluetooth::HSPAudioDevice>();
     } break;
 
     case AudioDevice::Type::Cellular: {
diff --git a/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.cpp b/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.cpp
index aca60e2e5df1f97b6d0acaa9212b2dae4ff61e5f..e386a28ec071d0a584cec8bdaa7527ebf0497109 100644
--- a/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.cpp
+++ b/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.cpp
@@ -3,42 +3,46 @@
 
 #include "BluetoothAudioDevice.hpp"
 
-#include <Audio/AudioCommon.hpp>
 #include <interface/profiles/A2DP/AVDTP.hpp>
-#include <module-audio/Audio/VolumeScaler.hpp>
+#include <interface/profiles/A2DP/AVRCP.hpp>
+#include <interface/profiles/HSP/SCO.hpp>
 
+#include <Audio/AudioCommon.hpp>
+#include <Audio/VolumeScaler.hpp>
 #include <Audio/Stream.hpp>
 
 #include <chrono>
 #include <cassert>
 
 using audio::AudioFormat;
-using namespace bluetooth;
+using bluetooth::A2DPAudioDevice;
+using bluetooth::BluetoothAudioDevice;
+using bluetooth::HSPAudioDevice;
+
 using namespace std::chrono_literals;
 
-BluetoothAudioDevice::BluetoothAudioDevice(MediaContext *mediaContext) : ctx(mediaContext)
-{
-    LOG_DEBUG("Bluetooth audio device created");
-}
+BluetoothAudioDevice::BluetoothAudioDevice(AudioProfile audioProfile) : profile(audioProfile)
+{}
 
 BluetoothAudioDevice::~BluetoothAudioDevice()
 {
     LOG_DEBUG("Destroying bluetooth audio device");
 }
 
-void BluetoothAudioDevice::setMediaContext(MediaContext *mediaContext)
+auto BluetoothAudioDevice::getProfileType() const -> AudioProfile
 {
-    ctx = mediaContext;
+    return profile;
 }
 
 auto BluetoothAudioDevice::setOutputVolume(float vol) -> audio::AudioDevice::RetCode
 {
     const auto volumeToSet = audio::volume::scaler::toAvrcpVolume(vol);
-    const auto status      = avrcp_controller_set_absolute_volume(ctx->avrcp_cid, volumeToSet);
+    const auto status      = avrcp_controller_set_absolute_volume(AVRCP::mediaTracker.avrcp_cid, volumeToSet);
     if (status != ERROR_CODE_SUCCESS) {
         LOG_ERROR("Can't set volume level. Status %x", status);
         return audio::AudioDevice::RetCode::Failure;
     }
+
     outputVolume = vol;
     return audio::AudioDevice::RetCode::Success;
 }
@@ -48,19 +52,125 @@ auto BluetoothAudioDevice::setInputGain(float gain) -> audio::AudioDevice::RetCo
     return audio::AudioDevice::RetCode::Success;
 }
 
-void BluetoothAudioDevice::onDataSend()
+auto BluetoothAudioDevice::isInputEnabled() const -> bool
+{
+    return inputEnabled;
+}
+
+auto BluetoothAudioDevice::isOutputEnabled() const -> bool
 {
-    if (outputEnabled) {
-        fillSbcAudioBuffer(ctx);
+    return outputEnabled;
+}
+
+void A2DPAudioDevice::onDataSend()
+{
+    if (isOutputEnabled()) {
+        fillSbcAudioBuffer();
     }
 }
 
-void BluetoothAudioDevice::onDataReceive()
+void A2DPAudioDevice::onDataReceive()
 {}
 
-void BluetoothAudioDevice::enableInput()
+void HSPAudioDevice::onDataSend()
 {}
 
+void HSPAudioDevice::onDataSend(std::uint16_t scoHandle)
+{
+    if (!isOutputEnabled()) {
+        return;
+    }
+
+    hci_reserve_packet_buffer();
+    auto scoPacket = hci_get_outgoing_packet_buffer();
+
+    // get data to send
+    audio::AbstractStream::Span dataSpan;
+    Sink::_stream->peek(dataSpan);
+
+    // prepare packet to send
+    std::copy(dataSpan.data, dataSpan.dataEnd(), &scoPacket[packetDataOffset]);
+    Sink::_stream->consume();
+    little_endian_store_16(scoPacket, packetHandleOffset, scoHandle);
+    scoPacket[packetLengthOffset] = dataSpan.dataSize;
+
+    // send packet
+    hci_send_sco_packet_buffer(dataSpan.dataSize + packetDataOffset);
+    hci_request_sco_can_send_now_event();
+}
+
+void HSPAudioDevice::receiveCVSD(audio::AbstractStream::Span receivedData)
+{
+    if (!isInputEnabled()) {
+        return;
+    }
+
+    auto blockSize          = Source::_stream->getInputTraits().blockSize;
+    auto decodedData        = decodeCVSD(receivedData);
+    auto processedDataIndex = 0;
+
+    // try to complete leftovers to the full block size
+    if (leftoversSize != 0) {
+        auto maxFillSize = blockSize - leftoversSize;
+        auto fillSize    = std::min(maxFillSize, decodedData.dataSize);
+
+        std::copy_n(decodedData.data, fillSize, &rxLeftovers[leftoversSize]);
+
+        if (fillSize + leftoversSize < blockSize) {
+            leftoversSize += fillSize;
+            return;
+        }
+
+        Source::_stream->push(&rxLeftovers[0], blockSize);
+        leftoversSize      = 0;
+        processedDataIndex = fillSize;
+    }
+
+    // push as many blocks as possible
+    while (decodedData.dataSize - processedDataIndex >= blockSize) {
+        Source::_stream->push(&decodedData.data[processedDataIndex], blockSize);
+        processedDataIndex += blockSize;
+    }
+
+    // save leftovers
+    leftoversSize = decodedData.dataSize - processedDataIndex;
+    if (leftoversSize > 0) {
+        std::copy_n(&decodedData.data[processedDataIndex], leftoversSize, &rxLeftovers[0]);
+    }
+}
+
+auto HSPAudioDevice::decodeCVSD(audio::AbstractStream::Span dataToDecode) -> audio::AbstractStream::Span
+{
+    auto decodedData = dataToDecode;
+    std::array<std::int16_t, scratchBufferSize> scratchBuffer;
+
+    const auto audioBytesRead = dataToDecode.dataSize - packetDataOffset;
+    const auto samplesCount   = audioBytesRead / sizeof(std::int16_t);
+    auto dataStart            = &dataToDecode.data[packetDataOffset];
+
+    for (auto i = 0; i < samplesCount; ++i) {
+        scratchBuffer[i] = little_endian_read_16(dataStart, i * sizeof(std::uint16_t));
+    }
+
+    auto packetStatusByte = dataToDecode.data[packetStatusOffset];
+    auto isBadFrame       = (packetStatusByte & allGoodMask) != 0;
+
+    btstack_cvsd_plc_process_data(&cvsdPlcState, isBadFrame, &scratchBuffer[0], samplesCount, &decoderBuffer[0]);
+
+    decodedData.data     = reinterpret_cast<std::uint8_t *>(decoderBuffer.get());
+    decodedData.dataSize = audioBytesRead;
+
+    return decodedData;
+}
+
+void HSPAudioDevice::onDataReceive()
+{}
+
+void BluetoothAudioDevice::enableInput()
+{
+    inputEnabled = true;
+}
+
 void BluetoothAudioDevice::enableOutput()
 {
     LOG_DEBUG("Enabling bluetooth audio output.");
@@ -68,7 +178,9 @@ void BluetoothAudioDevice::enableOutput()
 }
 
 void BluetoothAudioDevice::disableInput()
-{}
+{
+    inputEnabled = false;
+}
 
 void BluetoothAudioDevice::disableOutput()
 {
@@ -76,11 +188,21 @@ void BluetoothAudioDevice::disableOutput()
     outputEnabled = false;
 }
 
-auto BluetoothAudioDevice::fillSbcAudioBuffer(MediaContext *context) -> int
+void HSPAudioDevice::enableInput()
+{
+    auto blockSize = Source::_stream->getInputTraits().blockSize;
+    rxLeftovers    = std::make_unique<std::uint8_t[]>(blockSize);
+    decoderBuffer  = std::make_unique<std::int16_t[]>(scratchBufferSize);
+    btstack_cvsd_plc_init(&cvsdPlcState);
+    BluetoothAudioDevice::enableInput();
+}
+
+auto BluetoothAudioDevice::fillSbcAudioBuffer() -> int
 {
     // perform sbc encodin
     int totalNumBytesRead                    = 0;
     unsigned int numAudioSamplesPerSbcBuffer = btstack_sbc_encoder_num_audio_frames();
+    auto context                             = &AVRCP::mediaTracker;
 
     assert(context != nullptr);
 
@@ -104,7 +226,7 @@ auto BluetoothAudioDevice::fillSbcAudioBuffer(MediaContext *context) -> int
     return totalNumBytesRead;
 }
 
-auto BluetoothAudioDevice::getSupportedFormats() -> std::vector<audio::AudioFormat>
+auto A2DPAudioDevice::getSupportedFormats() -> std::vector<audio::AudioFormat>
 {
     constexpr static auto supportedBitWidth = 16U;
     return std::vector<AudioFormat>{AudioFormat{static_cast<unsigned>(AVDTP::sbcConfig.samplingFrequency),
@@ -112,12 +234,27 @@ auto BluetoothAudioDevice::getSupportedFormats() -> std::vector<audio::AudioForm
                                                 static_cast<unsigned>(AVDTP::sbcConfig.numChannels)}};
 }
 
-auto BluetoothAudioDevice::getSourceFormat() -> audio::AudioFormat
+auto HSPAudioDevice::getSupportedFormats() -> std::vector<audio::AudioFormat>
 {
-    return audio::nullFormat;
+    return std::vector<AudioFormat>{getSourceFormat()};
 }
 
-auto BluetoothAudioDevice::getTraits() const -> Traits
+auto A2DPAudioDevice::getTraits() const -> ::audio::Endpoint::Traits
 {
     return Traits{.usesDMA = false, .blockSizeConstraint = 512U, .timeConstraint = 10ms};
 }
+
+auto HSPAudioDevice::getTraits() const -> ::audio::Endpoint::Traits
+{
+    return Traits{.usesDMA = false, .blockSizeConstraint = 32U, .timeConstraint = 16ms};
+}
+
+auto A2DPAudioDevice::getSourceFormat() -> ::audio::AudioFormat
+{
+    return audio::nullFormat;
+}
+
+auto HSPAudioDevice::getSourceFormat() -> ::audio::AudioFormat
+{
+    return AudioFormat{bluetooth::SCO::CVSD_SAMPLE_RATE, supportedBitWidth, supportedChannels};
+}
diff --git a/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.hpp b/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.hpp
index e7d0f546a955f8380600420ddc226a37d196129a..6284447089b1954de8f952d0c7d22b4d86f817a9 100644
--- a/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.hpp
+++ b/module-bluetooth/Bluetooth/audio/BluetoothAudioDevice.hpp
@@ -3,9 +3,16 @@
 
 #pragma once
 
+#include <Audio/Endpoint.hpp>
 #include <Audio/AudioDevice.hpp>
 #include <Audio/AudioFormat.hpp>
 #include <interface/profiles/A2DP/MediaContext.hpp>
+#include <interface/profiles/AudioProfile.hpp>
+
+extern "C"
+{
+#include "classic/btstack_cvsd_plc.h"
+}
 
 namespace bluetooth
 {
@@ -13,31 +20,80 @@ namespace bluetooth
     class BluetoothAudioDevice : public audio::AudioDevice
     {
       public:
-        BluetoothAudioDevice() = default;
-        explicit BluetoothAudioDevice(MediaContext *mediaContext);
+        explicit BluetoothAudioDevice(AudioProfile audioProfile);
         virtual ~BluetoothAudioDevice();
 
-        RetCode setOutputVolume(float vol) override;
-        RetCode setInputGain(float gain) override;
-        void setMediaContext(MediaContext *MediaContext);
-        auto getTraits() const -> Traits override;
-        auto getSupportedFormats() -> std::vector<audio::AudioFormat> override;
-        auto getSourceFormat() -> audio::AudioFormat override;
+        virtual auto getProfileType() const -> AudioProfile;
+
+        auto setOutputVolume(float vol) -> audio::AudioDevice::RetCode override;
+        auto setInputGain(float gain) -> audio::AudioDevice::RetCode override;
 
         // Endpoint control methods
-        void onDataSend() override;
-        void onDataReceive() override;
         void enableInput() override;
         void enableOutput() override;
         void disableInput() override;
         void disableOutput() override;
 
+      protected:
+        auto isInputEnabled() const -> bool;
+        auto isOutputEnabled() const -> bool;
+        auto fillSbcAudioBuffer() -> int;
+
+      private:
+        bool outputEnabled   = false;
+        bool inputEnabled    = false;
+        AudioProfile profile = AudioProfile::None;
+        float outputVolume;
+    };
+
+    class A2DPAudioDevice : public BluetoothAudioDevice
+    {
+      public:
+        explicit A2DPAudioDevice() : BluetoothAudioDevice(AudioProfile::A2DP)
+        {}
+
+        void onDataSend() override;
+        void onDataReceive() override;
+        auto getSupportedFormats() -> std::vector<audio::AudioFormat> override;
+        auto getTraits() const -> Traits override;
+        auto getSourceFormat() -> ::audio::AudioFormat override;
+    };
+
+    class HSPAudioDevice : public BluetoothAudioDevice
+    {
+      public:
+        explicit HSPAudioDevice() : BluetoothAudioDevice(AudioProfile::HSP)
+        {}
+
+        void onDataSend() override;
+        void onDataSend(std::uint16_t scoHandle);
+        void onDataReceive() override;
+        auto getSupportedFormats() -> std::vector<audio::AudioFormat> override;
+        auto getTraits() const -> Traits override;
+        auto getSourceFormat() -> ::audio::AudioFormat override;
+        void enableInput() override;
+
+        void receiveCVSD(audio::AbstractStream::Span receivedData);
+
       private:
-        auto fillSbcAudioBuffer(MediaContext *context) -> int;
+        static constexpr std::size_t scratchBufferSize = 128;
+
+        static constexpr auto packetHandleOffset = 0;
+        static constexpr auto packetStatusOffset = 1;
+        static constexpr auto packetLengthOffset = 2;
+        static constexpr auto packetDataOffset   = 3;
+
+        constexpr static auto supportedBitWidth = 16U;
+        constexpr static auto supportedChannels = 1;
+
+        constexpr static auto allGoodMask = 0x30;
+
+        auto decodeCVSD(audio::AbstractStream::Span dataToDecode) -> audio::AbstractStream::Span;
 
-        MediaContext *ctx  = nullptr;
-        bool outputEnabled = false;
-        float outputVolume = 0.0;
+        std::unique_ptr<std::uint8_t[]> rxLeftovers;
+        std::unique_ptr<std::int16_t[]> decoderBuffer;
+        std::size_t leftoversSize = 0;
+        btstack_cvsd_plc_state_t cvsdPlcState;
     };
 
 } // namespace bluetooth
diff --git a/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DP.cpp b/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DP.cpp
index 7f9ce352761daf685976b09b06eb28d875fe44ae..f20cd30338c9a1c84c7e2484c149fb19ca6fd3f4 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DP.cpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DP.cpp
@@ -97,6 +97,7 @@ namespace bluetooth
 
     auto A2DP::startRinging() const noexcept -> Error::Code
     {
+        LOG_ERROR("Can't ring in A2DP profile");
         return Error::SystemError;
     }
 
@@ -622,7 +623,6 @@ namespace bluetooth
 
     void A2DP::A2DPImpl::setAudioDevice(std::shared_ptr<bluetooth::BluetoothAudioDevice> newAudioDevice)
     {
-        newAudioDevice->setMediaContext(&AVRCP::mediaTracker);
         A2DP::A2DPImpl::audioDevice = std::move(newAudioDevice);
     }
 
diff --git a/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DPImpl.hpp b/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DPImpl.hpp
index de028bb037c35e33f63516176b9be1e74b6a75aa..018a3b0e6d2196d5974cb9382fb07c80e3247cac 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DPImpl.hpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/A2DP/A2DPImpl.hpp
@@ -50,7 +50,6 @@ namespace bluetooth
         static void sendMediaPacket();
         static void sendAudioEvent(audio::EventType event, audio::Event::DeviceState state);
         static bool isConnected;
-
         static std::shared_ptr<BluetoothAudioDevice> audioDevice;
 
       public:
diff --git a/module-bluetooth/Bluetooth/interface/profiles/AudioProfile.hpp b/module-bluetooth/Bluetooth/interface/profiles/AudioProfile.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3ebb460ac8cc3fb46decd3468293aaa60e5f9c9a
--- /dev/null
+++ b/module-bluetooth/Bluetooth/interface/profiles/AudioProfile.hpp
@@ -0,0 +1,15 @@
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+namespace bluetooth
+{
+    enum class AudioProfile
+    {
+        A2DP,
+        HSP,
+        HFP,
+        None
+    };
+};
diff --git a/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.cpp b/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.cpp
index f4db01516c91992e1db3f3efe9dc044b5ae76f17..e6296680f09f9d0ca46856a6efffb6f4687b80ac 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.cpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.cpp
@@ -111,6 +111,7 @@ namespace bluetooth
     const sys::Service *HSP::HSPImpl::ownerService;
     std::string HSP::HSPImpl::agServiceName = "PurePhone HSP";
     bool HSP::HSPImpl::isConnected          = false;
+    std::shared_ptr<HSPAudioDevice> HSP::HSPImpl::audioDevice;
 
     void HSP::HSPImpl::sendAudioEvent(audio::EventType event, audio::Event::DeviceState state)
     {
@@ -127,7 +128,9 @@ namespace bluetooth
             if (READ_SCO_CONNECTION_HANDLE(event) != scoHandle) {
                 break;
             }
-            sco->receive(event, eventSize);
+            if (audioDevice != nullptr) {
+                audioDevice->receiveCVSD(audio::AbstractStream::Span{.data = event, .dataSize = eventSize});
+            }
             break;
 
         case HCI_EVENT_PACKET:
@@ -142,7 +145,9 @@ namespace bluetooth
     {
         switch (hci_event_packet_get_type(event)) {
         case HCI_EVENT_SCO_CAN_SEND_NOW:
-            sco->send(scoHandle);
+            if (audioDevice != nullptr) {
+                audioDevice->onDataSend(scoHandle);
+            }
             break;
         case HCI_EVENT_HSP_META:
             processHSPEvent(event);
@@ -163,6 +168,7 @@ namespace bluetooth
                 break;
             }
             LOG_DEBUG("RFCOMM connection established.\n");
+            sendAudioEvent(audio::EventType::BlutoothHSPDeviceState, audio::Event::DeviceState::Connected);
             isConnected = true;
             break;
         case HSP_SUBEVENT_RFCOMM_DISCONNECTION_COMPLETE:
@@ -314,4 +320,14 @@ namespace bluetooth
         stopRinging();
         establishAudioConnection();
     }
+
+    void HSP::setAudioDevice(std::shared_ptr<bluetooth::BluetoothAudioDevice> audioDevice)
+    {
+        pimpl->setAudioDevice(audioDevice);
+    }
+
+    void HSP::HSPImpl::setAudioDevice(std::shared_ptr<bluetooth::BluetoothAudioDevice> audioDevice)
+    {
+        HSP::HSPImpl::audioDevice = std::static_pointer_cast<HSPAudioDevice>(audioDevice);
+    }
 } // namespace bluetooth
diff --git a/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.hpp b/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.hpp
index 9e861d611b569a87152faed75b355d0aa7fbbbcc..9d6b2a08469794d74666e7e86facacac9599ebe3 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.hpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/HSP/HSP.hpp
@@ -55,8 +55,7 @@ namespace bluetooth
         /// @return Success
         [[nodiscard]] auto initializeCall() const noexcept -> Error::Code override;
 
-        void setAudioDevice(std::shared_ptr<bluetooth::BluetoothAudioDevice> audioDevice) override
-        {}
+        void setAudioDevice(std::shared_ptr<bluetooth::BluetoothAudioDevice> audioDevice) override;
 
       private:
         class HSPImpl;
diff --git a/module-bluetooth/Bluetooth/interface/profiles/HSP/HSPImpl.hpp b/module-bluetooth/Bluetooth/interface/profiles/HSP/HSPImpl.hpp
index bbd362e2feca0e7593dd75d9309bc9e59c3e203f..80f2c80c28e4b03a456d09df8406885784766442 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/HSP/HSPImpl.hpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/HSP/HSPImpl.hpp
@@ -27,6 +27,7 @@ namespace bluetooth
         void setDeviceAddress(bd_addr_t addr);
         void setOwnerService(const sys::Service *service);
         auto getStreamData() -> std::shared_ptr<BluetoothStreamData>;
+        void setAudioDevice(std::shared_ptr<bluetooth::BluetoothAudioDevice> audioDevice);
 
       private:
         static void sendAudioEvent(audio::EventType event, audio::Event::DeviceState state);
@@ -43,5 +44,6 @@ namespace bluetooth
         static bd_addr_t deviceAddr;
         static const sys::Service *ownerService;
         static bool isConnected;
+        static std::shared_ptr<HSPAudioDevice> audioDevice;
     };
-} // namespace Bt
+} // namespace bluetooth
diff --git a/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.cpp b/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.cpp
index 4b47199006456dbe9d6c40e2c1b27ee1265bbc2a..3d092196d019b226ee658173373bdf793bdbfd92 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.cpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.cpp
@@ -32,7 +32,6 @@ namespace bluetooth
         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;
diff --git a/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.hpp b/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.hpp
index fe944c94606d223f3aaf6d19fe8f0e9180ddfd89..8c3dbba5ee64bd2a98ee9b0023446e0e82fb8afc 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.hpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/HSP/SCO.hpp
@@ -30,8 +30,10 @@ namespace bluetooth
         [[nodiscard]] auto getStreamData() const -> std::shared_ptr<BluetoothStreamData>;
         void setOwnerService(const sys::Service *service);
 
+        static constexpr auto CVSD_SAMPLE_RATE = 8000;
+
       private:
         class SCOImpl;
         std::unique_ptr<SCOImpl> pimpl;
     };
-} // namespace Bt
+} // namespace bluetooth
diff --git a/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.cpp b/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.cpp
index 23267ce335bbbc3f7a4058255efa7938bf0d20d2..41cdcabf064c7b4e0a560108acc5257a05938673 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.cpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.cpp
@@ -97,6 +97,7 @@ namespace bluetooth
 
     auto ProfileManager::startRinging() -> Error::Code
     {
+        switchProfile(AudioProfile::HSP);
         return currentProfilePtr->startRinging();
     }
 
@@ -107,16 +108,19 @@ namespace bluetooth
 
     auto ProfileManager::initializeCall() -> Error::Code
     {
+        switchProfile(AudioProfile::HSP);
         return currentProfilePtr->initializeCall();
     }
 
     auto ProfileManager::setAudioDevice(std::shared_ptr<BluetoothAudioDevice> device) -> Error::Code
     {
-        if (currentProfilePtr == nullptr) {
+        auto profileType = device->getProfileType();
+
+        if (currentProfilePtr == nullptr || profilesList[profileType] == nullptr) {
             return Error::NotReady;
         }
 
-        currentProfilePtr->setAudioDevice(std::move(device));
-        return Error::Success;
+        profilesList[profileType]->setAudioDevice(device);
+        return switchProfile(profileType);
     }
 } // namespace bluetooth
diff --git a/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.hpp b/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.hpp
index cd282085b5d9009fdd05973a200587ae428660cd..3646f31a4b857c0b29dfaa759c30c89c62ecc65b 100644
--- a/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.hpp
+++ b/module-bluetooth/Bluetooth/interface/profiles/ProfileManager.hpp
@@ -6,6 +6,7 @@
 #include <Service/Service.hpp>
 #include <Error.hpp>
 #include "Profile.hpp"
+#include "AudioProfile.hpp"
 #include "interface/profiles/A2DP/A2DP.hpp"
 #include "interface/profiles/HSP/HSP.hpp"
 #include "audio/BluetoothAudioDevice.hpp"
@@ -21,14 +22,6 @@ extern "C"
 namespace bluetooth
 {
 
-    enum class AudioProfile
-    {
-        A2DP,
-        HSP,
-        HFP,
-        None
-    };
-
     using ProfileList = std::map<AudioProfile, std::shared_ptr<bluetooth::Profile>>;
 
     class ProfileManager
diff --git a/module-services/service-audio/ServiceAudio.cpp b/module-services/service-audio/ServiceAudio.cpp
index b496a1144fd0b2c9a80a6c3ec61d64fb687b9fcb..1ed3548284071229daea5194d8eb9aefe9b0c837 100644
--- a/module-services/service-audio/ServiceAudio.cpp
+++ b/module-services/service-audio/ServiceAudio.cpp
@@ -26,11 +26,11 @@ using namespace audio;
 
 inline constexpr auto audioServiceStackSize = 1024 * 4;
 
-static constexpr auto defaultVolumeHigh  = "10";
-static constexpr auto defaultVolumeLow   = "2";
-static constexpr auto defaultVolumeMuted = "0";
-static constexpr auto defaultTrue        = "1";
-static constexpr auto defaultFalse       = "0";
+static constexpr auto defaultVolumeHigh              = "10";
+static constexpr auto defaultVolumeLow               = "2";
+static constexpr auto defaultVolumeMuted             = "0";
+static constexpr auto defaultTrue                    = "1";
+static constexpr auto defaultFalse                   = "0";
 static constexpr auto defaultCallRingtonePath        = "assets/audio/ringtone/ringtone_drum_2.mp3";
 static constexpr auto defaultTextMessageRingtonePath = "assets/audio/sms/sms_drum_2.mp3";
 static constexpr auto defaultNotificationsPath       = "assets/audio/alarm/alarm_hang_drum.mp3";
@@ -153,9 +153,14 @@ std::optional<std::string> ServiceAudio::AudioServicesCallback(const sys::Messag
         return settings_it->second;
     }
     else if (const auto *deviceMsg = dynamic_cast<const AudioServiceMessage::AudioDeviceCreated *>(msg); deviceMsg) {
-        if (deviceMsg->getDeviceType() == AudioDevice::Type::Bluetooth) {
+        if (deviceMsg->getDeviceType() == AudioDevice::Type::BluetoothA2DP) {
             auto startBluetoothAudioMsg = std::make_shared<BluetoothAudioStartMessage>(
-                std::static_pointer_cast<bluetooth::BluetoothAudioDevice>(deviceMsg->getDevice()));
+                std::static_pointer_cast<bluetooth::A2DPAudioDevice>(deviceMsg->getDevice()));
+            bus.sendUnicast(std::move(startBluetoothAudioMsg), service::name::bluetooth);
+        }
+        else if (deviceMsg->getDeviceType() == AudioDevice::Type::BluetoothHSP) {
+            auto startBluetoothAudioMsg = std::make_shared<BluetoothAudioStartMessage>(
+                std::static_pointer_cast<bluetooth::HSPAudioDevice>(deviceMsg->getDevice()));
             bus.sendUnicast(std::move(startBluetoothAudioMsg), service::name::bluetooth);
         }
     }
@@ -325,11 +330,14 @@ std::unique_ptr<AudioResponseMessage> ServiceAudio::HandleStart(const Operation:
     if (opType == Operation::Type::Playback) {
         auto input = audioMux.GetPlaybackInput(playbackType);
         // stop bluetooth stream if available
-        if (bluetoothConnected) {
+        if (bluetoothA2DPConnected || bluetoothHSPConnected) {
             if (playbackType == audio::PlaybackType::CallRingtone) {
+                HandleSendEvent(std::make_shared<audio::Event>(EventType::BlutoothHSPDeviceState));
+
                 LOG_DEBUG("Sending Bluetooth start ringing");
                 bus.sendUnicast(std::make_shared<message::bluetooth::Ring>(message::bluetooth::Ring::State::Enable),
                                 service::name::bluetooth);
+                return std::make_unique<AudioStartPlaybackResponse>(audio::RetCode::Success, retToken);
             }
             else {
                 LOG_DEBUG("Sending Bluetooth start stream request");
@@ -348,7 +356,10 @@ std::unique_ptr<AudioResponseMessage> ServiceAudio::HandleStart(const Operation:
     }
     else if (opType == Operation::Type::Router) {
         auto input = audioMux.GetRoutingInput(true);
-        if (bluetoothConnected) {
+        if (bluetoothA2DPConnected) {
+            HandleSendEvent(std::make_shared<audio::Event>(EventType::BlutoothHSPDeviceState));
+        }
+        if (bluetoothHSPConnected) {
             LOG_DEBUG("Sending Bluetooth start routing");
             bus.sendUnicast(std::make_shared<message::bluetooth::StartAudioRouting>(), service::name::bluetooth);
         }
@@ -363,11 +374,10 @@ std::unique_ptr<AudioResponseMessage> ServiceAudio::HandleSendEvent(std::shared_
     // update bluetooth state
     if (evt->getType() == EventType::BlutoothA2DPDeviceState) {
         auto newState = evt->getDeviceState() == Event::DeviceState::Connected;
-        if (newState != bluetoothConnected) {
+        if (newState != bluetoothA2DPConnected) {
             LOG_DEBUG("Bluetooth connection status changed: %s", newState ? "connected" : "disconnected");
-            bluetoothConnected = newState;
+            bluetoothA2DPConnected = newState;
             HandleStop({audio::PlaybackType::Alarm,
-                        audio::PlaybackType::CallRingtone,
                         audio::PlaybackType::Meditation,
                         audio::PlaybackType::Notifications,
                         audio::PlaybackType::TextMessageRingtone},
@@ -375,6 +385,14 @@ std::unique_ptr<AudioResponseMessage> ServiceAudio::HandleSendEvent(std::shared_
         }
     }
 
+    if (evt->getType() == EventType::BlutoothHSPDeviceState) {
+        auto newState = evt->getDeviceState() == Event::DeviceState::Connected;
+        if (newState != bluetoothHSPConnected) {
+            LOG_DEBUG("Bluetooth connection status changed: %s", newState ? "connected" : "disconnected");
+            bluetoothHSPConnected = newState;
+        }
+    }
+
     // update information about endpoints availability
     for (auto &input : audioMux.GetAllInputs()) {
         input.audio->SendEvent(evt);
@@ -426,7 +444,7 @@ std::unique_ptr<AudioResponseMessage> ServiceAudio::HandleStop(const std::vector
     }
 
     // stop bluetooth stream if available
-    if (bluetoothConnected) {
+    if (bluetoothA2DPConnected) {
         LOG_DEBUG("Sending Bluetooth stop request");
         bus.sendUnicast(std::make_shared<BluetoothMessage>(BluetoothMessage::Request::Stop), service::name::bluetooth);
     }
diff --git a/module-services/service-audio/service-audio/ServiceAudio.hpp b/module-services/service-audio/service-audio/ServiceAudio.hpp
index 9fc340d8147a4209996a09bdf6c6a53f70ec4426..8c66a55e62a61bf17e35237012cae697d0f36777 100644
--- a/module-services/service-audio/service-audio/ServiceAudio.hpp
+++ b/module-services/service-audio/service-audio/ServiceAudio.hpp
@@ -57,7 +57,8 @@ class ServiceAudio : public sys::Service
     audio::AudioMux::VibrationStatus vibrationMotorStatus = audio::AudioMux::VibrationStatus::Off;
     std::unique_ptr<settings::Settings> settingsProvider;
     std::map<std::string, std::string> settingsCache;
-    bool bluetoothConnected = false;
+    bool bluetoothA2DPConnected = false;
+    bool bluetoothHSPConnected  = false;
 
     auto IsVibrationMotorOn()
     {