From f1fc9df1527237c0449585bcaa620851583a271e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Marcin=20Smoczy=C5=84ski?= <smoczynski.marcin@gmail.com>
Date: Tue, 12 Jan 2021 13:31:39 +0100
Subject: [PATCH] [EGD-4977] Reduce audio lag during voice call
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Reduce audio delay by reducing audio buffer size in router operation.
Audio streams are now created directly in the operations, not in the
audio service, which gives more flexibility.

Audio Buffer size is calculated based on endpoints (source, sink) and
operation capabilities. This commit also enables allocations in a
non-cacheable region of OCRAM for endpoints that use DMA for data
transport.

Introduce power-of-two operations that use built-in functions and
possibly dedicated hardware instructions of an MCU. These operations
are required by the audio stream buffer size calculation algorithm.

Signed-off-by: Marcin Smoczyński <smoczynski.marcin@gmail.com>
---
 module-audio/Audio/Audio.cpp                  |  4 +-
 module-audio/Audio/Audio.hpp                  | 28 +++-------
 module-audio/Audio/BluetoothProxyAudio.cpp    |  4 +-
 module-audio/Audio/Endpoint.cpp               | 11 +++-
 module-audio/Audio/Endpoint.hpp               | 18 +++++-
 module-audio/Audio/Operation/Operation.cpp    | 13 ++---
 module-audio/Audio/Operation/Operation.hpp    | 29 +++-------
 .../Audio/Operation/PlaybackOperation.cpp     | 14 ++++-
 .../Audio/Operation/PlaybackOperation.hpp     | 10 +++-
 .../Audio/Operation/RecorderOperation.cpp     | 10 ++--
 .../Audio/Operation/RouterOperation.cpp       | 18 ++++--
 .../Audio/Operation/RouterOperation.hpp       |  9 ++-
 module-audio/Audio/StreamFactory.cpp          | 55 +++++++++++++++++++
 module-audio/Audio/StreamFactory.hpp          | 29 ++++++++++
 module-audio/Audio/decoder/Decoder.cpp        |  5 +-
 module-audio/Audio/decoder/Decoder.hpp        |  5 +-
 module-audio/CMakeLists.txt                   |  1 +
 .../rt1051/bsp/audio/RT1051Audiocodec.cpp     |  6 +-
 .../rt1051/bsp/audio/RT1051Audiocodec.hpp     |  4 +-
 .../rt1051/bsp/audio/RT1051CellularAudio.cpp  |  7 +--
 .../rt1051/bsp/audio/RT1051CellularAudio.hpp  |  4 +-
 .../board/rt1051/bsp/audio/SAIAudioDevice.cpp |  7 +--
 .../board/rt1051/bsp/audio/SAIAudioDevice.hpp |  7 +--
 module-bsp/bsp/audio/bsp_audio.cpp            | 32 +++--------
 module-bsp/bsp/audio/bsp_audio.hpp            | 40 ++------------
 module-bsp/targets/Target_Linux.cmake         |  3 -
 module-utils/math/Math.hpp                    | 40 +++++++++++++-
 module-utils/test/test_math.cpp               | 40 +++++++++++++-
 28 files changed, 290 insertions(+), 163 deletions(-)
 create mode 100644 module-audio/Audio/StreamFactory.cpp
 create mode 100644 module-audio/Audio/StreamFactory.hpp

diff --git a/module-audio/Audio/Audio.cpp b/module-audio/Audio/Audio.cpp
index 887c3471dcea1874a98b8d80a1ba232e5bb014fd..7acc778e597f54b0573d83a47a5f6a4d51547f4f 100644
--- a/module-audio/Audio/Audio.cpp
+++ b/module-audio/Audio/Audio.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "Audio.hpp"
@@ -90,8 +90,6 @@ namespace audio
                 break;
             }
             currentOperation = std::move(ret);
-            currentOperation->SetDataStreams(&dataStreamOut, &dataStreamIn);
-
             UpdateProfiles();
         }
         catch (const AudioInitException &audioException) {
diff --git a/module-audio/Audio/Audio.hpp b/module-audio/Audio/Audio.hpp
index 2a42fb9a426a5d8ac9759232d92d263ba356990c..a110c6ab03f683e0bf3c6adc676baeb2bf9f4f6d 100644
--- a/module-audio/Audio/Audio.hpp
+++ b/module-audio/Audio/Audio.hpp
@@ -1,19 +1,18 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
 
-#include <memory>
-#include <optional>
-#include <functional>
-#include <bitset>
-
 #include <service-bluetooth/ServiceBluetoothCommon.hpp>
 
 #include "AudioCommon.hpp"
-#include "Stream.hpp"
-#include "Operation/Operation.hpp"
 #include "decoder/Decoder.hpp"
+#include "Operation/Operation.hpp"
+
+#include <bitset>
+#include <functional>
+#include <memory>
+#include <optional>
 
 namespace audio
 {
@@ -96,17 +95,14 @@ namespace audio
                                      const audio::PlaybackType &playbackType = audio::PlaybackType::None);
 
         virtual audio::RetCode Start();
-
         virtual audio::RetCode Stop();
-
         virtual audio::RetCode Pause();
-
         virtual audio::RetCode Resume();
-
         virtual audio::RetCode Mute();
 
       private:
         void UpdateProfiles();
+
         AudioSinkState audioSinkState;
 
         std::shared_ptr<BluetoothStreamData> btData;
@@ -115,14 +111,6 @@ namespace audio
         std::unique_ptr<Operation> currentOperation;
 
         AudioServiceMessage::Callback serviceCallback;
-
-        // for efficiency multiple of 24 and 32 (max audio samples size)
-        static constexpr auto defaultAudioStreamBlockSize = 2048;
-        StandardStreamAllocator allocatorOut;
-        Stream dataStreamOut{allocatorOut, defaultAudioStreamBlockSize};
-
-        StandardStreamAllocator allocatorIn;
-        Stream dataStreamIn{allocatorIn, defaultAudioStreamBlockSize};
     };
 
 } // namespace audio
