From 40bf381eca9c7146f205a4b5dd3f369dd2b0e317 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Marcin=20Smoczy=C5=84ski?= <smoczynski.marcin@gmail.com>
Date: Wed, 6 Jan 2021 23:02:35 +0100
Subject: [PATCH] [EGD-5086] Fix voice not starting when calling
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Due to a race condition between source and sink voice is not always
starting when calling. Introduce audio stream connections to avoid
race condition and improve handling of audio start and stop operations.

Signed-off-by: Marcin Smoczyński <smoczynski.marcin@gmail.com>
---
 module-audio/Audio/BluetoothProxyAudio.cpp    |  18 +++
 module-audio/Audio/BluetoothProxyAudio.hpp    |   7 ++
 module-audio/Audio/Endpoint.cpp               |  88 ++++++++++---
 module-audio/Audio/Endpoint.hpp               |  81 ++++++++++--
 .../Audio/Operation/PlaybackOperation.cpp     | 119 ++++++++----------
 .../Audio/Operation/PlaybackOperation.hpp     |   1 +
 .../Audio/Operation/RouterOperation.cpp       |  96 ++++++++------
 .../Audio/Operation/RouterOperation.hpp       |   5 +-
 .../Profiles/ProfilePlaybackBluetoothA2DP.hpp |   1 -
 module-audio/Audio/Stream.cpp                 |  28 +++--
 module-audio/Audio/decoder/Decoder.cpp        |  20 ++-
 module-audio/Audio/decoder/Decoder.hpp        |  11 +-
 module-audio/Audio/decoder/DecoderWorker.cpp  |  81 +++++++++---
 module-audio/Audio/decoder/DecoderWorker.hpp  |  25 +++-
 module-audio/Audio/decoder/decoderWAV.hpp     |   1 -
 .../board/linux/audio/LinuxCellularAudio.cpp  |  23 +++-
 .../board/linux/audio/LinuxCellularAudio.hpp  |   7 ++
 .../board/linux/audio/linux_audiocodec.cpp    |  23 +++-
 .../board/linux/audio/linux_audiocodec.hpp    |   7 ++
 .../rt1051/bsp/audio/RT1051Audiocodec.cpp     |  63 +---------
 .../rt1051/bsp/audio/RT1051Audiocodec.hpp     |   4 +-
 .../rt1051/bsp/audio/RT1051BluetoothAudio.cpp | 104 ---------------
 .../rt1051/bsp/audio/RT1051BluetoothAudio.hpp |  42 -------
 .../rt1051/bsp/audio/RT1051CellularAudio.cpp  |  62 +--------
 .../rt1051/bsp/audio/RT1051CellularAudio.hpp  |   4 +-
 .../board/rt1051/bsp/audio/SAIAudioDevice.cpp |  97 ++++++++++++++
 .../board/rt1051/bsp/audio/SAIAudioDevice.hpp |  38 ++++++
 module-bsp/bsp/audio/bsp_audio.cpp            |  52 ++++----
 module-bsp/bsp/audio/bsp_audio.hpp            |   5 +-
 module-bsp/targets/Target_RT1051.cmake        |   2 +-
 module-sys/Service/Worker.cpp                 |   5 +
 module-sys/Service/Worker.hpp                 |   4 +-
 32 files changed, 647 insertions(+), 477 deletions(-)
 delete mode 100644 module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.cpp
 delete mode 100644 module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.hpp
 create mode 100644 module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp
 create mode 100644 module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp

diff --git a/module-audio/Audio/BluetoothProxyAudio.cpp b/module-audio/Audio/BluetoothProxyAudio.cpp
index ddf04ac60..a7084eddf 100644
--- a/module-audio/Audio/BluetoothProxyAudio.cpp
+++ b/module-audio/Audio/BluetoothProxyAudio.cpp
@@ -72,4 +72,22 @@ namespace bsp
     {
         Stop();
     }
+
+    void BluetoothProxyAudio::onDataReceive()
+    {}
+
+    void BluetoothProxyAudio::onDataSend()
+    {}
+
+    void BluetoothProxyAudio::enableInput()
+    {}
+
+    void BluetoothProxyAudio::enableOutput()
+    {}
+
+    void BluetoothProxyAudio::disableInput()
+    {}
+
+    void BluetoothProxyAudio::disableOutput()
+    {}
 } // namespace bsp
diff --git a/module-audio/Audio/BluetoothProxyAudio.hpp b/module-audio/Audio/BluetoothProxyAudio.hpp
index 2be1222f8..de9308651 100644
--- a/module-audio/Audio/BluetoothProxyAudio.hpp
+++ b/module-audio/Audio/BluetoothProxyAudio.hpp
@@ -28,6 +28,13 @@ namespace bsp
         AudioDevice::RetCode InputPathCtrl(InputPath inputPath) final;
         bool IsFormatSupported(const Format &format) final;
 
+        void onDataReceive() final;
+        void onDataSend() final;
+        void enableInput() final;
+        void enableOutput() final;
+        void disableInput() final;
+        void disableOutput() final;
+
       private:
         audio::Stream &dataStreamOut;
         audio::Stream &dataStreamIn;
diff --git a/module-audio/Audio/Endpoint.cpp b/module-audio/Audio/Endpoint.cpp
index f45b3a617..9f6c6d2a8 100644
--- a/module-audio/Audio/Endpoint.cpp
+++ b/module-audio/Audio/Endpoint.cpp
@@ -7,18 +7,21 @@
 
 using namespace audio;
 
-void Endpoint::setStream(Stream &stream)
+Endpoint::Endpoint(const Capabilities &caps) : _caps(caps)
+{}
+
+const Endpoint::Capabilities &Endpoint::getCapabilities() const noexcept
+{
+    return _caps;
+}
+
+void Endpoint::connectStream(Stream &stream)
 {
     assert(_stream == nullptr);
     _stream = &stream;
 }
 
-Stream *Endpoint::getStream() const noexcept
-{
-    return _stream;
-}
-
-void Endpoint::unsetStream()
+void Endpoint::disconnectStream()
 {
     assert(_stream != nullptr);
     _stream = nullptr;
@@ -29,16 +32,71 @@ bool Endpoint::isConnected() const noexcept
     return _stream != nullptr;
 }
 
-void Source::connect(Sink &sink, Stream &stream)
+StreamConnection::StreamConnection(Source *source, Sink *sink, Stream *stream)
+    : _sink(sink), _source(source), _stream(stream)
 {
-    connectedSink = &sink;
-    connectedSink->setStream(stream);
-    setStream(stream);
+    assert(_sink != nullptr);
+    assert(_source != nullptr);
+    assert(_stream != nullptr);
+
+    _sink->connectStream(*_stream);
+    _source->connectStream(*_stream);
 }
 
-void Source::disconnectStream()
+StreamConnection::~StreamConnection()
 {
-    unsetStream();
-    connectedSink->unsetStream();
-    connectedSink = nullptr;
+    destroy();
+}
+
+void StreamConnection::destroy()
+{
+    disable();
+    _sink->disconnectStream();
+    _source->disconnectStream();
+}
+
+void StreamConnection::enable()
+{
+    if (enabled) {
+        return;
+    }
+
+    _stream->reset();
+    _sink->enableOutput();
+    _source->enableInput();
+
+    enabled = true;
+}
+
+void StreamConnection::disable()
+{
+    if (!enabled) {
+        return;
+    }
+
+    _source->disableInput();
+    _sink->disableOutput();
+    _stream->reset();
+
+    enabled = false;
+}
+
+bool StreamConnection::isEnabled() const noexcept
+{
+    return enabled;
+}
+
+Source *StreamConnection::getSource() const noexcept
+{
+    return _source;
+}
+
+Sink *StreamConnection::getSink() const noexcept
+{
+    return _sink;
+}
+
+Stream *StreamConnection::getStream() const noexcept
+{
+    return _stream;
 }
diff --git a/module-audio/Audio/Endpoint.hpp b/module-audio/Audio/Endpoint.hpp
index 858c31f9b..b291e83b0 100644
--- a/module-audio/Audio/Endpoint.hpp
+++ b/module-audio/Audio/Endpoint.hpp
@@ -10,26 +10,87 @@ namespace audio
     class Endpoint
     {
       public:
-        void setStream(Stream &stream);
-        Stream *getStream() const noexcept;
-        void unsetStream();
+        struct Capabilities
+        {
+            bool usesDMA             = false;
+            std::size_t maxBlockSize = 0;
+        };
+
+        Endpoint() = default;
+        Endpoint(const Capabilities &caps);
+
+        void connectStream(Stream &stream);
+        void disconnectStream();
         bool isConnected() const noexcept;
 
-      private:
+        [[nodiscard]] const Capabilities &getCapabilities() const noexcept;
+
+      protected:
+        Capabilities _caps;
         Stream *_stream = nullptr;
     };
 
     class Sink : public Endpoint