diff --git a/module-audio/Audio/BluetoothProxyAudio.cpp b/module-audio/Audio/BluetoothProxyAudio.cpp
index a7084eddfde7aceb5be97d9c987215d780d03c4e..f7e68d81ecf437f87c39665dca218c5e3221155d 100644
--- a/module-audio/Audio/BluetoothProxyAudio.cpp
+++ b/module-audio/Audio/BluetoothProxyAudio.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "BluetoothProxyAudio.hpp"
@@ -10,7 +10,7 @@ namespace bsp
                                              audio::Stream &dataStreamOut,
                                              audio::Stream &dataStreamIn,
                                              AudioDevice::Format &format)
-        : AudioDevice(nullptr), dataStreamOut(dataStreamOut), dataStreamIn(dataStreamIn), serviceCallback(callback),
+        : dataStreamOut(dataStreamOut), dataStreamIn(dataStreamIn), serviceCallback(std::move(callback)),
           audioFormat(format)
     {
         LOG_DEBUG("BluetoothProxyAudio created.");
diff --git a/module-audio/Audio/Endpoint.cpp b/module-audio/Audio/Endpoint.cpp
index 9f6c6d2a8924310fce67bccd52b22c74d672e503..1ec4d3f42dbaa86ab49590c5a4d59988a3389312 100644
--- a/module-audio/Audio/Endpoint.cpp
+++ b/module-audio/Audio/Endpoint.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "Endpoint.hpp"
@@ -32,6 +32,15 @@ bool Endpoint::isConnected() const noexcept
     return _stream != nullptr;
 }
 
+Sink::Sink(const Capabilities &caps) : Endpoint(caps)
+{}
+
+Source::Source(const Capabilities &caps) : Endpoint(caps)
+{}
+
+IOProxy::IOProxy(const Capabilities &sourceCaps, const Capabilities &sinkCaps) : Source(sourceCaps), Sink(sinkCaps)
+{}
+
 StreamConnection::StreamConnection(Source *source, Sink *sink, Stream *stream)
     : _sink(sink), _source(source), _stream(stream)
 {
diff --git a/module-audio/Audio/Endpoint.hpp b/module-audio/Audio/Endpoint.hpp
index b291e83b0815db026a83e6cd8fb2d3faacb04d49..89ac7c85eb748fd0ae28fcd450a8e2466035a631 100644
--- a/module-audio/Audio/Endpoint.hpp
+++ b/module-audio/Audio/Endpoint.hpp
@@ -1,10 +1,12 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
 
 #include "Stream.hpp"
 
+#include <cstdint>
+
 namespace audio
 {
     class Endpoint
@@ -13,7 +15,8 @@ namespace audio
         struct Capabilities
         {
             bool usesDMA             = false;
-            std::size_t maxBlockSize = 0;
+            std::size_t minBlockSize = 1;
+            std::size_t maxBlockSize = SIZE_MAX;
         };
 
         Endpoint() = default;
@@ -33,6 +36,9 @@ namespace audio
     class Sink : public Endpoint
     {
       public:
+        Sink() = default;
+        explicit Sink(const Capabilities &caps);
+
         virtual void onDataSend()    = 0;
         virtual void enableOutput()  = 0;
         virtual void disableOutput() = 0;
@@ -41,14 +47,20 @@ namespace audio
     class Source : public Endpoint
     {
       public:
+        Source() = default;
+        explicit Source(const Capabilities &caps);
+
         virtual void onDataReceive() = 0;
         virtual void enableInput()   = 0;
         virtual void disableInput()  = 0;
     };
 
-    class IOProxy : public Sink, public Source
+    class IOProxy : public Source, public Sink
     {
       public:
+        IOProxy() = default;
+        IOProxy(const Capabilities &sourceCaps, const Capabilities &sinkCaps);
+
         inline bool isSinkConnected() const noexcept
         {
             return Sink::isConnected();
diff --git a/module-audio/Audio/Operation/Operation.cpp b/module-audio/Audio/Operation/Operation.cpp
index 3284ddc5b44fddd648f31a769c30b8d007f3b3b3..bae1785cb64afb7401df36018913d5566dfa05c7 100644
--- a/module-audio/Audio/Operation/Operation.cpp
+++ b/module-audio/Audio/Operation/Operation.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "Operation.hpp"
@@ -93,14 +93,9 @@ namespace audio
 
         supportedProfiles.emplace_back(Profile::Create(profile, nullptr, volume, gain), isAvailable);
     }
-    std::optional<std::unique_ptr<bsp::AudioDevice>> Operation::CreateDevice(bsp::AudioDevice::Type type,
-                                                                             bsp::AudioDevice::audioCallback_t callback)
+
+    std::optional<std::unique_ptr<bsp::AudioDevice>> Operation::CreateDevice(bsp::AudioDevice::Type type)
     {
-        if (type == bsp::AudioDevice::Type::Bluetooth) {
-            auto audioFormat = currentProfile->GetAudioFormat();
-            return std::make_unique<bsp::BluetoothProxyAudio>(
-                serviceCallback, *dataStreamOut, *dataStreamIn, audioFormat);
-        }
-        return bsp::AudioDevice::Create(type, callback).value_or(nullptr);
+        return bsp::AudioDevice::Create(type).value_or(nullptr);
     }
 } // namespace audio
diff --git a/module-audio/Audio/Operation/Operation.hpp b/module-audio/Audio/Operation/Operation.hpp
index 21416f595842cbe1e0ca5e21e8d59bdcd1b81e24..126ca4fb8c3d278f45ae9ef89bd54d46738c77e6 100644
--- a/module-audio/Audio/Operation/Operation.hpp
+++ b/module-audio/Audio/Operation/Operation.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -8,7 +8,6 @@
 #include <functional>
 
 #include <Audio/AudioCommon.hpp>
-#include <Audio/Stream.hpp>
 #include <Audio/encoder/Encoder.hpp>
 #include <Audio/Profiles/Profile.hpp>
 
@@ -60,13 +59,13 @@ namespace audio
                                                  const audio::PlaybackType &operations  = audio::PlaybackType::None,
                                                  AudioServiceMessage::Callback callback = nullptr);
 
-        virtual audio::RetCode Start(audio::Token token)                                = 0;
-        virtual audio::RetCode Stop()                                                   = 0;
-        virtual audio::RetCode Pause()                                                  = 0;
-        virtual audio::RetCode Resume()                                                 = 0;
-        virtual audio::RetCode SendEvent(std::shared_ptr<Event> evt)                    = 0;
-        virtual audio::RetCode SetOutputVolume(float vol)                               = 0;
-        virtual audio::RetCode SetInputGain(float gain)                                 = 0;
+        virtual audio::RetCode Start(audio::Token token)             = 0;
+        virtual audio::RetCode Stop()                                = 0;
+        virtual audio::RetCode Pause()                               = 0;
+        virtual audio::RetCode Resume()                              = 0;
+        virtual audio::RetCode SendEvent(std::shared_ptr<Event> evt) = 0;
+        virtual audio::RetCode SetOutputVolume(float vol)            = 0;
+        virtual audio::RetCode SetInputGain(float gain)              = 0;
 
         virtual Position GetPosition() = 0;
 
@@ -112,12 +111,6 @@ namespace audio
 
         audio::RetCode SwitchToPriorityProfile();
 
-        void SetDataStreams(Stream *dStreamOut, Stream *dStreamIn)
-        {
-            dataStreamOut = dStreamOut;
-            dataStreamIn  = dStreamIn;
-        }
-
       protected:
         struct SupportedProfile
         {
@@ -129,9 +122,6 @@ namespace audio
             bool isAvailable;
         };
 
-        Stream *dataStreamOut = nullptr;
-        Stream *dataStreamIn  = nullptr;
-
         std::shared_ptr<Profile> currentProfile;
         std::unique_ptr<bsp::AudioDevice> audioDevice;
         std::vector<SupportedProfile> supportedProfiles;
@@ -152,8 +142,7 @@ namespace audio
         virtual audio::RetCode SwitchProfile(const Profile::Type type) = 0;
         std::shared_ptr<Profile> GetProfile(const Profile::Type type);
 
-        std::optional<std::unique_ptr<bsp::AudioDevice>> CreateDevice(bsp::AudioDevice::Type type,
-                                                                      bsp::AudioDevice::audioCallback_t callback);
+        std::optional<std::unique_ptr<bsp::AudioDevice>> CreateDevice(bsp::AudioDevice::Type type);
     };
 
 } // namespace audio
diff --git a/module-audio/Audio/Operation/PlaybackOperation.cpp b/module-audio/Audio/Operation/PlaybackOperation.cpp
index 30fe261c4cc015ea2d73dddb95af15720b696ff4..1e34115e514e3790997274018444e259dbfa0abe 100644
--- a/module-audio/Audio/Operation/PlaybackOperation.cpp
+++ b/module-audio/Audio/Operation/PlaybackOperation.cpp
@@ -1,10 +1,11 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "PlaybackOperation.hpp"
 
 #include "Audio/decoder/Decoder.hpp"
 #include "Audio/Profiles/Profile.hpp"
+#include "Audio/StreamFactory.hpp"
 
 #include "Audio/AudioCommon.hpp"
 
@@ -55,8 +56,12 @@ namespace audio
             return RetCode::InvokedInIncorrectState;
         }
 
+        // create stream
+        StreamFactory streamFactory(playbackCapabilities);
+        dataStreamOut = streamFactory.makeStream(*dec.get(), *audioDevice.get());
+
         // create audio connection
-        outputConnection = std::make_unique<StreamConnection>(dec.get(), audioDevice.get(), dataStreamOut);
+        outputConnection = std::make_unique<StreamConnection>(dec.get(), audioDevice.get(), dataStreamOut.get());
 
         // decoder worker soft start - must be called after connection setup
         dec->startDecodingWorker(endOfFileCallback);
@@ -82,6 +87,7 @@ namespace audio
         // stop playback by destroying audio connection
         outputConnection.reset();
         dec->stopDecodingWorker();
+        dataStreamOut.reset();
 
         return GetDeviceError(audioDevice->Stop());
     }
@@ -154,8 +160,10 @@ namespace audio
         /// profile change - (re)create output device; stop audio first by
         /// killing audio connection
         outputConnection.reset();
+        dec->stopDecodingWorker();
         audioDevice.reset();