-    {};
+    {
+      public:
+        virtual void onDataSend()    = 0;
+        virtual void enableOutput()  = 0;
+        virtual void disableOutput() = 0;
+    };
 
     class Source : public Endpoint
     {
       public:
-        void connect(Sink &sink, Stream &stream);
-        void disconnectStream();
-
-      private:
-        Sink *connectedSink = nullptr;
+        virtual void onDataReceive() = 0;
+        virtual void enableInput()   = 0;
+        virtual void disableInput()  = 0;
     };
 
+    class IOProxy : public Sink, public Source
+    {
+      public:
+        inline bool isSinkConnected() const noexcept
+        {
+            return Sink::isConnected();
+        }
+
+        inline bool isSourceConnected() const noexcept
+        {
+            return Source::isConnected();
+        }
+
+        inline void connectOutputStream(Stream &stream)
+        {
+            Sink::connectStream(stream);
+        }
+
+        inline void connectInputStream(Stream &stream)
+        {
+            Source::connectStream(stream);
+        }
+    };
+
+    class StreamConnection
+    {
+      public:
+        StreamConnection() = default;
+        StreamConnection(Source *source, Sink *sink, Stream *stream);
+        ~StreamConnection();
+
+        void enable();
+        void disable();
+        void destroy();
+
+        [[nodiscard]] Source *getSource() const noexcept;
+        [[nodiscard]] Sink *getSink() const noexcept;
+        [[nodiscard]] Stream *getStream() const noexcept;
+
+        [[nodiscard]] bool isEnabled() const noexcept;
+
+      private:
+        bool enabled    = false;
+        Sink *_sink     = nullptr;
+        Source *_source = nullptr;
+        Stream *_stream = nullptr;
+    };
 }; // namespace audio
diff --git a/module-audio/Audio/Operation/PlaybackOperation.cpp b/module-audio/Audio/Operation/PlaybackOperation.cpp
index 92edb433c..30fe261c4 100644
--- a/module-audio/Audio/Operation/PlaybackOperation.cpp
+++ b/module-audio/Audio/Operation/PlaybackOperation.cpp
@@ -16,8 +16,6 @@ namespace audio
     using namespace AudioServiceMessage;
     using namespace utils;
 
-#define PERF_STATS_ON 0
-
     PlaybackOperation::PlaybackOperation(const char *file, const audio::PlaybackType &playbackType, Callback callback)
         : Operation(callback, playbackType), dec(nullptr)
     {
@@ -26,6 +24,13 @@ namespace audio
         AddProfile(Profile::Type::PlaybackHeadphones, playbackType, false);
         AddProfile(Profile::Type::PlaybackLoudspeaker, playbackType, true);
 
+        endOfFileCallback = [this]() {
+            state          = State::Idle;
+            const auto req = AudioServiceMessage::EndOfFile(operationToken);
+            serviceCallback(&req);
+            return std::string();
+        };
+
         auto defaultProfile = GetProfile(Profile::Type::PlaybackLoudspeaker);
         if (!defaultProfile) {
             throw AudioInitException("Error during initializing profile", RetCode::ProfileNotSet);
@@ -36,18 +41,12 @@ namespace audio
         if (dec == nullptr) {
             throw AudioInitException("Error during initializing decoder", RetCode::FileDoesntExist);
         }
+        tags = dec->fetchTags();
 
         auto retCode = SwitchToPriorityProfile();
         if (retCode != RetCode::Success) {
             throw AudioInitException("Failed to switch audio profile", retCode);
         }
-
-        endOfFileCallback = [this]() {
-            state          = State::Idle;
-            const auto req = AudioServiceMessage::EndOfFile(operationToken);
-            serviceCallback(&req);
-            return std::string();
-        };
     }
 
     audio::RetCode PlaybackOperation::Start(audio::Token token)
@@ -55,31 +54,21 @@ namespace audio
         if (state == State::Active || state == State::Paused) {
             return RetCode::InvokedInIncorrectState;
         }
+
+        // create audio connection
+        outputConnection = std::make_unique<StreamConnection>(dec.get(), audioDevice.get(), dataStreamOut);
+
+        // decoder worker soft start - must be called after connection setup
+        dec->startDecodingWorker(endOfFileCallback);
+
+        // start output device and enable audio connection
+        auto ret = audioDevice->Start(currentProfile->GetAudioFormat());
+        outputConnection->enable();
+
+        // update state and token
+        state          = State::Active;
         operationToken = token;
 
-        assert(dataStreamOut != nullptr);
-
-        dec->startDecodingWorker(*dataStreamOut, endOfFileCallback);
-
-        if (!tags) {
-            tags = dec->fetchTags();
-        }
-
-        state         = State::Active;
-
-        if (tags->num_channel == channel::stereoSound) {
-            currentProfile->SetInOutFlags(static_cast<uint32_t>(bsp::AudioDevice::Flags::OutputStereo));
-        }
-        else {
-            currentProfile->SetInOutFlags(static_cast<uint32_t>(bsp::AudioDevice::Flags::OutputMono));
-            if (currentProfile->GetOutputPath() == bsp::AudioDevice::OutputPath::Headphones) {
-                currentProfile->SetOutputPath(bsp::AudioDevice::OutputPath::HeadphonesMono);
-            }
-        }
-
-        currentProfile->SetSampleRate(tags->sample_rate);
-
-        auto ret = audioDevice->Start(currentProfile->GetAudioFormat());
         return GetDeviceError(ret);
     }
 
@@ -90,7 +79,10 @@ namespace audio
             return audio::RetCode::DeviceFailure;
         }
 
+        // stop playback by destroying audio connection
+        outputConnection.reset();
         dec->stopDecodingWorker();
+
         return GetDeviceError(audioDevice->Stop());
     }
 
@@ -100,23 +92,18 @@ namespace audio
             return RetCode::InvokedInIncorrectState;
         }
         state = State::Paused;
-
-        dec->stopDecodingWorker();
-        return GetDeviceError(audioDevice->Stop());
+        outputConnection->disable();
+        return audio::RetCode::Success;
     }
 
     audio::RetCode PlaybackOperation::Resume()
     {
-
         if (state == State::Active || state == State::Idle) {
             return RetCode::InvokedInIncorrectState;
         }
-        state    = State::Active;
-
-        assert(dataStreamOut != nullptr);
-        dec->startDecodingWorker(*dataStreamOut, endOfFileCallback);
-        auto ret = audioDevice->Start(currentProfile->GetAudioFormat());
-        return GetDeviceError(ret);
+        state = State::Active;
+        outputConnection->enable();
+        return audio::RetCode::Success;
     }
 
     audio::RetCode PlaybackOperation::SetOutputVolume(float vol)