-        audioDevice = CreateDevice(newProfile->GetAudioDeviceType(), audioCallback).value_or(nullptr);
+        dataStreamOut.reset();
+        audioDevice = CreateDevice(newProfile->GetAudioDeviceType()).value_or(nullptr);
         if (audioDevice == nullptr) {
             LOG_ERROR("Error creating AudioDevice");
             return RetCode::Failed;
diff --git a/module-audio/Audio/Operation/PlaybackOperation.hpp b/module-audio/Audio/Operation/PlaybackOperation.hpp
index de0f1abfecfcc216e96f1871a51bdb924393dab0..a1cd40248a5f80ea957d76dfecae427e84bc5ccc 100644
--- a/module-audio/Audio/Operation/PlaybackOperation.hpp
+++ b/module-audio/Audio/Operation/PlaybackOperation.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -39,9 +39,15 @@ namespace audio
         Position GetPosition() final;
 
       private:
+        static constexpr auto minimumBlockSize = 256U;
+        static constexpr auto maximumBlockSize = 2048U;
+        static constexpr Endpoint::Capabilities playbackCapabilities{.minBlockSize = minimumBlockSize,
+                                                                     .maxBlockSize = maximumBlockSize};
+
+        std::unique_ptr<Stream> dataStreamOut;
         std::unique_ptr<Decoder> dec;
         std::unique_ptr<Tags> tags;
-        std::unique_ptr<StreamConnection> outputConnection = nullptr;
+        std::unique_ptr<StreamConnection> outputConnection;
 
         DecoderWorker::EndOfFileCallback endOfFileCallback;
     };
diff --git a/module-audio/Audio/Operation/RecorderOperation.cpp b/module-audio/Audio/Operation/RecorderOperation.cpp
index 3b891860da2c5a8fb133cf05219bc1d05daa00c0..747853e214c4d0f15e22230f6483e0e5efda1ce8 100644
--- a/module-audio/Audio/Operation/RecorderOperation.cpp
+++ b/module-audio/Audio/Operation/RecorderOperation.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "RecorderOperation.hpp"
@@ -37,7 +37,7 @@ namespace audio
             // during sf tests on hardware
 #endif
             if (ret == 0) {
-                state = State::Idle;
+                state          = State::Idle;
                 const auto req = AudioServiceMessage::FileSystemNoSpace(operationToken);
                 serviceCallback(&req);
             }
@@ -82,7 +82,7 @@ namespace audio
             return RetCode::InvokedInIncorrectState;
         }
         operationToken = token;
-        state         = State::Active;
+        state          = State::Active;
 
         if (audioDevice->IsFormatSupported(currentProfile->GetAudioFormat())) {
             auto ret = audioDevice->Start(currentProfile->GetAudioFormat());
@@ -122,7 +122,7 @@ namespace audio
             return RetCode::InvokedInIncorrectState;
         }
 
-        state = State::Active;
+        state    = State::Active;
         auto ret = audioDevice->Start(currentProfile->GetAudioFormat());
         return GetDeviceError(ret);
     }
@@ -157,7 +157,7 @@ namespace audio
             return RetCode::UnsupportedProfile;
         }
 
-        audioDevice = CreateDevice(currentProfile->GetAudioDeviceType(), audioCallback).value_or(nullptr);
+        audioDevice = CreateDevice(currentProfile->GetAudioDeviceType()).value_or(nullptr);
         if (audioDevice == nullptr) {
             LOG_ERROR("Error creating AudioDevice");
             return RetCode::Failed;
diff --git a/module-audio/Audio/Operation/RouterOperation.cpp b/module-audio/Audio/Operation/RouterOperation.cpp
index 051bb78084a321094f21e65946c9d8914b1d1ce5..f840045d63457ee29202dd15dcab2c01124679a9 100644
--- a/module-audio/Audio/Operation/RouterOperation.cpp
+++ b/module-audio/Audio/Operation/RouterOperation.cpp
@@ -1,10 +1,11 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "RouterOperation.hpp"
 
 #include <Audio/AudioCommon.hpp>
 #include <Audio/Profiles/Profile.hpp>
+#include <Audio/StreamFactory.hpp>
 
 #include <bsp_audio.hpp>
 #include <log/log.hpp>
@@ -69,11 +70,16 @@ namespace audio
             return GetDeviceError(ret);
         }
 
+        // create streams
+        StreamFactory streamFactory(routerCapabilities);
+        dataStreamIn  = streamFactory.makeStream(*audioDevice.get(), *audioDeviceCellular.get());
+        dataStreamOut = streamFactory.makeStream(*audioDevice.get(), *audioDeviceCellular.get());
+
         // 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);
+            std::make_unique<audio::StreamConnection>(audioDeviceCellular.get(), audioDevice.get(), dataStreamIn.get());
+        outputConnection = std::make_unique<audio::StreamConnection>(
+            audioDevice.get(), audioDeviceCellular.get(), dataStreamOut.get());
 
         // enable audio connections
         inputConnection->enable();
@@ -174,13 +180,13 @@ namespace audio
             Stop();
         }
 
-        audioDevice = CreateDevice(newProfile->GetAudioDeviceType(), nullptr).value_or(nullptr);
+        audioDevice = CreateDevice(newProfile->GetAudioDeviceType()).value_or(nullptr);
         if (audioDevice == nullptr) {
             LOG_ERROR("Error creating AudioDevice");
             return RetCode::Failed;
         }
 
-        audioDeviceCellular = CreateDevice(bsp::AudioDevice::Type::Cellular, nullptr).value_or(nullptr);
+        audioDeviceCellular = CreateDevice(bsp::AudioDevice::Type::Cellular).value_or(nullptr);
         if (audioDeviceCellular == nullptr) {
             LOG_ERROR("Error creating AudioDeviceCellular");
             return RetCode::Failed;
diff --git a/module-audio/Audio/Operation/RouterOperation.hpp b/module-audio/Audio/Operation/RouterOperation.hpp
index d6f88440a40ad35501d711a400b58faacbef8561..02c410696cb7cedefc86b9d126374248be9957ca 100644
--- a/module-audio/Audio/Operation/RouterOperation.hpp
+++ b/module-audio/Audio/Operation/RouterOperation.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -43,9 +43,16 @@ namespace audio
         Position GetPosition() final;
 
       private:
+        static constexpr auto minimumBlockSize = 64U;
+        static constexpr auto maximumBlockSize = 64U;
+        static constexpr Endpoint::Capabilities routerCapabilities{.minBlockSize = minimumBlockSize,
+                                                                   .maxBlockSize = maximumBlockSize};
+
         bool Mute(bool enable);
 
         bool muteEnable = false;
+        std::unique_ptr<Stream> dataStreamOut;
+        std::unique_ptr<Stream> dataStreamIn;
         std::unique_ptr<Encoder> enc;
         std::unique_ptr<bsp::AudioDevice> audioDeviceCellular;
         std::unique_ptr<StreamConnection> outputConnection;
diff --git a/module-audio/Audio/StreamFactory.cpp b/module-audio/Audio/StreamFactory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..78339d542fbc95aafb07a282e56eaaf71fe7920b
--- /dev/null
+++ b/module-audio/Audio/StreamFactory.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "StreamFactory.hpp"
+#include "Endpoint.hpp"
+
+#include <math/Math.hpp>
+
+#include <algorithm>
+#include <memory>
+#include <vector>
+
+#include <cassert>
+
+using namespace audio;
+
+StreamFactory::StreamFactory(Endpoint::Capabilities factoryCaps) : caps(std::move(factoryCaps))
+{}
+
+auto StreamFactory::makeStream(const Source &source, const Sink &sink) -> std::unique_ptr<Stream>
+{
+    auto negotiatedCaps = negotiateCaps({source, sink});
+
+    return std::make_unique<Stream>(getAllocator(negotiatedCaps.usesDMA), negotiatedCaps.maxBlockSize);
+}
+
+auto StreamFactory::negotiateCaps(std::vector<std::reference_wrapper<const Endpoint>> v) -> Endpoint::Capabilities
+{
+    auto negotiatedCaps = caps;
+
+    for (const auto &endpointRef : v) {
+        auto &endpointCaps = endpointRef.get().getCapabilities();
+
+        negotiatedCaps.maxBlockSize = std::min(negotiatedCaps.maxBlockSize, endpointCaps.maxBlockSize);
+        negotiatedCaps.minBlockSize = std::max(negotiatedCaps.minBlockSize, endpointCaps.minBlockSize);
+        negotiatedCaps.usesDMA      = negotiatedCaps.usesDMA || endpointCaps.usesDMA;
+    }
+
+    negotiatedCaps.minBlockSize = binary::ceilPowerOfTwo(negotiatedCaps.minBlockSize);
+    negotiatedCaps.maxBlockSize = binary::floorPowerOfTwo(negotiatedCaps.maxBlockSize);
+
+    assert(negotiatedCaps.minBlockSize <= negotiatedCaps.maxBlockSize);
+
+    return negotiatedCaps;
+}
+
+auto StreamFactory::getAllocator(bool usesDMA) -> Stream::Allocator &
+{
+    if (usesDMA) {
+        return nonCacheableAlloc;
+    }
+    else {
+        return stdAlloc;
+    }
+}
diff --git a/module-audio/Audio/StreamFactory.hpp b/module-audio/Audio/StreamFactory.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a74556cd78c19a134962ab7b9548a0be74d93457
--- /dev/null
+++ b/module-audio/Audio/StreamFactory.hpp
@@ -0,0 +1,29 @@
+// 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 "Endpoint.hpp"
+
+#include <memory>
+#include <vector>
+
+namespace audio
+{
+
+    class StreamFactory
+    {
+      public:
+        explicit StreamFactory(Endpoint::Capabilities factoryCaps);
+        auto makeStream(const Source &source, const Sink &sink) -> std::unique_ptr<Stream>;
+
+      private:
+        auto negotiateCaps(std::vector<std::reference_wrapper<const Endpoint>> v) -> Endpoint::Capabilities;
+        auto getAllocator(bool usesDMA) -> Stream::Allocator &;
+
+        Endpoint::Capabilities caps;
+        StandardStreamAllocator stdAlloc;
+        NonCacheableStreamAllocator nonCacheableAlloc;
+    };
+
+} // namespace audio
diff --git a/module-audio/Audio/decoder/Decoder.cpp b/module-audio/Audio/decoder/Decoder.cpp
index ae507b920fe79be0cac81d779a73f14cd58ad279..cceaed5d3d4dfbb35b38410140b3ec1372b44873 100644
--- a/module-audio/Audio/decoder/Decoder.cpp
+++ b/module-audio/Audio/decoder/Decoder.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include <cstdio>
@@ -16,7 +16,8 @@ namespace audio
 {
 
     Decoder::Decoder(const char *fileName)
-        : filePath(fileName), workerBuffer(std::make_unique<int16_t[]>(workerBufferSize)), tag(std::make_unique<Tags>())
+        : Source(Endpoint::Capabilities{.maxBlockSize = workerBufferSize * sizeof(int16_t)}), filePath(fileName),
+          workerBuffer(std::make_unique<int16_t[]>(workerBufferSize)), tag(std::make_unique<Tags>())
     {
 
         fd = std::fopen(fileName, "r");
diff --git a/module-audio/Audio/decoder/Decoder.hpp b/module-audio/Audio/decoder/Decoder.hpp
index 2f8f4d5e331ede1d3dee75ddbb71d17d7a1745b3..9212c21f31b7d6aded5eeb5e197ee6bcd98aa4ec 100644
--- a/module-audio/Audio/decoder/Decoder.hpp
+++ b/module-audio/Audio/decoder/Decoder.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -115,7 +115,8 @@ namespace audio
 
         void convertmono2stereo(int16_t *pcm, uint32_t samplecount);
 
-        const uint32_t workerBufferSize = 1024 * 8;
+        static constexpr auto workerBufferSize              = 1024 * 8;
+        static constexpr Endpoint::Capabilities decoderCaps = {.usesDMA = false};
 
         uint32_t sampleRate = 0;
         uint32_t chanNumber = 0;
diff --git a/module-audio/CMakeLists.txt b/module-audio/CMakeLists.txt
index 0241c4579ffd379eb0003b1990e6771289b64597..26105d0d5fbd39b2839fb125f3100a19f9c59a19 100755
--- a/module-audio/CMakeLists.txt
+++ b/module-audio/CMakeLists.txt
@@ -20,6 +20,7 @@ set(SOURCES
         "${CMAKE_CURRENT_SOURCE_DIR}/Audio/AudioCommon.cpp"
         "${CMAKE_CURRENT_SOURCE_DIR}/Audio/Endpoint.cpp"
         "${CMAKE_CURRENT_SOURCE_DIR}/Audio/Stream.cpp"
+        "${CMAKE_CURRENT_SOURCE_DIR}/Audio/StreamFactory.cpp"
         "${CMAKE_CURRENT_SOURCE_DIR}/Audio/StreamQueuedEventsListener.cpp"
         "${CMAKE_CURRENT_SOURCE_DIR}/Audio/BluetoothProxyAudio.cpp"
         "${CMAKE_CURRENT_SOURCE_DIR}/Audio/Operation/Operation.cpp"
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp
index ae0b5cb560c5dc2e5852bd8f08ca84bf5a02258c..4666a15a3ce72a78420fb0309afe3a694b79d037 100644
--- a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "RT1051Audiocodec.hpp"
@@ -25,8 +25,8 @@ namespace bsp
     sai_edma_handle_t RT1051Audiocodec::txHandle      = {};
     sai_edma_handle_t RT1051Audiocodec::rxHandle      = {};
 
-    RT1051Audiocodec::RT1051Audiocodec(bsp::AudioDevice::audioCallback_t callback)
-        : SAIAudioDevice(callback, BOARD_AUDIOCODEC_SAIx, &rxHandle, &txHandle), saiInFormat{}, saiOutFormat{},
+    RT1051Audiocodec::RT1051Audiocodec()
+        : SAIAudioDevice(BOARD_AUDIOCODEC_SAIx, &rxHandle, &txHandle), saiInFormat{}, saiOutFormat{},
           codecParams{}, codec{}
     {
         isInitialized = true;
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp
index d32af43434cdf7e95ee85bbc1072c42449ce38dd..c42eb8e6fc22878f23034a42779de77d93267abb 100755
--- a/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051Audiocodec.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -34,7 +34,7 @@ namespace bsp
         friend void txAudioCodecCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
         friend void rxAudioCodecCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
 
-        RT1051Audiocodec(AudioDevice::audioCallback_t callback);
+        RT1051Audiocodec();
         virtual ~RT1051Audiocodec();
 
         AudioDevice::RetCode Start(const Format &format) override final;
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp
index 568f808600e9a2bb14db1e4444238c0d68005985..afd5691c0b0078e005d57cd2880eb2d042d21704 100644
--- a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "RT1051CellularAudio.hpp"
@@ -18,9 +18,8 @@ namespace bsp
     sai_edma_handle_t RT1051CellularAudio::txHandle = {};
     sai_edma_handle_t RT1051CellularAudio::rxHandle = {};
 
-    RT1051CellularAudio::RT1051CellularAudio(bsp::AudioDevice::audioCallback_t callback)
-        : SAIAudioDevice(callback, BOARD_CELLULAR_AUDIO_SAIx, &rxHandle, &txHandle), saiInFormat{},
-          saiOutFormat{}, config{}
+    RT1051CellularAudio::RT1051CellularAudio()
+        : SAIAudioDevice(BOARD_CELLULAR_AUDIO_SAIx, &rxHandle, &txHandle), saiInFormat{}, saiOutFormat{}, config{}
     {
         isInitialized = true;
     }
diff --git a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp
index 171d05a4e7dedf143673f1b5c203590fbd503103..547ce64855afa893c53d3418507e19f2c6dd4181 100755
--- a/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp
+++ b/module-bsp/board/rt1051/bsp/audio/RT1051CellularAudio.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #ifndef PUREPHONE_RT1051CELLULARAUDIO_HPP
@@ -30,7 +30,7 @@ namespace bsp
         friend void txCellularCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
         friend void rxCellularCallback(I2S_Type *base, sai_edma_handle_t *handle, status_t status, void *userData);
 
-        RT1051CellularAudio(AudioDevice::audioCallback_t callback);
+        RT1051CellularAudio();
         virtual ~RT1051CellularAudio();
 
         AudioDevice::RetCode Start(const Format &format) override final;
diff --git a/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp
index 38f61b7227253f68f7f2b3ad14917dc40453f44c..5ab9320283ad1fccf4467c07a06a6b003e57ecc2 100644
--- a/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp
+++ b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.cpp
@@ -5,11 +5,8 @@
 
 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)
+SAIAudioDevice::SAIAudioDevice(I2S_Type *base, sai_edma_handle_t *rxHandle, sai_edma_handle_t *txHandle)
+    : AudioDevice(saiCapabilities, saiCapabilities), _base(base), rx(rxHandle), tx(txHandle)
 {}
 
 void SAIAudioDevice::initiateRxTransfer()
diff --git a/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp
index b51e91cb2970697e17263d44feeab8f712b8bdd4..29ed18590b9e259191054428cb8cd404ad3465de 100644
--- a/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp
+++ b/module-bsp/board/rt1051/bsp/audio/SAIAudioDevice.hpp
@@ -13,10 +13,7 @@ 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);
+        SAIAudioDevice(I2S_Type *base, sai_edma_handle_t *rxHandle, sai_edma_handle_t *txHandle);
 
         void onDataSend() override;
         void onDataReceive() override;
@@ -33,6 +30,8 @@ namespace bsp
         sai_edma_handle_t *tx = nullptr;
         bool txEnabled        = false;
         bool rxEnabled        = false;
+
+        static constexpr Capabilities saiCapabilities = {.usesDMA = true};
     };
 
 } // namespace bsp
diff --git a/module-bsp/bsp/audio/bsp_audio.cpp b/module-bsp/bsp/audio/bsp_audio.cpp
index d13186c360850a11be0fe38e7a7128d67742ad1f..680b48fe2407183a51c3e7892dc32ddc4f3a33bf 100644
--- a/module-bsp/bsp/audio/bsp_audio.cpp
+++ b/module-bsp/bsp/audio/bsp_audio.cpp
@@ -5,22 +5,16 @@
 #include "board/rt1051/bsp/audio/RT1051Audiocodec.hpp"
 #include "board/rt1051/bsp/audio/RT1051CellularAudio.hpp"
 
+#endif
+
 #include <Audio/Stream.hpp>
 
 #include <cassert>
 
-#elif defined(TARGET_Linux)
-#include "audio/linux_audiocodec.hpp"
-#include "audio/LinuxCellularAudio.hpp"
-#else
-#error "Unsupported target"
-#endif
-
 namespace bsp
 {
 
-    std::optional<std::unique_ptr<AudioDevice>> AudioDevice::Create(bsp::AudioDevice::Type type,
-                                                                    audioCallback_t callback)
+    std::optional<std::unique_ptr<AudioDevice>> AudioDevice::Create(bsp::AudioDevice::Type type)
     {
         std::unique_ptr<AudioDevice> inst;
 
@@ -28,33 +22,23 @@ namespace bsp
 
         case Type ::Audiocodec: {
 #if defined(TARGET_RT1051)
-            inst = std::make_unique<bsp::RT1051Audiocodec>(callback);
-#elif defined(TARGET_Linux)
-            inst = std::make_unique<bsp::LinuxAudiocodec>(callback);
+            inst = std::make_unique<bsp::RT1051Audiocodec>();
 #else
-#error "Unsupported target"
+            inst = nullptr;
 #endif
 
         } break;
 
         case Type ::Bluetooth: {
-#if defined(TARGET_RT1051)
+            LOG_FATAL("Bluetooth audio is not yet supported");
             inst = nullptr;
-
-#elif defined(TARGET_Linux)
-
-#else
-#error "Unsupported target"
-#endif
         } break;
 
         case Type::Cellular: {
 #if defined(TARGET_RT1051)
-            inst = std::make_unique<bsp::RT1051CellularAudio>(callback);
-#elif defined(TARGET_Linux)
-            inst = std::make_unique<bsp::LinuxCellularAudio>(callback);
+            inst = std::make_unique<bsp::RT1051CellularAudio>();
 #else
-#error "Unsupported target"
+            inst = nullptr;
 #endif
         } break;
         }
diff --git a/module-bsp/bsp/audio/bsp_audio.hpp b/module-bsp/bsp/audio/bsp_audio.hpp
index 99a7da2d0a3cd2f9c08ac8edce64708b2c89f9eb..29a8704791911ad085e6c9823a3e9519cd8f31c2 100755
--- a/module-bsp/bsp/audio/bsp_audio.hpp
+++ b/module-bsp/bsp/audio/bsp_audio.hpp
@@ -67,39 +67,15 @@ namespace bsp
             OutputPath outputPath  = OutputPath::None;
         };
 
-        /**
-         * User defined callback.
-         * It will be invoked when opened stream needs more frames to process( outputBuffer will be != NULL) or if
-         * requested frames count are available to user( inputBuffer will be != NULL). From this callback you can safely
-         * use file operations, system calls etc This is because audiostream callbacks are not invoked from IRQ context.
-         *
-         * If there is more data to process or read user should return:
-         *  'AudiostreamCallbackContinue'
-         *  if there is no more data to process or read user should return:
-         *  'AudiostreamCallbackComplete'
-         *  this will close stream and clean up all internally allocated resources.
-         *  In case of error return:
-         *  'AudiostreamCallbackAbort'
-         *  this has the same effect as AudiostreamCallbackComplete.
-         *
-         * @param stream[in] - pointer to valid stream
-         * @param inputBuffer[in] - pointer to buffer where user should copy PCM data
-         * @param outputBuffer[out] - pointer to buffer containing valid PCM data
-         * @param framesPerBuffer[in] - how many frames user should copy or read from buffer
-         * @param userData[in] - user specified data
-         * @return audiostream_callback_err_t
-         */
-        using audioCallback_t =
-            std::function<int32_t(const void *inputBuffer, void *outputBuffer, unsigned long framesPerBuffer)>;
-
-        explicit AudioDevice(audioCallback_t callback) : callback(callback)
+        AudioDevice() = default;
+        explicit AudioDevice(const audio::Endpoint::Capabilities &sourceCaps,
+                             const audio::Endpoint::Capabilities &sinkCaps)
+            : IOProxy(sourceCaps, sinkCaps)
         {}
 
-        AudioDevice() = delete;
-
         virtual ~AudioDevice() = default;
 
-        static std::optional<std::unique_ptr<AudioDevice>> Create(Type type, audioCallback_t callback);
+        static std::optional<std::unique_ptr<AudioDevice>> Create(Type type);
 
         virtual RetCode Start(const Format &format) = 0;
         virtual RetCode Stop()                      = 0;
@@ -135,14 +111,8 @@ namespace bsp
             return currentFormat;
         }
 