@@ -159,41 +146,40 @@ namespace audio
 
     audio::RetCode PlaybackOperation::SwitchProfile(const Profile::Type type)
     {
-        uint32_t currentSampleRate = currentProfile->GetSampleRate();
-        uint32_t currentInOutFlags = currentProfile->GetInOutFlags();
-
-        auto ret = GetProfile(type);
-        if (ret) {
-            currentProfile = ret;
-        }
-        else {
+        auto newProfile = GetProfile(type);
+        if (newProfile == nullptr) {
             return RetCode::UnsupportedProfile;
         }
 
-        if (dec->isConnected()) {
-            dec->disconnectStream();
-        }
-
-        audioDevice = CreateDevice(currentProfile->GetAudioDeviceType(), audioCallback).value_or(nullptr);
+        /// profile change - (re)create output device; stop audio first by
+        /// killing audio connection
+        outputConnection.reset();
+        audioDevice.reset();
+        audioDevice = CreateDevice(newProfile->GetAudioDeviceType(), audioCallback).value_or(nullptr);
         if (audioDevice == nullptr) {
             LOG_ERROR("Error creating AudioDevice");
             return RetCode::Failed;
         }
 
-        dec->connect(audioDevice->sink, *dataStreamOut);
+        // adjust new profile with information from file's tags
+        newProfile->SetSampleRate(tags->sample_rate);
+        if (tags->num_channel == channel::stereoSound) {
+            newProfile->SetInOutFlags(static_cast<uint32_t>(bsp::AudioDevice::Flags::OutputStereo));
+        }
+        else {
+            newProfile->SetInOutFlags(static_cast<uint32_t>(bsp::AudioDevice::Flags::OutputMono));
+            if (newProfile->GetOutputPath() == bsp::AudioDevice::OutputPath::Headphones) {
+                newProfile->SetOutputPath(bsp::AudioDevice::OutputPath::HeadphonesMono);
+            }
+        }
 
-        currentProfile->SetSampleRate(currentSampleRate);
-        currentProfile->SetInOutFlags(currentInOutFlags);
+        // store profile
+        currentProfile = newProfile;
 
-        switch (state) {
-        case State::Idle:
-        case State::Paused:
-            break;
-
-        case State::Active:
+        if (state == State::Active) {
+            // playback in progress, restart
             state = State::Idle;
             Start(operationToken);
-            break;
         }
 
         return audio::RetCode::Success;
@@ -201,10 +187,7 @@ namespace audio
 
     PlaybackOperation::~PlaybackOperation()
     {
-        dec->stopDecodingWorker();
         Stop();
-        dataStreamOut->reset();
-        dataStreamIn->reset();
     }
 
 } // namespace audio
diff --git a/module-audio/Audio/Operation/PlaybackOperation.hpp b/module-audio/Audio/Operation/PlaybackOperation.hpp
index 376e0c15c..de0f1abfe 100644
--- a/module-audio/Audio/Operation/PlaybackOperation.hpp
+++ b/module-audio/Audio/Operation/PlaybackOperation.hpp
@@ -41,6 +41,7 @@ namespace audio
       private:
         std::unique_ptr<Decoder> dec;
         std::unique_ptr<Tags> tags;
+        std::unique_ptr<StreamConnection> outputConnection = nullptr;
 
         DecoderWorker::EndOfFileCallback endOfFileCallback;
     };
diff --git a/module-audio/Audio/Operation/RouterOperation.cpp b/module-audio/Audio/Operation/RouterOperation.cpp
index 8297c79b0..051bb7808 100644
--- a/module-audio/Audio/Operation/RouterOperation.cpp
+++ b/module-audio/Audio/Operation/RouterOperation.cpp
@@ -49,23 +49,37 @@ namespace audio
         operationToken = token;
         state          = State::Active;
 
-        if (audioDevice->IsFormatSupported(currentProfile->GetAudioFormat())) {
-            auto ret = audioDevice->Start(currentProfile->GetAudioFormat());
-            if (ret != bsp::AudioDevice::RetCode::Success) {
-                LOG_ERROR("Start error: %s", audio::str(audio::RetCode::DeviceFailure).c_str());
-            }
-        }
-        else {
+        // check if audio devices support desired audio format
+        if (!audioDevice->IsFormatSupported(currentProfile->GetAudioFormat())) {
             return RetCode::InvalidFormat;
         }
 
-        if (audioDeviceCellular->IsFormatSupported(currentProfile->GetAudioFormat())) {
-            auto ret = audioDeviceCellular->Start(currentProfile->GetAudioFormat());
-            return GetDeviceError(ret);
-        }
-        else {
+        if (!audioDeviceCellular->IsFormatSupported(currentProfile->GetAudioFormat())) {
             return RetCode::InvalidFormat;
         }
+
+        // try to run devices with the format
+        if (auto ret = audioDevice->Start(currentProfile->GetAudioFormat());
+            ret != bsp::AudioDevice::RetCode::Success) {
+            return GetDeviceError(ret);
+        }
+
+        if (auto ret = audioDeviceCellular->Start(currentProfile->GetAudioFormat());
+            ret != bsp::AudioDevice::RetCode::Success) {
+            return GetDeviceError(ret);
+        }
+
+        // create audio connections
+        inputConnection =
+            std::make_unique<audio::StreamConnection>(audioDeviceCellular.get(), audioDevice.get(), dataStreamIn);
+        outputConnection =
+            std::make_unique<audio::StreamConnection>(audioDevice.get(), audioDeviceCellular.get(), dataStreamOut);
+
+        // enable audio connections
+        inputConnection->enable();
+        outputConnection->enable();
+
+        return audio::RetCode::Success;
     }
 
     audio::RetCode RouterOperation::Stop()
@@ -75,10 +89,14 @@ namespace audio
         }
 
         state = State::Idle;
+        outputConnection.reset();
+        inputConnection.reset();
+
         audioDevice->Stop();
         audioDeviceCellular->Stop();
-        dataStreamOut->reset();
-        dataStreamIn->reset();
+
+        audioDevice.reset();
+        audioDeviceCellular.reset();
 
         return RetCode::Success;
     }
@@ -90,8 +108,8 @@ namespace audio
         }
 
         state = State::Paused;
-        audioDevice->Stop();
-        audioDeviceCellular->Stop();
+        outputConnection->disable();
+        inputConnection->disable();
         return RetCode::Success;
     }
 
@@ -102,8 +120,8 @@ namespace audio
         }
 
         state = State::Active;
-        audioDevice->Start(currentProfile->GetAudioFormat());
-        audioDeviceCellular->Start(currentProfile->GetAudioFormat());
+        inputConnection->enable();
+        outputConnection->enable();
         return RetCode::Success;
     }
 
@@ -141,41 +159,38 @@ namespace audio
 
     audio::RetCode RouterOperation::SwitchProfile(const audio::Profile::Type type)
     {
-        auto ret = GetProfile(type);
+        auto newProfile     = GetProfile(type);
+        auto callInProgress = state == State::Active;
 
-        if (ret) {
-            if (currentProfile && currentProfile->GetType() == ret->GetType()) {
-                return RetCode::Success;
-            }
-            currentProfile = ret;
-        }
-        else {
+        if (newProfile == nullptr) {
             return RetCode::UnsupportedProfile;
         }
 
-        audioDevice = CreateDevice(currentProfile->GetAudioDeviceType(), nullptr).value_or(nullptr);
+        if (currentProfile && currentProfile->GetType() == newProfile->GetType()) {
+            return RetCode::Success;
+        }
+
+        if (callInProgress) {
+            Stop();
+        }
+
+        audioDevice = CreateDevice(newProfile->GetAudioDeviceType(), nullptr).value_or(nullptr);
         if (audioDevice == nullptr) {
             LOG_ERROR("Error creating AudioDevice");
             return RetCode::Failed;
         }
+
         audioDeviceCellular = CreateDevice(bsp::AudioDevice::Type::Cellular, nullptr).value_or(nullptr);
         if (audioDeviceCellular == nullptr) {
             LOG_ERROR("Error creating AudioDeviceCellular");
             return RetCode::Failed;
         }
 
-        audioDevice->source.connect(audioDeviceCellular->sink, *dataStreamIn);
-        audioDeviceCellular->source.connect(audioDevice->sink, *dataStreamOut);
+        // store new profile
+        currentProfile = newProfile;
 
-        switch (state) {
-        case State::Idle:
-        case State::Paused:
-            break;
-
-        case State::Active:
-            state = State::Idle;
-            Start(operationToken);
-            break;
+        if (callInProgress) {
+            return Start(operationToken);
         }
 
         return RetCode::Success;
@@ -192,4 +207,9 @@ namespace audio
         return 0.0;
     }
 
+    RouterOperation::~RouterOperation()
+    {
+        Stop();
+    }
+
 } // namespace audio
diff --git a/module-audio/Audio/Operation/RouterOperation.hpp b/module-audio/Audio/Operation/RouterOperation.hpp
index 1365202e5..d6f88440a 100644
--- a/module-audio/Audio/Operation/RouterOperation.hpp
+++ b/module-audio/Audio/Operation/RouterOperation.hpp
@@ -8,6 +8,7 @@
 #include <Audio/encoder/Encoder.hpp>
 #include <Audio/AudioCommon.hpp>
 #include <Audio/Profiles/Profile.hpp>
+#include <Audio/Endpoint.hpp>
 #include <bsp/audio/bsp_audio.hpp>
 #include <mutex.hpp>
 
@@ -28,7 +29,7 @@ namespace audio
 
       public:
         RouterOperation(const char *file, AudioServiceMessage::Callback callback);
-        ~RouterOperation() = default;
+        ~RouterOperation();
 
         audio::RetCode Start(audio::Token token) final;
         audio::RetCode Stop() final;
@@ -47,6 +48,8 @@ namespace audio
         bool muteEnable = false;
         std::unique_ptr<Encoder> enc;
         std::unique_ptr<bsp::AudioDevice> audioDeviceCellular;
+        std::unique_ptr<StreamConnection> outputConnection;
+        std::unique_ptr<StreamConnection> inputConnection;
     };
 
 } // namespace audio
diff --git a/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp b/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp
index c333b5e03..b0b72cb16 100644
--- a/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp
+++ b/module-audio/Audio/Profiles/ProfilePlaybackBluetoothA2DP.hpp
@@ -4,7 +4,6 @@
 #pragma once
 
 #include "Profile.hpp"
-#include <module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.hpp>
 
 namespace audio
 {
diff --git a/module-audio/Audio/Stream.cpp b/module-audio/Audio/Stream.cpp
index 72d18f084..f57b9d890 100644
--- a/module-audio/Audio/Stream.cpp
+++ b/module-audio/Audio/Stream.cpp
@@ -27,7 +27,7 @@ bool Stream::push(void *data, std::size_t dataSize)
 
 bool Stream::push(const Span &span)
 {
-    LockGuard lock();
+    LockGuard lock;
 
     /// sanity - do not store buffers different than internal block size
     if (span.dataSize != _blockSize) {
@@ -65,7 +65,7 @@ bool Stream::push()
 
 bool Stream::pop(Span &span)
 {
-    LockGuard lock();
+    LockGuard lock;
 
     /// sanity - do not store buffers different than internal block size
     if (span.dataSize != _blockSize) {
@@ -95,7 +95,7 @@ bool Stream::pop(Span &span)
 
 void Stream::consume()
 {
-    LockGuard lock();
+    LockGuard lock;
 
     _blocksUsed -= _peekCount;
     _peekCount = 0;
@@ -106,7 +106,7 @@ void Stream::consume()
 
 bool Stream::peek(Span &span)
 {
-    LockGuard lock();
+    LockGuard lock;
 
     if (getPeekedCount() < getUsedBlockCount()) {
         span = *_peekPosition++;
@@ -121,7 +121,7 @@ bool Stream::peek(Span &span)
 
 void Stream::unpeek()
 {
-    LockGuard lock();
+    LockGuard lock;
 
     _peekPosition = _dataStart;
     _peekCount    = 0;
@@ -129,7 +129,7 @@ void Stream::unpeek()
 
 bool Stream::reserve(Span &span)
 {
-    LockGuard lock();
+    LockGuard lock;
 
     if (getBlockCount() - getUsedBlockCount() > _reserveCount) {
         span = *++_writeReservationPosition;
@@ -143,7 +143,7 @@ bool Stream::reserve(Span &span)
 
 void Stream::commit()
 {
-    LockGuard lock();
+    LockGuard lock;
 
     _blocksUsed += _reserveCount;
     _reserveCount = 0;
@@ -154,7 +154,7 @@ void Stream::commit()
 
 void Stream::release()
 {
-    LockGuard lock();
+    LockGuard lock;
 
     _reserveCount             = 0;
     _writeReservationPosition = _dataEnd;
@@ -162,21 +162,21 @@ void Stream::release()
 
 std::size_t Stream::getBlockSize() const noexcept
 {
-    LockGuard lock();
+    LockGuard lock;
 
     return _blockSize;
 }
 
 void Stream::registerListener(EventListener *listener)
 {
-    LockGuard lock();
+    LockGuard lock;
 
     listeners.push_back(std::ref(listener));
 }
 
 void Stream::unregisterListeners(Stream::EventListener *listener)
 {
-    LockGuard lock();
+    LockGuard lock;
 
     auto it = std::find(listeners.begin(), listeners.end(), listener);
     if (it != listeners.end()) {
@@ -230,13 +230,13 @@ std::size_t Stream::getReservedCount() const noexcept
 
 bool Stream::isEmpty() const noexcept
 {
-    LockGuard lock();
+    LockGuard lock;
     return getUsedBlockCount() == 0;
 }
 
 bool Stream::isFull() const noexcept
 {
-    LockGuard lock();
+    LockGuard lock;
     return getUsedBlockCount() == getBlockCount();
 }
 
@@ -247,6 +247,8 @@ bool Stream::blocksAvailable() const noexcept
 
 void Stream::reset()
 {
+    LockGuard lock;
+
     _dataStart                = {_buffer.get(), _blockSize * _blockCount, _buffer.get(), _blockSize};
     _dataEnd                  = _dataStart;
     _peekPosition             = _dataStart;
diff --git a/module-audio/Audio/decoder/Decoder.cpp b/module-audio/Audio/decoder/Decoder.cpp
index 05d0abea5..ae507b920 100644
--- a/module-audio/Audio/decoder/Decoder.cpp
+++ b/module-audio/Audio/decoder/Decoder.cpp
@@ -121,10 +121,11 @@ namespace audio
         memcpy(pcm, &workerBuffer[0], samplecount * 2 * sizeof(int16_t));
     }
 
-    void Decoder::startDecodingWorker(Stream &audioStream, DecoderWorker::EndOfFileCallback endOfFileCallback)
+    void Decoder::startDecodingWorker(DecoderWorker::EndOfFileCallback endOfFileCallback)
     {
+        assert(_stream != nullptr);
         if (!audioWorker) {
-            audioWorker = std::make_unique<DecoderWorker>(audioStream, this, endOfFileCallback);
+            audioWorker = std::make_unique<DecoderWorker>(_stream, this, endOfFileCallback);
             audioWorker->init();
             audioWorker->run();
         }
@@ -141,4 +142,19 @@ namespace audio
         audioWorker = nullptr;
     }
 
+    void Decoder::onDataReceive()
+    {
+        audioWorker->enablePlayback();
+    }
+
+    void Decoder::enableInput()
+    {
+        audioWorker->enablePlayback();
+    }
+
+    void Decoder::disableInput()
+    {
+        audioWorker->disablePlayback();
+    }
+
 } // namespace audio
diff --git a/module-audio/Audio/decoder/Decoder.hpp b/module-audio/Audio/decoder/Decoder.hpp
index e4191ece1..2f8f4d5e3 100644
--- a/module-audio/Audio/decoder/Decoder.hpp
+++ b/module-audio/Audio/decoder/Decoder.hpp
@@ -80,9 +80,6 @@ namespace audio
 
         virtual uint32_t decode(uint32_t samplesToRead, int16_t *pcmData) = 0;
 
-        void startDecodingWorker(Stream &audioStream, DecoderWorker::EndOfFileCallback endOfFileCallback);
-        void stopDecodingWorker();
-
         std::unique_ptr<Tags> fetchTags();
 
         // Range 0 - 1
@@ -103,6 +100,13 @@ namespace audio
             return position;
         }
 
+        void onDataReceive() override;
+        void enableInput() override;
+        void disableInput() override;
+
+        void startDecodingWorker(DecoderWorker::EndOfFileCallback endOfFileCallback);
+        void stopDecodingWorker();
+
         // Factory method
         static std::unique_ptr<Decoder> Create(const char *file);
 
@@ -127,6 +131,7 @@ namespace audio
 
         // decoding worker
         std::unique_ptr<DecoderWorker> audioWorker;
+        DecoderWorker::EndOfFileCallback _endOfFileCallback;
     };
 
 } // namespace audio
diff --git a/module-audio/Audio/decoder/DecoderWorker.cpp b/module-audio/Audio/decoder/DecoderWorker.cpp
index 6071683f1..8275c1c54 100644
--- a/module-audio/Audio/decoder/DecoderWorker.cpp
+++ b/module-audio/Audio/decoder/DecoderWorker.cpp
@@ -4,15 +4,15 @@
 #include "DecoderWorker.hpp"
 #include "Audio/decoder/Decoder.hpp"
 
-audio::DecoderWorker::DecoderWorker(Stream &audioStreamOut, Decoder *decoder, EndOfFileCallback endOfFileCallback)
+audio::DecoderWorker::DecoderWorker(Stream *audioStreamOut, Decoder *decoder, EndOfFileCallback endOfFileCallback)
     : sys::Worker(DecoderWorker::workerName, DecoderWorker::workerPriority), audioStreamOut(audioStreamOut),
       decoder(decoder), endOfFileCallback(endOfFileCallback),
-      bufferSize(audioStreamOut.getBlockSize() / sizeof(BufferInternalType))
+      bufferSize(audioStreamOut->getBlockSize() / sizeof(BufferInternalType))
 {}
 
 audio::DecoderWorker::~DecoderWorker()
 {
-    audioStreamOut.unregisterListeners(queueListener.get());
+    audioStreamOut->unregisterListeners(queueListener.get());
 }
 
 auto audio::DecoderWorker::init(std::list<sys::WorkerQueueInfo> queues) -> bool
@@ -27,7 +27,7 @@ auto audio::DecoderWorker::init(std::list<sys::WorkerQueueInfo> queues) -> bool
         return false;
     }
 
-    audioStreamOut.registerListener(queueListener.get());
+    audioStreamOut->registerListener(queueListener.get());
 
     decoderBuffer = std::make_unique<BufferInternalType[]>(bufferSize);
     if (!decoderBuffer) {
@@ -39,8 +39,9 @@ auto audio::DecoderWorker::init(std::list<sys::WorkerQueueInfo> queues) -> bool
 
 bool audio::DecoderWorker::handleMessage(uint32_t queueID)
 {
-    auto queue = queues[queueID];
-    if (queue->GetQueueName() == listenerQueueName && queueListener) {
+    auto queue      = queues[queueID];
+    auto &queueName = queue->GetQueueName();
+    if (queueName == listenerQueueName && queueListener) {
         auto event = queueListener->getEvent();
 
         switch (event.second) {
@@ -55,22 +56,64 @@ bool audio::DecoderWorker::handleMessage(uint32_t queueID)
         case Stream::Event::StreamHalfUsed:
             [[fallthrough]];
         case Stream::Event::StreamEmpty:
-            auto samplesRead = 0;
+            pushAudioData();
+        }
+    }
+    else if (queueName == SERVICE_QUEUE_NAME) {
+        auto &serviceQueue = getServiceQueue();
+        sys::WorkerCommand cmd;
 
-            while (!audioStreamOut.isFull()) {
-                samplesRead = decoder->decode(bufferSize, decoderBuffer.get());
-
-                if (samplesRead == 0) {
-                    endOfFileCallback();
-                    break;
-                }
-
-                if (!audioStreamOut.push(decoderBuffer.get(), samplesRead * sizeof(BufferInternalType))) {
-                    LOG_FATAL("Decoder failed to push to stream.");
-                    break;
-                }
+        if (serviceQueue.Dequeue(&cmd)) {
+            switch (static_cast<Command>(cmd.command)) {
+            case Command::EnablePlayback: {
+                playbackEnabled = true;
+                stateSemaphore.Give();
+                pushAudioData();
+                break;
+            }
+            case Command::DisablePlayback: {
+                playbackEnabled = false;
+                stateSemaphore.Give();
+            }
             }
         }
     }
+
     return true;
 }
+
+void audio::DecoderWorker::pushAudioData()
+{
+    auto samplesRead = 0;
+
+    while (!audioStreamOut->isFull() && playbackEnabled) {
+        samplesRead = decoder->decode(bufferSize, decoderBuffer.get());
+
+        if (samplesRead == 0) {
+            endOfFileCallback();
+            break;
+        }
+
+        if (!audioStreamOut->push(decoderBuffer.get(), samplesRead * sizeof(BufferInternalType))) {
+            LOG_FATAL("Decoder failed to push to stream.");
+            break;
+        }
+    }
+}
+
+bool audio::DecoderWorker::enablePlayback()
+{
+    return sendCommand({.command = static_cast<uint32_t>(Command::EnablePlayback), .data = nullptr}) &&
+           stateChangeWait();
+}
+
+bool audio::DecoderWorker::disablePlayback()
+{
+    return sendCommand({.command = static_cast<uint32_t>(Command::DisablePlayback), .data = nullptr}) &&
+           stateChangeWait();
+}
+
+bool audio::DecoderWorker::stateChangeWait()
+{
+    return stateSemaphore.Take();
+}
diff --git a/module-audio/Audio/decoder/DecoderWorker.hpp b/module-audio/Audio/decoder/DecoderWorker.hpp
index 0b80e21c2..c2556e31a 100644
--- a/module-audio/Audio/decoder/DecoderWorker.hpp
+++ b/module-audio/Audio/decoder/DecoderWorker.hpp
@@ -3,9 +3,11 @@
 
 #pragma once
 
-#include <Service/Worker.hpp>
 #include "Audio/StreamQueuedEventsListener.hpp"
 
+#include <Service/Worker.hpp>
+#include <semaphore.hpp>
+
 namespace audio
 {
     class Decoder;
@@ -13,14 +15,25 @@ namespace audio
     {
       public:
         using EndOfFileCallback = std::function<void()>;
+        enum class Command
+        {
+            EnablePlayback,
+            DisablePlayback,
+        };
 
-        DecoderWorker(Stream &audioStreamOut, Decoder *decoder, EndOfFileCallback endOfFileCallback);
+        DecoderWorker(Stream *audioStreamOut, Decoder *decoder, EndOfFileCallback endOfFileCallback);
         ~DecoderWorker() override;
 
         virtual auto init(std::list<sys::WorkerQueueInfo> queues = std::list<sys::WorkerQueueInfo>()) -> bool override;
-        virtual auto handleMessage(uint32_t queueID) -> bool override;
+
+        auto enablePlayback() -> bool;
+        auto disablePlayback() -> bool;
 
       private:
+        virtual auto handleMessage(uint32_t queueID) -> bool override;
+        void pushAudioData();
+        bool stateChangeWait();
+
         using BufferInternalType = int16_t;
 
         static constexpr auto workerName            = "DecoderWorker";
@@ -28,10 +41,12 @@ namespace audio
         static constexpr auto listenerQueueName     = "DecoderWorkerQueue";
         static constexpr auto listenerQueueCapacity = 1024;
 
-        Stream &audioStreamOut;
-        Decoder *decoder = nullptr;
+        Stream *audioStreamOut = nullptr;
+        Decoder *decoder       = nullptr;
         EndOfFileCallback endOfFileCallback;
         std::unique_ptr<StreamQueuedEventsListener> queueListener;
+        bool playbackEnabled = false;
+        cpp_freertos::BinarySemaphore stateSemaphore;
 
         const int bufferSize;
         std::unique_ptr<BufferInternalType[]> decoderBuffer;
diff --git a/module-audio/Audio/decoder/decoderWAV.hpp b/module-audio/Audio/decoder/decoderWAV.hpp
index 183985405..ff6175575 100644
--- a/module-audio/Audio/decoder/decoderWAV.hpp
+++ b/module-audio/Audio/decoder/decoderWAV.hpp
@@ -44,4 +44,3 @@ namespace audio
     };
 
 } // namespace audio
-
diff --git a/module-bsp/board/linux/audio/LinuxCellularAudio.cpp b/module-bsp/board/linux/audio/LinuxCellularAudio.cpp
index 3cca63be3..c856bb454 100755
--- a/module-bsp/board/linux/audio/LinuxCellularAudio.cpp
+++ b/module-bsp/board/linux/audio/LinuxCellularAudio.cpp
@@ -95,7 +95,7 @@ namespace bsp
         }
 
         if (inputBuffer) {
-            int16_t *pBuff = reinterpret_cast<int16_t *>(const_cast<void *>(inputBuffer));
+            int16_t *pBuff              = reinterpret_cast<int16_t *>(const_cast<void *>(inputBuffer));
             constexpr float scaleFactor = .1f;
             std::transform(pBuff, pBuff + framesToFetch, pBuff, [ptr, &scaleFactor](int16_t c) -> int16_t {
                 return static_cast<float>(c * ptr->currentFormat.inputGain * scaleFactor);
@@ -111,7 +111,7 @@ namespace bsp
 
             // Scale output buffer
             if (outputBuffer) {
-                int16_t *pBuff = reinterpret_cast<int16_t *>(outputBuffer);
+                int16_t *pBuff              = reinterpret_cast<int16_t *>(outputBuffer);
                 constexpr float scaleFactor = .1f;
                 std::transform(pBuff, pBuff + framesToFetch, pBuff, [ptr](int16_t c) -> int16_t {
                     return static_cast<float>(c * ptr->currentFormat.outputVolume / 10.f);
@@ -202,4 +202,23 @@ namespace bsp
             return false;
         }
     }
+
+    void LinuxCellularAudio::onDataReceive()
+    {}
+
+    void LinuxCellularAudio::onDataSend()
+    {}
+
+    void LinuxCellularAudio::enableInput()
+    {}
+
+    void LinuxCellularAudio::enableOutput()
+    {}
+
+    void LinuxCellularAudio::disableInput()
+    {}
+
+    void LinuxCellularAudio::disableOutput()
+    {}
+
 } // namespace bsp
diff --git a/module-bsp/board/linux/audio/LinuxCellularAudio.hpp b/module-bsp/board/linux/audio/LinuxCellularAudio.hpp
index 588a9d509..9c061c18e 100755
--- a/module-bsp/board/linux/audio/LinuxCellularAudio.hpp
+++ b/module-bsp/board/linux/audio/LinuxCellularAudio.hpp
@@ -31,6 +31,13 @@ namespace bsp
 
         bool IsFormatSupported(const Format &format) override final;
 
+        void onDataReceive() final;
+        void onDataSend() final;
+        void enableInput() final;
+        void enableOutput() final;
+        void disableInput() final;
+        void disableOutput() final;
+
       private:
         PaStream *stream;
 
diff --git a/module-bsp/board/linux/audio/linux_audiocodec.cpp b/module-bsp/board/linux/audio/linux_audiocodec.cpp
index ab09428bf..c393a0af7 100755
--- a/module-bsp/board/linux/audio/linux_audiocodec.cpp
+++ b/module-bsp/board/linux/audio/linux_audiocodec.cpp
@@ -93,7 +93,7 @@ namespace bsp
         }
 
         if (inputBuffer) {
-            int16_t *pBuff = reinterpret_cast<int16_t *>(const_cast<void *>(inputBuffer));
+            int16_t *pBuff              = reinterpret_cast<int16_t *>(const_cast<void *>(inputBuffer));
             constexpr float scaleFactor = .1f;
             std::transform(pBuff, pBuff + framesToFetch, pBuff, [ptr, &scaleFactor](int16_t c) -> int16_t {
                 return static_cast<float>(c * ptr->currentFormat.inputGain * scaleFactor);
@@ -109,7 +109,7 @@ namespace bsp
 
             // Scale output buffer
             if (outputBuffer) {
-                int16_t *pBuff = reinterpret_cast<int16_t *>(outputBuffer);
+                int16_t *pBuff              = reinterpret_cast<int16_t *>(outputBuffer);
                 constexpr float scaleFactor = .1f;
                 std::transform(pBuff, pBuff + framesToFetch, pBuff, [ptr, &scaleFactor](int16_t c) -> int16_t {
                     return static_cast<float>(c * ptr->currentFormat.outputVolume * scaleFactor);
@@ -200,4 +200,23 @@ namespace bsp
             return false;
         }
     }
+
+    void LinuxAudiocodec::onDataReceive()
+    {}
+
+    void LinuxAudiocodec::onDataSend()
+    {}
+
+    void LinuxAudiocodec::enableInput()
+    {}
+
+    void LinuxAudiocodec::enableOutput()
+    {}
+
+    void LinuxAudiocodec::disableInput()
+    {}
+
+    void LinuxAudiocodec::disableOutput()
+    {}
+
 } // namespace bsp
diff --git a/module-bsp/board/linux/audio/linux_audiocodec.hpp b/module-bsp/board/linux/audio/linux_audiocodec.hpp
index 36cac5cb2..e1f30e187 100755
--- a/module-bsp/board/linux/audio/linux_audiocodec.hpp
+++ b/module-bsp/board/linux/audio/linux_audiocodec.hpp
@@ -24,6 +24,13 @@ namespace bsp
         AudioDevice::RetCode InputPathCtrl(InputPath inputPath) override final;
         bool IsFormatSupported(const Format &format) override final;
 
+        void onDataReceive() final;
+        void onDataSend() final;
+        void enableInput() final;
+        void enableOutput() final;
+        void disableInput() final;
+        void disableOutput() final;
+
       private:
         PaStream *stream;
 
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp
index 8eb5abc9c..ae0b5cb56 100644
--- a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp
@@ -26,7 +26,8 @@ namespace bsp
     sai_edma_handle_t RT1051Audiocodec::rxHandle      = {};
 
     RT1051Audiocodec::RT1051Audiocodec(bsp::AudioDevice::audioCallback_t callback)
-        : AudioDevice(callback), saiInFormat{}, saiOutFormat{}, codecParams{}, codec{}
+        : SAIAudioDevice(callback, BOARD_AUDIOCODEC_SAIx, &rxHandle, &txHandle), saiInFormat{}, saiOutFormat{},
+          codecParams{}, codec{}
     {
         isInitialized = true;
     }
@@ -238,17 +239,6 @@ namespace bsp
 
         /* Reset SAI Rx internal logic */
         SAI_RxSoftwareReset(BOARD_AUDIOCODEC_SAIx, kSAI_ResetTypeSoftware);
-
-        if (!source.isConnected()) {
-            LOG_FATAL("No output stream connected!");
-            return;
-        }
-
-        /// initiate first read
-        audio::Stream::Span dataSpan;
-        source.getStream()->reserve(dataSpan);
-        auto xfer = sai_transfer_t{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
-        SAI_TransferReceiveEDMA(BOARD_AUDIOCODEC_SAIx, &rxHandle, &xfer);
     }
 
     void RT1051Audiocodec::OutStart()
@@ -279,15 +269,6 @@ namespace bsp
 
         /* Reset SAI Tx internal logic */
         SAI_TxSoftwareReset(BOARD_AUDIOCODEC_SAIx, kSAI_ResetTypeSoftware);
-
-        if (!sink.isConnected()) {
-            LOG_FATAL("No input stream connected!");
-            return;
-        }
-
-        auto nullSpan = sink.getStream()->getNullSpan();
-        auto xfer     = sai_transfer_t{.data = nullSpan.data, .dataSize = nullSpan.dataSize};
-        SAI_TransferSendEDMA(BOARD_AUDIOCODEC_SAIx, &txHandle, &xfer);
     }
 
     void RT1051Audiocodec::OutStop()
@@ -297,9 +278,6 @@ namespace bsp
             SAI_TransferTerminateSendEDMA(BOARD_AUDIOCODEC_SAIx, &txHandle);
         }
         memset(&txHandle, 0, sizeof(txHandle));
-        if (sink.isConnected()) {
-            sink.getStream()->unpeek();
-        }
     }
 
     void RT1051Audiocodec::InStop()
@@ -309,47 +287,18 @@ namespace bsp
             SAI_TransferAbortReceiveEDMA(BOARD_AUDIOCODEC_SAIx, &rxHandle);
         }
         memset(&rxHandle, 0, sizeof(rxHandle));
-        if (source.isConnected()) {
-            source.getStream()->release();
-        }
     }
 
     void rxAudioCodecCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData)
     {
-        audio::Stream::Span dataSpan;
-        auto self    = static_cast<RT1051Audiocodec *>(userData);
-        auto &source = self->source;
-
-        /// exit if not connected to the stream
-        if (!source.isConnected()) {
-            return;
-        }
-
-        /// reserve space for the next read commiting previously reserved block before
-        source.getStream()->commit();
-        source.getStream()->reserve(dataSpan);
-
-        sai_transfer_t xfer{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
-        SAI_TransferReceiveEDMA(BOARD_AUDIOCODEC_SAIx, &self->rxHandle, &xfer);
+        auto self = static_cast<RT1051Audiocodec *>(userData);
+        self->onDataReceive();
     }
 
     void txAudioCodecCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData)
     {
-        audio::Stream::Span dataSpan;
-        auto self  = static_cast<RT1051Audiocodec *>(userData);
-        auto &sink = self->sink;
-
-        /// exit if not connected to the stream
-        if (!sink.isConnected()) {
-            return;
-        }
-
-        /// pop previous read and peek next
-        sink.getStream()->consume();
-        sink.getStream()->peek(dataSpan);
-
-        sai_transfer_t xfer{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
-        SAI_TransferSendEDMA(BOARD_AUDIOCODEC_SAIx, &self->txHandle, &xfer);
+        auto self = static_cast<RT1051Audiocodec *>(userData);
+        self->onDataSend();
     }
 
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp
index 475eceefb..d32af4343 100755
--- a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp
@@ -3,7 +3,7 @@
 
 #pragma once
 
-#include "bsp/audio/bsp_audio.hpp"
+#include "SAIAudioDevice.hpp"
 #include "fsl_sai_edma.h"
 
 #include "FreeRTOS.h"
@@ -24,7 +24,7 @@ namespace bsp
     void txAudioCodecCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
     void rxAudioCodecCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
 
-    class RT1051Audiocodec : public AudioDevice
+    class RT1051Audiocodec : public SAIAudioDevice
     {
 
       public:
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.cpp b/module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.cpp
deleted file mode 100644
index 8578257ce..000000000
--- a/module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
-// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
-
-#include "RT1051BluetoothAudio.hpp"
-
-namespace bsp
-{
-    RT1051BluetoothAudio::RT1051BluetoothAudio(AudioDevice::audioCallback_t callback) : AudioDevice(callback)
-    {
-        isInitialized = true;
-        LOG_INFO("Device created!");
-    }
-    AudioDevice::RetCode RT1051BluetoothAudio::Start(const AudioDevice::Format &format)
-    {
-        LOG_INFO("Start");
-        Init();
-        if ((format.flags & static_cast<uint32_t>(AudioDevice::Flags::OutputMono)) != 0u) {
-            OutStart();
-        }
-        else if ((format.flags & static_cast<uint32_t>(AudioDevice::Flags::OutputStereo)) != 0u) {
-            OutStart();
-        }
-        return AudioDevice::RetCode::Success;
-    }
-    AudioDevice::RetCode RT1051BluetoothAudio::Stop()
-    {
-        return AudioDevice::RetCode::Success;
-    }
-    AudioDevice::RetCode RT1051BluetoothAudio::OutputVolumeCtrl(float vol)
-    {
-        return AudioDevice::RetCode::Success;
-    }
-    AudioDevice::RetCode RT1051BluetoothAudio::InputGainCtrl(float gain)
-    {
-        return AudioDevice::RetCode::Success;
-    }
-    AudioDevice::RetCode RT1051BluetoothAudio::OutputPathCtrl(AudioDevice::OutputPath outputPath)
-    {
-        return AudioDevice::RetCode::Success;
-    }
-    AudioDevice::RetCode RT1051BluetoothAudio::InputPathCtrl(AudioDevice::InputPath inputPath)
-    {
-        return AudioDevice::RetCode::Success;
-    }
-    bool RT1051BluetoothAudio::IsFormatSupported(const AudioDevice::Format &format)
-    {
-        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 (!fatalError) {
-            auto framesFetched = inst->GetAudioCallback()(nullptr, inst->audioData.data.data(), dataSize);
-            if (framesFetched == 0) {
-                break;
-            }
-            else if (framesFetched < inst->audioData.data.size()) {
-                std::fill(inst->audioData.data.begin() + framesFetched, inst->audioData.data.end(), 0);
-            }
-
-            if (inst->sourceQueue != nullptr) {
-                xQueueSend(inst->sourceQueue, inst->audioData.data.data(), 2);
-            }
-            else {
-                LOG_ERROR("Queue nullptr");
-                vTaskDelay(2);
-            }
-        }
-
-        inst->OutStop();
-        inst->outWorkerThread = nullptr;
-        vTaskDelete(nullptr);
-    }
-
-    void RT1051BluetoothAudio::OutStart()
-    {
-        if (xTaskCreate(outBluetoothAudioWorkerTask,
-                        "outbluetoothaudio",
-                        stackSize,
-                        this,
-                        tskIDLE_PRIORITY,
-                        &outWorkerThread) != pdPASS) {
-            LOG_ERROR("Error during creating  output bluetooth audio task");
-        }
-    }
-    void RT1051BluetoothAudio::OutStop()
-    {}
-
-    void RT1051BluetoothAudio::Init()
-    {}
-
-    RT1051BluetoothAudio::~RT1051BluetoothAudio()
-    {
-        Stop();
-    }
-} // namespace bsp
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.hpp b/module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.hpp
deleted file mode 100644
index 33daaf4ea..000000000
--- a/module-bsp/board/rt1051/bsp/audio/RT1051BluetoothAudio.hpp
+++ /dev/null
@@ -1,42 +0,0 @@
-// 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 "FreeRTOS.h"
-#include "macros.h"
-#include "task.h"
-#include "bsp/audio/bsp_audio.hpp"
-#include <module-bluetooth/Bluetooth/Device.hpp>
-#include <mutex.hpp>
-
-namespace bsp
-{
-    class RT1051BluetoothAudio final : public AudioDevice
-    {
-        static constexpr uint16_t stackSize = 1024;
-
-      public:
-        RT1051BluetoothAudio(AudioDevice::audioCallback_t callback);
-        ~RT1051BluetoothAudio() final;
-
-        AudioDevice::RetCode Start(const Format &format) final;
-        AudioDevice::RetCode Stop() final;
-        AudioDevice::RetCode OutputVolumeCtrl(float vol) final;
-        AudioDevice::RetCode InputGainCtrl(float gain) final;
-        AudioDevice::RetCode OutputPathCtrl(OutputPath outputPath) final;
-        AudioDevice::RetCode InputPathCtrl(InputPath inputPath) final;
-        bool IsFormatSupported(const Format &format) final;
-
-        TaskHandle_t outWorkerThread;
-        AudioData_t audioData;
-        QueueHandle_t sourceQueue;
-        DeviceMetadata_t metadata;
-        void OutStop();
-
-      private:
-        void Init();
-        void OutStart();
-    };
-
-} // namespace bsp
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp
index d422b2b65..568f80860 100644
--- a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp
@@ -19,7 +19,8 @@ namespace bsp
     sai_edma_handle_t RT1051CellularAudio::rxHandle = {};
 
     RT1051CellularAudio::RT1051CellularAudio(bsp::AudioDevice::audioCallback_t callback)
-        : AudioDevice(callback), saiInFormat{}, saiOutFormat{}, config{}
+        : SAIAudioDevice(callback, BOARD_CELLULAR_AUDIO_SAIx, &rxHandle, &txHandle), saiInFormat{},
+          saiOutFormat{}, config{}
     {
         isInitialized = true;
     }
@@ -199,17 +200,6 @@ namespace bsp
 
         /* Reset SAI Rx internal logic */
         SAI_RxSoftwareReset(BOARD_CELLULAR_AUDIO_SAIx, kSAI_ResetTypeSoftware);
-
-        if (!source.isConnected()) {
-            LOG_FATAL("No output stream connected!");
-            return;
-        }
-
-        /// initiate first read
-        audio::Stream::Span dataSpan;
-        source.getStream()->reserve(dataSpan);
-        auto xfer = sai_transfer_t{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
-        SAI_TransferReceiveEDMA(BOARD_CELLULAR_AUDIO_SAIx, &rxHandle, &xfer);
     }
 
     void RT1051CellularAudio::OutStart()
@@ -233,7 +223,6 @@ namespace bsp
                                        txCellularCallback,
                                        this,
                                        reinterpret_cast<edma_handle_t *>(txDMAHandle->GetHandle()));
-
         SAI_TransferTxSetFormatEDMA(
             BOARD_CELLULAR_AUDIO_SAIx, &txHandle, &sai_format, mclkSourceClockHz, mclkSourceClockHz);
 
@@ -241,15 +230,6 @@ namespace bsp
 
         /* Reset SAI Tx internal logic */
         SAI_TxSoftwareReset(BOARD_CELLULAR_AUDIO_SAIx, kSAI_ResetTypeSoftware);
-
-        if (!sink.isConnected()) {
-            LOG_FATAL("No input stream connected!");
-            return;
-        }
-
-        auto nullSpan = sink.getStream()->getNullSpan();
-        auto xfer     = sai_transfer_t{.data = nullSpan.data, .dataSize = nullSpan.dataSize};
-        SAI_TransferSendEDMA(BOARD_CELLULAR_AUDIO_SAIx, &txHandle, &xfer);
     }
 
     void RT1051CellularAudio::OutStop()
@@ -259,9 +239,6 @@ namespace bsp
             SAI_TransferTerminateSendEDMA(BOARD_CELLULAR_AUDIO_SAIx, &txHandle);
         }
         memset(&txHandle, 0, sizeof(txHandle));
-        if (sink.isConnected()) {
-            sink.getStream()->unpeek();
-        }
     }
 
     void RT1051CellularAudio::InStop()
@@ -271,47 +248,20 @@ namespace bsp
             SAI_TransferAbortReceiveEDMA(BOARD_CELLULAR_AUDIO_SAIx, &rxHandle);
         }
         memset(&rxHandle, 0, sizeof(rxHandle));
-        if (source.isConnected()) {
-            source.getStream()->release();
-        }
     }
 
     void rxCellularCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData)
     {
-        audio::Stream::Span dataSpan;
-        auto self    = static_cast<RT1051CellularAudio *>(userData);
-        auto &source = self->source;
+        auto self = static_cast<RT1051CellularAudio *>(userData);
 
-        /// exit if not connected to the stream
-        if (!source.isConnected()) {
-            return;
-        }
-
-        /// reserve space for the next read commiting previously reserved block before
-        source.getStream()->commit();
-        source.getStream()->reserve(dataSpan);
-
-        sai_transfer_t xfer{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
-        SAI_TransferReceiveEDMA(BOARD_CELLULAR_AUDIO_SAIx, &self->rxHandle, &xfer);
+        self->onDataReceive();
     }
 
     void txCellularCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData)
     {
-        audio::Stream::Span dataSpan;
-        auto self  = static_cast<RT1051CellularAudio *>(userData);
-        auto &sink = self->sink;
+        auto self = static_cast<RT1051CellularAudio *>(userData);
 
-        /// exit if not connected to the stream
-        if (!sink.isConnected()) {
-            return;
-        }
-
-        /// pop previous read and peek next
-        sink.getStream()->consume();
-        sink.getStream()->peek(dataSpan);
-
-        sai_transfer_t xfer{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
-        SAI_TransferSendEDMA(BOARD_CELLULAR_AUDIO_SAIx, &self->txHandle, &xfer);
+        self->onDataSend();
     }
 
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp
index 7caa68f9d..171d05a4e 100755
--- a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp
@@ -4,7 +4,7 @@
 #ifndef PUREPHONE_RT1051CELLULARAUDIO_HPP
 #define PUREPHONE_RT1051CELLULARAUDIO_HPP
 
-#include "bsp/audio/bsp_audio.hpp"
+#include "SAIAudioDevice.hpp"
 #include "fsl_sai_edma.h"
 
 #include "FreeRTOS.h"
@@ -23,7 +23,7 @@ namespace bsp
     void txCellularCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
     void rxCellularCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
 
-    class RT1051CellularAudio : public AudioDevice
+    class RT1051CellularAudio : public SAIAudioDevice
     {
 
       public:
diff --git a/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp
new file mode 100644
index 000000000..38f61b722
--- /dev/null
+++ b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp
@@ -0,0 +1,97 @@
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "SAIAudioDevice.hpp"
+
+using namespace bsp;
+
+SAIAudioDevice::SAIAudioDevice(AudioDevice::audioCallback_t callback,
+                               I2S_Type *base,
+                               sai_edma_handle_t *rxHandle,
+                               sai_edma_handle_t *txHandle)
+    : AudioDevice(callback), _base(base), rx(rxHandle), tx(txHandle)
+{}
+
+void SAIAudioDevice::initiateRxTransfer()
+{
+    audio::Stream::Span dataSpan;
+
+    Source::_stream->reserve(dataSpan);
+    auto xfer = sai_transfer_t{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
+    SAI_TransferReceiveEDMA(_base, rx, &xfer);
+}
+
+void SAIAudioDevice::initiateTxTransfer()
+{
+    auto nullSpan = Sink::_stream->getNullSpan();
+    auto xfer     = sai_transfer_t{.data = nullSpan.data, .dataSize = nullSpan.dataSize};
+    SAI_TransferSendEDMA(_base, tx, &xfer);
+}
+
+void SAIAudioDevice::onDataSend()
+{
+    audio::Stream::Span dataSpan;
+
+    if (!txEnabled || !isSinkConnected()) {
+        return;
+    }
+
+    /// pop previous read and peek next
+    Sink::_stream->consume();
+    Sink::_stream->peek(dataSpan);
+
+    sai_transfer_t xfer{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
+    SAI_TransferSendEDMA(_base, tx, &xfer);
+}
+
+void SAIAudioDevice::onDataReceive()
+{
+    audio::Stream::Span dataSpan;
+
+    if (!rxEnabled || !isSourceConnected()) {
+        return;
+    }
+
+    /// reserve space for the next read commiting previously reserved block before
+    Source::_stream->commit();
+    Source::_stream->reserve(dataSpan);
+
+    sai_transfer_t xfer{.data = dataSpan.data, .dataSize = dataSpan.dataSize};
+    SAI_TransferReceiveEDMA(_base, rx, &xfer);
+}
+
+void SAIAudioDevice::enableInput()
+{
+    if (!isSourceConnected()) {
+        LOG_FATAL("No output stream connected!");
+        return;
+    }
+
+    rxEnabled = true;
+
+    /// initiate first read
+    initiateRxTransfer();
+}
+
+void SAIAudioDevice::enableOutput()
+{
+    if (!isSinkConnected()) {
+        LOG_FATAL("No input stream connected!");
+        return;
+    }
+
+    txEnabled = true;
+
+    /// initiate first write
+    initiateTxTransfer();
+}
+
+void SAIAudioDevice::disableInput()
+{
+    rxEnabled = false;
+}
+
+void SAIAudioDevice::disableOutput()
+{
+    txEnabled = false;
+}
diff --git a/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp
new file mode 100644
index 000000000..b51e91cb2
--- /dev/null
+++ b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp
@@ -0,0 +1,38 @@
+// 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 "bsp/audio/bsp_audio.hpp"
+
+#include "fsl_sai_edma.h"
+
+namespace bsp
+{
+
+    class SAIAudioDevice : public bsp::AudioDevice
+    {
+      public:
+        SAIAudioDevice(AudioDevice::audioCallback_t callback,
+                       I2S_Type *base,
+                       sai_edma_handle_t *rxHandle,
+                       sai_edma_handle_t *txHandle);
+
+        void onDataSend() override;
+        void onDataReceive() override;
+        void enableInput() override;
+        void enableOutput() override;
+        void disableInput() override;
+        void disableOutput() override;
+
+      protected:
+        void initiateRxTransfer();
+        void initiateTxTransfer();
+        I2S_Type *_base;
+        sai_edma_handle_t *rx = nullptr;
+        sai_edma_handle_t *tx = nullptr;
+        bool txEnabled        = false;
+        bool rxEnabled        = false;
+    };
+
+} // namespace bsp
diff --git a/module-bsp/bsp/audio/bsp_audio.cpp b/module-bsp/bsp/audio/bsp_audio.cpp
index 4bd91fbf3..d13186c36 100644
--- a/module-bsp/bsp/audio/bsp_audio.cpp
+++ b/module-bsp/bsp/audio/bsp_audio.cpp
@@ -4,7 +4,10 @@
 
 #include "board/rt1051/bsp/audio/RT1051Audiocodec.hpp"
 #include "board/rt1051/bsp/audio/RT1051CellularAudio.hpp"
-#include "board/rt1051/bsp/audio/RT1051BluetoothAudio.hpp"
+
+#include <Audio/Stream.hpp>
+
+#include <cassert>
 
 #elif defined(TARGET_Linux)
 #include "audio/linux_audiocodec.hpp"
@@ -13,60 +16,53 @@
 #error "Unsupported target"
 #endif
 
-namespace bsp{
-
-    std::optional<std::unique_ptr<AudioDevice>> AudioDevice::Create(bsp::AudioDevice::Type type, audioCallback_t callback) {
+namespace bsp
+{
 
+    std::optional<std::unique_ptr<AudioDevice>> AudioDevice::Create(bsp::AudioDevice::Type type,
+                                                                    audioCallback_t callback)
+    {
         std::unique_ptr<AudioDevice> inst;
 
-        switch(type){
+        switch (type) {
 
-            case Type ::Audiocodec:
-            {
+        case Type ::Audiocodec: {
 #if defined(TARGET_RT1051)
-                inst = std::make_unique<bsp::RT1051Audiocodec>(callback);
+            inst = std::make_unique<bsp::RT1051Audiocodec>(callback);
 #elif defined(TARGET_Linux)
-                inst = std::make_unique<bsp::LinuxAudiocodec>(callback );
+            inst = std::make_unique<bsp::LinuxAudiocodec>(callback);
 #else
-                #error "Unsupported target"
+#error "Unsupported target"
 #endif
 
-            }
-                break;
+        } break;
 
-
-            case Type ::Bluetooth:
-            {
+        case Type ::Bluetooth: {
 #if defined(TARGET_RT1051)
-                inst = std::make_unique<bsp::RT1051BluetoothAudio>(callback);
+            inst = nullptr;
 
 #elif defined(TARGET_Linux)
 
 #else
 #error "Unsupported target"
 #endif
-            }
-                break;
+        } break;
 
-            case Type::Cellular:
-            {
+        case Type::Cellular: {
 #if defined(TARGET_RT1051)
-                inst = std::make_unique<bsp::RT1051CellularAudio>(callback);
+            inst = std::make_unique<bsp::RT1051CellularAudio>(callback);
 #elif defined(TARGET_Linux)
-                inst = std::make_unique<bsp::LinuxCellularAudio>(callback );
+            inst = std::make_unique<bsp::LinuxCellularAudio>(callback);
 #else
 #error "Unsupported target"
 #endif
-            }
-                break;
-
+        } break;
         }
 
-        if(inst->isInitialized){
+        if (inst->isInitialized) {
             return inst;
         }
 
         return {};
     }
-
-}
+} // namespace bsp
diff --git a/module-bsp/bsp/audio/bsp_audio.hpp b/module-bsp/bsp/audio/bsp_audio.hpp
index fb9f6d27f..99a7da2d0 100755
--- a/module-bsp/bsp/audio/bsp_audio.hpp
+++ b/module-bsp/bsp/audio/bsp_audio.hpp
@@ -9,7 +9,7 @@
 namespace bsp
 {
 
-    class AudioDevice
+    class AudioDevice : public audio::IOProxy
     {
 
       public:
@@ -140,9 +140,6 @@ namespace bsp
             return callback;
         }
 
-        audio::Sink sink;
-        audio::Source source;
-
       protected:
         Format currentFormat;
         audioCallback_t callback = nullptr;
diff --git a/module-bsp/targets/Target_RT1051.cmake b/module-bsp/targets/Target_RT1051.cmake
index 3c9259086..1fd314675 100644
--- a/module-bsp/targets/Target_RT1051.cmake
+++ b/module-bsp/targets/Target_RT1051.cmake
@@ -58,7 +58,7 @@ set(BOARD_SOURCES ${BOARD_SOURCES}
 	"${CMAKE_CURRENT_SOURCE_DIR}/board/rt1051/bsp/battery-charger/battery_charger.cpp"
 	"${CMAKE_CURRENT_SOURCE_DIR}/board/rt1051/bsp/audio/RT1051Audiocodec.cpp"
 	"${CMAKE_CURRENT_SOURCE_DIR}/board/rt1051/bsp/audio/RT1051CellularAudio.cpp"
-	"${CMAKE_CURRENT_SOURCE_DIR}/board/rt1051/bsp/audio/RT1051BluetoothAudio.cpp"
+	"${CMAKE_CURRENT_SOURCE_DIR}/board/rt1051/bsp/audio/SAIAudioDevice.cpp"
 	"${CMAKE_CURRENT_SOURCE_DIR}/board/rt1051/bsp/audio/CodecMAX98090.cpp"
 	"${CMAKE_CURRENT_SOURCE_DIR}/board/rt1051/bsp/audio/qfilter.c"
 	"${USB_SRC}"
diff --git a/module-sys/Service/Worker.cpp b/module-sys/Service/Worker.cpp
index a4ebef94f..7c9386460 100644
--- a/module-sys/Service/Worker.cpp
+++ b/module-sys/Service/Worker.cpp
@@ -237,6 +237,11 @@ namespace sys
         return getControlQueue().Enqueue(&messageToSend, portMAX_DELAY);
     }
 
+    bool Worker::sendCommand(WorkerCommand command)
+    {
+        return getServiceQueue().Enqueue(&command);
+    }
+
     bool Worker::send(uint32_t cmd, uint32_t *data)
     {
         assert(xTaskGetCurrentTaskHandle() == runnerTask);
diff --git a/module-sys/Service/Worker.hpp b/module-sys/Service/Worker.hpp
index 45966f703..a28c47e46 100644
--- a/module-sys/Service/Worker.hpp
+++ b/module-sys/Service/Worker.hpp
@@ -98,7 +98,6 @@ namespace sys
         std::optional<size_t> controlQueueIndex;
         std::optional<size_t> serviceQueueIndex;
         WorkerQueue &getControlQueue() const;
-        WorkerQueue &getServiceQueue() const;
 
         static constexpr std::size_t controlMessagesCount = static_cast<std::size_t>(ControlMessage::MessageCount);
         static constexpr std::size_t defaultStackSize     = 2048;
@@ -117,10 +116,13 @@ namespace sys
       protected:
         virtual bool handleMessage(uint32_t queueID) = 0;
 
+        WorkerQueue &getServiceQueue() const;
+
         xQueueHandle getQueueHandleByName(const std::string &qname) const;
         std::shared_ptr<WorkerQueue> getQueueByName(const std::string &qname) const;
 
         bool sendControlMessage(ControlMessage message);
+        bool sendCommand(WorkerCommand command);
         State getState() const;
 
         const static uint32_t SERVICE_QUEUE_LENGTH = 10;