-        audioCallback_t GetAudioCallback()
-        {
-            return callback;
-        }
-
       protected:
         Format currentFormat;
-        audioCallback_t callback = nullptr;
 
         bool isInitialized = false;
     };
diff --git a/module-bsp/targets/Target_Linux.cmake b/module-bsp/targets/Target_Linux.cmake
index 95acc03594d64adb21bbe3735e8a9aca651ed1f1..3061ec84181aea325d3605c3be95bf7e26d26d86 100644
--- a/module-bsp/targets/Target_Linux.cmake
+++ b/module-bsp/targets/Target_Linux.cmake
@@ -14,9 +14,6 @@ set(BOARD_SOURCES
 
         "${CMAKE_CURRENT_SOURCE_DIR}/board/linux/lpm/LinuxLPM.cpp"
 
-        "${CMAKE_CURRENT_SOURCE_DIR}/board/linux/audio/linux_audiocodec.cpp"
-        "${CMAKE_CURRENT_SOURCE_DIR}/board/linux/audio/LinuxCellularAudio.cpp"
-
         "${CMAKE_CURRENT_SOURCE_DIR}/board/linux/bluetooth/Bluetooth.cpp"
 
         "${CMAKE_CURRENT_SOURCE_DIR}/board/linux/usb_cdc/usb_cdc.cpp"
diff --git a/module-utils/math/Math.hpp b/module-utils/math/Math.hpp
index be0bc9b607d521df85e4b5b28c4232ff0c889036..c0c7d88c2dff8d583e3ae8a93e1835127cc0a08f 100644
--- a/module-utils/math/Math.hpp
+++ b/module-utils/math/Math.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -47,3 +47,41 @@ namespace trigonometry
         }
     };
 } // namespace trigonometry
+
+namespace binary
+{
+    constexpr static inline auto isPowerOfTwo(unsigned int x) -> bool
+    {
+        return x == 0 ? false : __builtin_popcount(x) == 1;
+    }
+
+    static inline auto floorPowerOfTwo(unsigned int x) -> unsigned int
+    {
+        constexpr auto xBitCount = sizeof(x) * 8;
+
+        if (x == 0) {
+            return 0;
+        }
+        else if (isPowerOfTwo(x)) {
+            return x;
+        }
+
+        return 1 << (xBitCount - __builtin_clz(x) - 1);
+    }
+
+    static inline auto ceilPowerOfTwo(unsigned int x) -> unsigned int
+    {
+        constexpr auto xBitCount = sizeof(x) * 8;
+        auto leadingZeroes       = __builtin_clz(x);
+
+        if (leadingZeroes == 0 || x == 0) {
+            return 0;
+        }
+        else if (isPowerOfTwo(x)) {
+            return x;
+        }
+
+        return 1 << (xBitCount - leadingZeroes);
+    }
+
+} // namespace binary
diff --git a/module-utils/test/test_math.cpp b/module-utils/test/test_math.cpp
index 1f1939a4f1d4a18e897936432a059536f0ed2b1b..cb77a20b1755f17823e14c5040b033cf07eb1159 100644
--- a/module-utils/test/test_math.cpp
+++ b/module-utils/test/test_math.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2020, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #define CATCH_CONFIG_MAIN // This tells Catch to provide a main() - only do this in one cpp file
@@ -7,7 +7,10 @@
 
 #include "math/Math.hpp"
 
+#include <climits>
+
 using namespace trigonometry;
+using namespace binary;
 
 TEST_CASE("Math")
 {
@@ -47,4 +50,39 @@ TEST_CASE("Math")
             REQUIRE(OppositeSide::fromSine(std::sin(toRadians(60)), 6) == 5);
         }
     }
+
+    SECTION("Binary operations")
+    {
+        SECTION("Is number a power of two")
+        {
+            REQUIRE(isPowerOfTwo(0) == false);
+            REQUIRE(isPowerOfTwo(1) == true);
+            REQUIRE(isPowerOfTwo(2) == true);
+            REQUIRE(isPowerOfTwo(3) == false);
+            REQUIRE(isPowerOfTwo(64) == true);
+            REQUIRE(isPowerOfTwo(128) == true);
+            REQUIRE(isPowerOfTwo(64 + 128) == false);
+        }
+
+        SECTION("Find nearest power of two (floor)")
+        {
+            REQUIRE(floorPowerOfTwo(0) == 0);
+            REQUIRE(floorPowerOfTwo(1) == 1);
+            REQUIRE(floorPowerOfTwo(2) == 2);
+            REQUIRE(floorPowerOfTwo(3) == 2);
+            REQUIRE(floorPowerOfTwo(7) == 4);
+            REQUIRE(floorPowerOfTwo(0xffffffff) == 0x80000000);
+        }
+
+        SECTION("Find nearest power of two (ceil)")
+        {
+            REQUIRE(ceilPowerOfTwo(0) == 0);
+            REQUIRE(ceilPowerOfTwo(1) == 1);
+            REQUIRE(ceilPowerOfTwo(2) == 2);
+            REQUIRE(ceilPowerOfTwo(3) == 4);
+            REQUIRE(ceilPowerOfTwo(7) == 8);
+            REQUIRE(ceilPowerOfTwo(62) == 64);
+            REQUIRE(ceilPowerOfTwo(UINT_MAX) == 0);
+        }
+    }
 }