43 files changed, 670 insertions(+), 387 deletions(-)

M module-apps/application-settings/windows/advanced/CPUModeTestWindow.cpp
M module-bsp/board/linux/CMakeLists.txt
M module-bsp/board/linux/lpm/PowerProfile.cpp
M module-bsp/board/rt1051/bsp/lpm/RT1051LPM.cpp
M module-bsp/board/rt1051/bsp/lpm/RT1051LPM.hpp
M module-bsp/board/rt1051/puretx/bsp/battery_charger/battery_charger.hpp
M module-bsp/board/rt1051/puretx/bsp/lpm/PowerProfile.cpp
M module-bsp/board/rt1051/puretx/hal/battery_charger/CurrentMeasurementScope.cpp
A module-bsp/bsp/battery_charger/battery_charger.hpp
M module-bsp/bsp/lpm/PowerProfile.hpp
M module-bsp/bsp/lpm/bsp_lpm.hpp
M module-bsp/drivers/semc/DriverSEMC.hpp
M module-services/service-cellular/connection-manager/ConnectionManagerCellularCommands.cpp
M module-services/service-cellular/src/ServiceCellularPriv.cpp
M module-sys/SystemManager/CMakeLists.txt
M module-sys/SystemManager/CpuGovernor.cpp
M module-sys/SystemManager/CpuLogPrinter.cpp
M module-sys/SystemManager/CpuPackPrinter.cpp
M module-sys/SystemManager/CpuSentinel.cpp
M module-sys/SystemManager/CpuStatistics.cpp
M module-sys/SystemManager/PowerManager.cpp
M module-sys/SystemManager/SystemManagerCommon.cpp
A module-sys/SystemManager/cpu/AlgorithmFactory.cpp
A module-sys/SystemManager/cpu/AlgorithmFactory.hpp
A module-sys/SystemManager/cpu/algorithm/Algorithm.cpp
A module-sys/SystemManager/cpu/algorithm/Algorithm.hpp
A module-sys/SystemManager/cpu/algorithm/AlgorithmID.hpp
A module-sys/SystemManager/cpu/algorithm/FrequencyHold.cpp
A module-sys/SystemManager/cpu/algorithm/FrequencyHold.hpp
A module-sys/SystemManager/cpu/algorithm/FrequencyStepping.cpp
A module-sys/SystemManager/cpu/algorithm/FrequencyStepping.hpp
A module-sys/SystemManager/cpu/algorithm/ImmediateUpscale.cpp
A module-sys/SystemManager/cpu/algorithm/ImmediateUpscale.hpp
M module-sys/SystemManager/include/SystemManager/CpuGovernor.hpp
M module-sys/SystemManager/include/SystemManager/CpuPrinter.hpp
M module-sys/SystemManager/include/SystemManager/CpuSentinel.hpp
M module-sys/SystemManager/include/SystemManager/CpuStatistics.hpp
M module-sys/SystemManager/include/SystemManager/PowerManager.hpp
A module-sys/SystemManager/include/SystemManager/SentinelView.hpp
M module-sys/SystemManager/include/SystemManager/SysCpuUpdateResult.hpp
M module-sys/SystemManager/include/SystemManager/SystemManagerCommon.hpp
M module-sys/common/include/system/SystemReturnCodes.hpp
M module-sys/common/include/system/messages/RequestCpuFrequencyMessage.hpp

@@ 1,9 1,10 @@
-// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "CPUModeTestWindow.hpp"
 
 #include <system/messages/SentinelRegistrationMessage.hpp>
+#include <system/messages/RequestCpuFrequencyMessage.hpp>
 #include <application-settings/windows/WindowNames.hpp>
 #include <Application.hpp>
 #include <TextFixedSize.hpp>


@@ 131,18 132,23 @@ namespace gui
 
         permanentFreqSpinner = new gui::TextSpinnerBox(permanentFreqBody, {"OFF", "ON"}, Boundaries::Continuous);
         permanentFreqSpinner->setMinimumSize(100, 30);
-        permanentFreqSpinner->setCurrentValue(cpuModeTester->isPermanentFrequencyActive() ? "ON" : "OFF");
+        auto ret = application->async_call<sys::IsCpuPernament, sys::IsCpuPernamentResponse>(service::name::system_manager);
+        application->sync(ret);
+        permanentFreqSpinner->setCurrentValue( ret.getResult().pernament ? "ON" : "OFF");
 
         permanentFreqBody->inputCallback = [&](Item &item, const InputEvent &event) {
             auto ret = permanentFreqSpinner->onInput(event);
             if (ret) {
                 if (permanentFreqSpinner->getCurrentValue() == "ON") {
-                    cpuModeTester->HoldFrequencyPermanently(
-                        magic_enum::enum_cast<bsp::CpuFrequencyMHz>(std::stoi(currentFreqSpinner->getCurrentValue()))
-                            .value());
+                    application->bus.sendUnicastSync(std::make_shared<sys::HoldCpuFrequencyPermanentlyMessage>(
+                                                     magic_enum::enum_cast<bsp::CpuFrequencyMHz>(
+                                                         std::stoi(currentFreqSpinner->getCurrentValue()))
+                                                         .value()),
+                                                 service::name::system_manager, 5000);
                 }
                 else {
-                    cpuModeTester->ReleasePermanentFrequency();
+                    application->bus.sendUnicastSync(std::make_shared<sys::ReleaseCpuPermanentFrequencyMessage>(),
+                                                 service::name::system_manager, 5000);
                 }
             }
 


@@ 183,10 189,16 @@ namespace gui
         newFreqBody->inputCallback = [&](Item &item, const InputEvent &event) {
             auto ret = currentFreqSpinner->onInput(event);
 
-            if (cpuModeTester->isPermanentFrequencyActive()) {
-                cpuModeTester->HoldFrequencyPermanently(
-                    magic_enum::enum_cast<bsp::CpuFrequencyMHz>(std::stoi(currentFreqSpinner->getCurrentValue()))
-                        .value());
+            auto async = application->async_call<sys::IsCpuPernament, sys::IsCpuPernamentResponse>(
+                service::name::system_manager);
+            application->sync(async);
+            if (async.getResult().pernament) {
+                application->bus.sendUnicastSync(
+                    std::make_shared<sys::HoldCpuFrequencyPermanentlyMessage>(
+                        magic_enum::enum_cast<bsp::CpuFrequencyMHz>(std::stoi(currentFreqSpinner->getCurrentValue()))
+                            .value()),
+                    service::name::system_manager,
+                    5000);
             }
 
             return ret;

@@ 4,6 4,7 @@ target_sources(module-bsp
         PRIVATE
                 board.cpp
 
+                battery_charger/battery_charger.cpp
                 bluetooth/Bluetooth.cpp
                 bluetooth/test/bsp_bt.cpp
                 cellular/linux_cellular.cpp

@@ 1,4 1,4 @@
-// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include <bsp/lpm/PowerProfile.hpp>


@@ 15,7 15,6 @@ namespace bsp
         linuxPowerProfile.maxBelowThresholdInRowCount       = 10;
         linuxPowerProfile.maxAboveThresholdCount            = 3;
         linuxPowerProfile.minimalFrequency                  = CpuFrequencyMHz::Level_1;
-        linuxPowerProfile.frequencyIncreaseIntermediateStep = false;
 
         return linuxPowerProfile;
     }

@@ 14,6 14,7 @@
 #include "ClockState.hpp"
 #include "Oscillator.hpp"
 #include "critical.hpp"
+#include "drivers/semc/DriverSEMC.hpp"
 
 namespace bsp
 {


@@ 22,6 23,7 @@ namespace bsp
 
     RT1051LPM::RT1051LPM()
     {
+        driverSEMC      = drivers::DriverSEMC::Create(drivers::name::ExternalRAM);
         gpio_1 = DriverGPIO::Create(static_cast<GPIOInstances>(BoardDefinitions::POWER_SWITCH_HOLD_GPIO),
                                     DriverGPIOParams{});
         gpio_2 = DriverGPIO::Create(static_cast<GPIOInstances>(BoardDefinitions::DCDC_INVERTER_MODE_GPIO),


@@ 72,9 74,62 @@ namespace bsp
         return 0;
     }
 
+    enum class Change {
+        Up, Down
+    };
+
+    CpuFrequencyMHz RT1051LPM::onChangeUp(CpuFrequencyMHz freq, bsp::CpuFrequencyMHz newFrequency)
+    {
+        if ((freq <= CpuFrequencyMHz::Level_1) && (newFrequency > CpuFrequencyMHz::Level_1)) {
+            // connect internal the load resistor
+            ConnectInternalLoadResistor();
+            // turn off power save mode for DCDC inverter
+            DisableDcdcPowerSaveMode();
+            // Switch DCDC to full throttle during oscillator switch
+            SetHighestCoreVoltage();
+            // Enable regular 2P5 and 1P1 LDO and Turn off weak 2P5 and 1P1 LDO
+            SwitchToRegularModeLDO();
+            // switch oscillator source
+            SwitchOscillatorSource(LowPowerMode::OscillatorSource::External);
+            // then switch external RAM clock source
+            if (driverSEMC) {
+                driverSEMC->SwitchToPLL2ClockSource();
+            }
+            // Add intermediate step in frequency
+            if (newFrequency > CpuFrequencyMHz::Level_4)
+                return CpuFrequencyMHz::Level_4;
+        }
+        return newFrequency;
+    }
+
+    void RT1051LPM::onChangeDown(CpuFrequencyMHz newFrequency)
+    {
+        if (newFrequency <= bsp::CpuFrequencyMHz::Level_1) {
+            // Enable weak 2P5 and 1P1 LDO and Turn off regular 2P5 and 1P1 LDO
+            SwitchToLowPowerModeLDO();
+            // then switch osc source
+            SwitchOscillatorSource(bsp::LowPowerMode::OscillatorSource::Internal);
+            // and switch external RAM clock source
+            if (driverSEMC) {
+                driverSEMC->SwitchToPeripheralClockSource();
+            }
+            // turn on power save mode for DCDC inverter
+            EnableDcdcPowerSaveMode();
+
+            // disconnect internal the load resistor
+            DisconnectInternalLoadResistor();
+        }
+    }
+
     void RT1051LPM::SetCpuFrequency(bsp::CpuFrequencyMHz freq)
     {
-        currentFrequency = freq;
+        if ( currentFrequency == freq ) {
+            return;
+        }
+        Change change = currentFrequency < freq ? Change::Up : Change::Down;
+        if (Change::Up == change) {
+            freq = onChangeUp(currentFrequency, freq);
+        }
         switch (freq) {
         case bsp::CpuFrequencyMHz::Level_0:
             CpuFreq->SetCpuFrequency(CpuFreqLPM::CpuClock::CpuClock_Osc_4_Mhz);


@@ 98,7 153,12 @@ namespace bsp
             CpuFreq->SetCpuFrequency(CpuFreqLPM::CpuClock::CpuClock_Pll2_528_Mhz);
             break;
         }
+
+        if (Change::Down == change) {
+            onChangeDown(freq);
+        }
         LOG_INFO("CPU frequency changed to %lu", CLOCK_GetFreq(kCLOCK_CpuClk));
+        currentFrequency = freq;
     }
 
     void RT1051LPM::SetHighestCoreVoltage()

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


@@ 32,9 32,12 @@ namespace bsp
         void SwitchToLowPowerModeLDO() final;
 
       private:
+        CpuFrequencyMHz onChangeUp(CpuFrequencyMHz freq, CpuFrequencyMHz newFrequency);
+        void onChangeDown(bsp::CpuFrequencyMHz freq);
         std::shared_ptr<drivers::DriverGPIO> gpio_1;
         std::shared_ptr<drivers::DriverGPIO> gpio_2;
         std::unique_ptr<bsp::CpuFreqLPM> CpuFreq;
+        std::shared_ptr<drivers::DriverSEMC> driverSEMC;
     };
 
 } // namespace bsp

@@ 92,10 92,6 @@ namespace bsp::battery_charger
 
     int getVoltageFilteredMeasurement();
 
-    int getAvgCurrent();
-
-    int getCurrentMeasurement();
-
     MaxMinVolt getMaxMinVolt();
 
     void printFuelGaugeInfo();

@@ 1,4 1,4 @@
-// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include <bsp/lpm/PowerProfile.hpp>


@@ 15,7 15,6 @@ namespace bsp
         purePowerProfile.maxBelowThresholdInRowCount       = 1;
         purePowerProfile.maxAboveThresholdCount            = 2;
         purePowerProfile.minimalFrequency                  = CpuFrequencyMHz::Level_0;
-        purePowerProfile.frequencyIncreaseIntermediateStep = true;
 
         return purePowerProfile;
     }

@@ 8,7 8,7 @@ extern "C"
 #include "FreeRTOS.h"
 }
 #include <timers.h>
-#include <bsp/battery_charger/battery_charger.hpp>
+#include <battery_charger/battery_charger.hpp>
 #include <ticks.hpp>
 
 #include <chrono>

@@ 0,0 1,6 @@
+#pragma once
+
+namespace bsp::battery_charger {
+    int getAvgCurrent();
+    int getCurrentMeasurement();
+}

@@ 16,7 16,6 @@ namespace bsp
         std::uint32_t maxBelowThresholdInRowCount;
         std::uint32_t maxAboveThresholdCount;
         CpuFrequencyMHz minimalFrequency;
-        bool frequencyIncreaseIntermediateStep;
     };
 
     const PowerProfile getPowerProfile();

@@ 7,6 7,11 @@
 #include <memory>
 #include <bsp/common.hpp>
 
+namespace drivers
+{
+    class DriverSEMC;
+}
+
 namespace bsp
 {
 

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


@@ 8,6 8,9 @@
 
 namespace drivers
 {
+    namespace name {
+        constexpr auto ExternalRAM =  "ExternalRAM";
+    }
 
     class DriverSEMC : public devices::Device
     {

@@ 1,4 1,4 @@
-// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include <service-cellular/service-cellular/connection-manager/ConnectionManagerCellularCommands.hpp>


@@ 100,11 100,9 @@ void ConnectionManagerCellularCommands::stopConnectionTimer()
 
 void ConnectionManagerCellularCommands::holdMinimumCpuFrequency()
 {
-    auto handle = cellular.getTaskHandle();
     if (cellular.cpuSentinel) {
-        cellular.cpuSentinel->HoldMinimumFrequencyAndWait(bsp::CpuFrequencyMHz::Level_4, handle, 2000);
+        cellular.cpuSentinel->HoldMinimumFrequency(bsp::CpuFrequencyMHz::Level_4);
     }
-    return;
 }
 void ConnectionManagerCellularCommands::retryPhoneModeChange()
 {

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


@@ 398,11 398,8 @@ namespace cellular::internal
     void ServiceCellularPriv::initCSQHandler()
     {
         csqHandler->onEnableCsqReporting = [this]() {
-            constexpr auto cpuSentinelTimeout = 2000;
-            auto handle                       = owner->getTaskHandle();
             if (owner->cpuSentinel) {
-                owner->cpuSentinel->HoldMinimumFrequencyAndWait(
-                    bsp::CpuFrequencyMHz::Level_4, handle, cpuSentinelTimeout);
+                owner->cpuSentinel->HoldMinimumFrequency(bsp::CpuFrequencyMHz::Level_4);
             }
 
             auto channel = owner->cmux->get(CellularMux::Channel::Commands);


@@ 416,11 413,8 @@ namespace cellular::internal
         };
 
         csqHandler->onDisableCsqReporting = [this]() {
-            constexpr auto cpuSentinelTimeout = 2000;
-            auto handle                       = owner->getTaskHandle();
             if (owner->cpuSentinel) {
-                owner->cpuSentinel->HoldMinimumFrequencyAndWait(
-                    bsp::CpuFrequencyMHz::Level_4, handle, cpuSentinelTimeout);
+                owner->cpuSentinel->HoldMinimumFrequency(bsp::CpuFrequencyMHz::Level_4);
             }
 
             auto channel = owner->cmux->get(CellularMux::Channel::Commands);

@@ 23,6 23,11 @@ target_sources(sys-manager
         graph/TopologicalSort.hpp
         PowerManager.cpp
         SystemManagerCommon.cpp
+        cpu/AlgorithmFactory.cpp
+        cpu/algorithm/Algorithm.cpp
+        cpu/algorithm/FrequencyHold.cpp
+        cpu/algorithm/ImmediateUpscale.cpp
+        cpu/algorithm/FrequencyStepping.cpp
 )
 
 target_include_directories(sys-manager

@@ 83,15 83,8 @@ namespace sys
         std::for_each(std::begin(sentinels), std::end(sentinels), PrintName);
     }
 
-    void CpuGovernor::SetCpuFrequencyRequest(std::string sentinelName,
-                                             bsp::CpuFrequencyMHz request,
-                                             bool permanentBlock)
+    void CpuGovernor::SetCpuFrequencyRequest(const std::string &sentinelName, bsp::CpuFrequencyMHz request)
     {
-        if (permanentBlock) {
-            permanentFrequencyToHold.isActive        = true;
-            permanentFrequencyToHold.frequencyToHold = request;
-            return;
-        }
         for (auto &sentinel : sentinels) {
             auto sentinelWeakPointer = sentinel->GetSentinel();
             if (!sentinelWeakPointer.expired()) {


@@ 103,19 96,14 @@ namespace sys
         }
     }
 
-    void CpuGovernor::ResetCpuFrequencyRequest(std::string sentinelName, bool permanentBlock)
+    void CpuGovernor::ResetCpuFrequencyRequest(const std::string &sentinelName)
     {
-        if (permanentBlock) {
-            permanentFrequencyToHold.isActive        = false;
-            permanentFrequencyToHold.frequencyToHold = bsp::CpuFrequencyMHz::Level_0;
-            return;
-        }
         SetCpuFrequencyRequest(sentinelName, bsp::CpuFrequencyMHz::Level_0);
     }
 
-    [[nodiscard]] auto CpuGovernor::GetMinimumFrequencyRequested() const noexcept -> sentinel::Data
+    [[nodiscard]] auto CpuGovernor::GetMinimumFrequencyRequested() const noexcept -> sentinel::View
     {
-        sentinel::Data d;
+        sentinel::View d;
         if (sentinels.empty()) {
             d.reason = "empty";
             return d;


@@ 133,7 121,6 @@ namespace sys
         d.frequency = (*minSentinel)->GetRequestedFrequency();
         if (auto p = (*minSentinel)->GetSentinel().lock()) {
             d.name   = p->GetName();
-            d.task   = p->getTask();
             d.reason = p->getReason();
         }
         else {


@@ 161,10 148,4 @@ namespace sys
             LOG_INFO("Sentinel %s", sharedResource->GetName().c_str());
         }
     }
-
-    [[nodiscard]] auto CpuGovernor::GetPermanentFrequencyRequested() const noexcept -> PermanentFrequencyToHold
-    {
-        return permanentFrequencyToHold;
-    }
-
 } // namespace sys

@@ 44,4 44,7 @@ namespace sys::cpu::stats
                    int(ret.data.frequency),
                    CLOCK_GetFreq(0));
     }
+
+    void LogPrinter::printPowerConsumption()
+    {}
 } // namespace sys::cpu::stats

@@ 5,6 5,8 @@
 #include "SystemManager/SysCpuUpdateResult.hpp"
 #include <SystemManager/SystemManagerCommon.hpp>
 #include "third-party/msgpack11/msgpack11/msgpack11.hpp"
+#include "battery_charger/battery_charger.hpp"
+#include "time/time_conversion.hpp"
 
 namespace sys::cpu::stats
 {


@@ 12,35 14,57 @@ namespace sys::cpu::stats
     using namespace msgpack11;
     enum class PackID
     {
-        Proc,
-        Usage,
+        Usage   = 1,
+        ProcEnd = 2,
+        Proc    = 3,
+        Power   = 4,
     };
 
     void PackPrinter::printSysUsage(struct task_prof_data *data, size_t size)
     {
+        MsgPack procEnd = MsgPack::object({{"id",uint8_t(PackID::ProcEnd)}});
         vTaskSuspendAll();
         {
             for (size_t i = 0; i < size; ++i) {
                 if (data[i].exec_time == 0 && data[i].switches == 0) {
                     continue;
                 }
-                MsgPack obj = MsgPack::object{{"id", uint32_t(PackID::Proc)},
-                                              {"name", SystemManagerCommon::ServiceProcessor(i)},
-                                              {"tcb", uint32_t(data[i].task_TCB_id)},
-                                              {"t", data[i].exec_time}};
-                LOG_PRINTF("%c%s\n", 2, obj.dump().c_str());
+                MsgPack obj = MsgPack::object{
+                    {"name", SystemManagerCommon::ServiceProcessor(i)},
+                    {"tcb", uint16_t(data[i].task_TCB_id)},
+                    {"time", data[i].exec_time},
+                    {"id", uint8_t(PackID::Proc)}
+                };
+
+                LOG_PRINTF("\n%c%s\n", 2, obj.dump().c_str());
             }
+            LOG_PRINTF("\n%c%s\n", 2, procEnd.dump().c_str());
         }
         xTaskResumeAll();
     }
 
     void PackPrinter::printCPUChange(const cpu::UpdateResult &ret)
     {
-        MsgPack obj = MsgPack::object{{"id", uint32_t(PackID::Usage)},
+        MsgPack obj = MsgPack::object{{"id", uint8_t(PackID::Usage)},
                                       {"freq", uint32_t(ret.frequencySet)},
                                       {"name", ret.data.name},
                                       {"reason", ret.data.reason},
-                                      {"requested", uint32_t(ret.data.frequency)}};
-        LOG_PRINTF("%c%s\n", 2, obj.dump().c_str());
+                                      {"requested", uint32_t(ret.data.frequency)},
+                                      {"avgA" , int32_t(bsp::battery_charger::getAvgCurrent())},
+                                      {"nowA" , int32_t(bsp::battery_charger::getCurrentMeasurement())},
+                                      {"ts", static_cast<uint64_t>(utils::time::getCurrentTimestamp().getTime())}
+        };
+        LOG_PRINTF("\n%c%s\n",2, obj.dump().c_str());
+    }
+
+
+    void PackPrinter::printPowerConsumption()
+    {
+        MsgPack obj = MsgPack::object{{"id", uint8_t(PackID::Power)},
+                                      {"avgA" , int32_t(bsp::battery_charger::getAvgCurrent())},
+                                      {"nowA" , int32_t(bsp::battery_charger::getCurrentMeasurement())},
+                                      {"ts", static_cast<uint64_t>(utils::time::getCurrentTimestamp().getTime())}
+        };
+        LOG_PRINTF("\n%c%s\n",2, obj.dump().c_str());
     }
 } // namespace sys::cpu::stats

@@ 28,11 28,12 @@ namespace sys
     void CpuSentinel::HoldMinimumFrequency(bsp::CpuFrequencyMHz frequencyToHold)
     {
         if (currentFrequencyToHold != frequencyToHold) {
-            auto msg = std::make_shared<sys::HoldCpuFrequencyMessage>(GetName(), frequencyToHold);
+            auto msg = std::make_shared<sys::HoldCpuFrequencyMessage>(GetName(), frequencyToHold, xTaskGetCurrentTaskHandle());
             owner->bus.sendUnicast(std::move(msg), service::name::system_manager);
             currentFrequencyToHold = frequencyToHold;
             currentReason          = std::string("up: ") + owner->getCurrentProcessing() + std::string(" req: ") +
                             std::to_string(int(frequencyToHold));
+            ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
         }
     }
 


@@ 46,37 47,9 @@ namespace sys
         }
     }
 
-    void CpuSentinel::HoldFrequencyPermanently(bsp::CpuFrequencyMHz frequencyToHold)
-    {
-        permanentFrequencyToHold.isActive        = true;
-        permanentFrequencyToHold.frequencyToHold = frequencyToHold;
-        auto msg = std::make_shared<sys::HoldCpuFrequencyPermanentlyMessage>(GetName(), frequencyToHold);
-        owner->bus.sendUnicast(std::move(msg), service::name::system_manager);
-    }
-
     [[nodiscard]] auto CpuSentinel::GetFrequency() const noexcept -> bsp::CpuFrequencyMHz
     {
-        if (permanentFrequencyToHold.isActive) {
-            return permanentFrequencyToHold.frequencyToHold;
-        }
-        else {
             return currentFrequency;
-        }
-    }
-
-    void CpuSentinel::ReleasePermanentFrequency()
-    {
-        if (permanentFrequencyToHold.isActive) {
-            auto msg = std::make_shared<sys::ReleaseCpuPermanentFrequencyMessage>(GetName());
-            owner->bus.sendUnicast(std::move(msg), service::name::system_manager);
-            permanentFrequencyToHold.isActive        = false;
-            permanentFrequencyToHold.frequencyToHold = bsp::CpuFrequencyMHz::Level_0;
-        }
-    }
-
-    bool CpuSentinel::isPermanentFrequencyActive()
-    {
-        return permanentFrequencyToHold.isActive;
     }
 
     void CpuSentinel::CpuFrequencyHasChanged(bsp::CpuFrequencyMHz newFrequency)


@@ 85,35 58,11 @@ namespace sys
         if (callback) {
             callback(newFrequency);
         }
-        if (taskWaitingForFrequency != nullptr && newFrequency >= currentFrequencyToHold) {
-            xTaskNotifyGive(taskWaitingForFrequency);
-            taskWaitingForFrequency = nullptr;
-        }
     }
 
-    bool CpuSentinel::HoldMinimumFrequencyAndWait(bsp::CpuFrequencyMHz frequencyToHold,
-                                                  TaskHandle_t taskToNotify,
-                                                  uint32_t timeout)
+    void CpuSentinel::ReadRegistrationData(bsp::CpuFrequencyMHz frequencyHz)
     {
-        currentReason = std::string("h+w: ") + owner->getCurrentProcessing();
-        HoldMinimumFrequency(frequencyToHold);
-
-        if (currentFrequencyToHold < frequencyToHold) {
-            taskWaitingForFrequency = taskToNotify;
-            return ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(timeout)) == 0;
-        }
-
-        return true;
-    }
-
-    void CpuSentinel::ReadRegistrationData(bsp::CpuFrequencyMHz frequencyHz, bool permanentFrequency)
-    {
-        currentFrequency                  = frequencyHz;
-        permanentFrequencyToHold.isActive = permanentFrequency;
-
-        if (permanentFrequencyToHold.isActive) {
-            permanentFrequencyToHold.frequencyToHold = currentFrequency;
-        }
+        currentFrequency = frequencyHz;
     }
 
     TimedCpuSentinel::TimedCpuSentinel(std::string name, sys::Service *service)

@@ 22,7 22,10 @@ namespace sys
         data_size = prof_pool_get_data().size;
         data      = new task_prof_data[data_size];
 #endif
-        printer = std::make_unique<cpu::stats::LogPrinter>();
+        // to change printer change assignment
+        // printer = std::make_unique<cpu::stats::LogPrinter>();
+        printer = std::make_unique<cpu::stats::PackPrinter>();
+        // printer = std::make_unique<cpu::stats::NullPrinter>();
     }
 
     CpuStatistics::~CpuStatistics()


@@ 47,12 50,13 @@ namespace sys
 
     void CpuStatistics::TrackChange(const cpu::UpdateResult &ret)
     {
-        if (ret.changed) {
+        if (ret.changed != sys::cpu::UpdateResult::Result::NoChange) {
             printer->printCPUChange(ret);
 #if PROF_ON
             printer->printSysUsage(data, data_size);
 #endif
         }
+        printer->printPowerConsumption();
         StoreSysUsage();
     }
 


@@ 72,8 76,9 @@ namespace sys
         }
     }
 
-    uint32_t CpuStatistics::GetPercentageCpuLoad() const noexcept
+    uint32_t CpuStatistics::GetPercentageCpuLoad()
     {
+        UpdatePercentageCpuLoad();
         return cpuLoad;
     }
 

@@ 1,11 1,16 @@
 // Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
+#include "SystemManager/cpu/algorithm/FrequencyHold.hpp"
+#include "SystemManager/cpu/algorithm/ImmediateUpscale.hpp"
+#include "SystemManager/cpu/algorithm/FrequencyStepping.hpp"
+#include "cpu/AlgorithmFactory.hpp"
+#include "magic_enum.hpp"
+#include <SystemManager/CpuStatistics.hpp>
+#include <SystemManager/PowerManager.hpp>
 #include <gsl/util>
 #include <log/log.hpp>
 
-#include <SystemManager/PowerManager.hpp>
-
 namespace sys
 {
     namespace


@@ 34,12 39,18 @@ namespace sys
         totalTicksCount += ticks;
     }
 
-    PowerManager::PowerManager() : powerProfile{bsp::getPowerProfile()}
+
+    PowerManager::PowerManager(CpuStatistics &stats) : powerProfile{bsp::getPowerProfile()}, cpuStatistics(stats)
     {
+        driverSEMC      = drivers::DriverSEMC::Create(drivers::name::ExternalRAM);
         lowPowerControl = bsp::LowPowerMode::Create().value_or(nullptr);
-        driverSEMC      = drivers::DriverSEMC::Create("ExternalRAM");
         cpuGovernor     = std::make_unique<CpuGovernor>();
 
+        cpuAlgorithms = std::make_unique<cpu::AlgorithmFactory>();
+        cpuAlgorithms->emplace(sys::cpu::AlgoID::ImmediateUpscale, std::make_unique<sys::cpu::ImmediateUpscale>());
+        cpuAlgorithms->emplace(sys::cpu::AlgoID::FrequencyStepping,
+                               std::make_unique<sys::cpu::FrequencyStepping>(powerProfile, *cpuGovernor));
+
         cpuFrequencyMonitor.push_back(CpuFrequencyMonitor(lowestLevelName));
         cpuFrequencyMonitor.push_back(CpuFrequencyMonitor(middleLevelName));
         cpuFrequencyMonitor.push_back(CpuFrequencyMonitor(highestLevelName));


@@ 77,202 88,94 @@ namespace sys
         }
     }
 
-    [[nodiscard]] cpu::UpdateResult PowerManager::UpdateCpuFrequency(uint32_t cpuLoad)
+    [[nodiscard]] cpu::UpdateResult PowerManager::UpdateCpuFrequency()
     {
+        uint32_t cpuLoad = cpuStatistics.GetPercentageCpuLoad(); 
         cpu::UpdateResult result;
+        cpu::AlgorithmData data {cpuLoad,lowPowerControl->GetCurrentFrequencyLevel(), cpuGovernor->GetMinimumFrequencyRequested()};
 
-        const auto currentCpuFreq           = lowPowerControl->GetCurrentFrequencyLevel();
-        const auto min                      = cpuGovernor->GetMinimumFrequencyRequested();
-        const auto permanent                = cpuGovernor->GetPermanentFrequencyRequested();
-
-        auto _ = gsl::finally([&result, this, &currentCpuFreq, min, permanent] {
+        auto _ = gsl::finally([&result, this, data] {
             result.frequencySet = lowPowerControl->GetCurrentFrequencyLevel();
-            result.changed      = result.frequencySet != currentCpuFreq;
-            if (not permanent.isActive) {
-                result.data = min;
-            }
-            else {
-                result.data.reason = "perm";
-            }
+            result.changed      = result.frequencySet > data.curentFrequency   ? sys::cpu::UpdateResult::Result::UpScaled
+                                  : result.frequencySet < data.curentFrequency ? sys::cpu::UpdateResult::Result::Downscaled
+                                                                         : sys::cpu::UpdateResult::Result::NoChange;
+            result.data = data.sentinel;
         });
 
-        if (permanent.isActive) {
-            auto frequencyToHold = std::max(permanent.frequencyToHold, powerProfile.minimalFrequency);
-
-            if (currentCpuFreq < frequencyToHold) {
-                IncreaseCpuFrequency(frequencyToHold);
-            }
-            else if (currentCpuFreq > frequencyToHold) {
-                do {
-                    DecreaseCpuFrequency();
-                } while (lowPowerControl->GetCurrentFrequencyLevel() > frequencyToHold);
+        auto algorithms = {
+            sys::cpu::AlgoID::FrequencyHold, sys::cpu::AlgoID::ImmediateUpscale, sys::cpu::AlgoID::FrequencyStepping};
+
+        for (auto id : algorithms) {
+            auto algo = cpuAlgorithms->get(id);
+            if (algo != nullptr) {
+                if (auto frequencyToSet = algo->calculate(data); frequencyToSet != data.curentFrequency) {
+                    result.id = id;
+                    SetCpuFrequency(frequencyToSet);
+                    for (auto again : algorithms) {
+                        if (auto al = cpuAlgorithms->get(again); al != nullptr) {
+                            al->reset();
+                        }
+                    }
+                    return result;
+                }
             }
-            ResetFrequencyShiftCounter();
-            return result;
         }
 
-        if (cpuLoad > powerProfile.frequencyShiftUpperThreshold && currentCpuFreq < bsp::CpuFrequencyMHz::Level_6) {
-            aboveThresholdCounter++;
-            belowThresholdCounter = 0;
-        }
-        else if (cpuLoad < powerProfile.frequencyShiftLowerThreshold &&
-                 currentCpuFreq > powerProfile.minimalFrequency) {
-            belowThresholdCounter++;
-            aboveThresholdCounter = 0;
-        }
-        else {
-            ResetFrequencyShiftCounter();
-        }
-
-        if (!belowThresholdCounter) {
-            isFrequencyLoweringInProgress = false;
-        }
-
-        if (min.frequency > currentCpuFreq) {
-            ResetFrequencyShiftCounter();
-            IncreaseCpuFrequency(min.frequency);
-        }
-        else if (aboveThresholdCounter >= powerProfile.maxAboveThresholdCount) {
-            if (powerProfile.frequencyIncreaseIntermediateStep && currentCpuFreq < bsp::CpuFrequencyMHz::Level_4) {
-                ResetFrequencyShiftCounter();
-                IncreaseCpuFrequency(bsp::CpuFrequencyMHz::Level_4);
-            }
-            else {
-                ResetFrequencyShiftCounter();
-                IncreaseCpuFrequency(bsp::CpuFrequencyMHz::Level_6);
-            }
-        }
-        else {
-            if (belowThresholdCounter >= (isFrequencyLoweringInProgress ? powerProfile.maxBelowThresholdInRowCount
-                                                                        : powerProfile.maxBelowThresholdCount) &&
-                currentCpuFreq > min.frequency) {
-                ResetFrequencyShiftCounter();
-                DecreaseCpuFrequency();
-            }
-        }
         return result;
     }
 
-    void PowerManager::IncreaseCpuFrequency(bsp::CpuFrequencyMHz newFrequency)
+    void PowerManager::RegisterNewSentinel(std::shared_ptr<CpuSentinel> newSentinel) const
     {
-        const auto freq = lowPowerControl->GetCurrentFrequencyLevel();
-
-        if ((freq <= bsp::CpuFrequencyMHz::Level_1) && (newFrequency > bsp::CpuFrequencyMHz::Level_1)) {
-            // connect internal the load resistor
-            lowPowerControl->ConnectInternalLoadResistor();
-            // turn off power save mode for DCDC inverter
-            lowPowerControl->DisableDcdcPowerSaveMode();
-            // Switch DCDC to full throttle during oscillator switch
-            lowPowerControl->SetHighestCoreVoltage();
-            // Enable regular 2P5 and 1P1 LDO and Turn off weak 2P5 and 1P1 LDO
-            lowPowerControl->SwitchToRegularModeLDO();
-            // switch oscillator source
-            lowPowerControl->SwitchOscillatorSource(bsp::LowPowerMode::OscillatorSource::External);
-            // then switch external RAM clock source
-            if (driverSEMC) {
-                driverSEMC->SwitchToPLL2ClockSource();
-            }
-            // Add intermediate step in frequency
-            if (newFrequency > bsp::CpuFrequencyMHz::Level_4)
-                SetCpuFrequency(bsp::CpuFrequencyMHz::Level_4);
-        }
-
-        // and increase frequency
-        if (freq < newFrequency) {
-            SetCpuFrequency(newFrequency);
+        if (cpuGovernor->RegisterNewSentinel(newSentinel)) {
+            newSentinel->ReadRegistrationData(lowPowerControl->GetCurrentFrequencyLevel());
         }
     }
 
-    void PowerManager::DecreaseCpuFrequency()
+    void PowerManager::RemoveSentinel(std::string sentinelName) const
     {
-        const auto freq = lowPowerControl->GetCurrentFrequencyLevel();
-        auto level      = powerProfile.minimalFrequency;
-
-        switch (freq) {
-        case bsp::CpuFrequencyMHz::Level_6:
-            level = bsp::CpuFrequencyMHz::Level_5;
-            break;
-        case bsp::CpuFrequencyMHz::Level_5:
-            level = bsp::CpuFrequencyMHz::Level_4;
-            break;
-        case bsp::CpuFrequencyMHz::Level_4:
-            level = bsp::CpuFrequencyMHz::Level_3;
-            break;
-        case bsp::CpuFrequencyMHz::Level_3:
-            level = bsp::CpuFrequencyMHz::Level_2;
-            break;
-        case bsp::CpuFrequencyMHz::Level_2:
-            level = powerProfile.minimalFrequency;
-            break;
-        case bsp::CpuFrequencyMHz::Level_1:
-            [[fallthrough]];
-        case bsp::CpuFrequencyMHz::Level_0:
-            break;
-        }
-
-        // decrease frequency first
-        if (level != freq) {
-            SetCpuFrequency(level);
-        }
-
-        if (level <= bsp::CpuFrequencyMHz::Level_1) {
-            // Enable weak 2P5 and 1P1 LDO and Turn off regular 2P5 and 1P1 LDO
-            lowPowerControl->SwitchToLowPowerModeLDO();
-
-            // then switch osc source
-            lowPowerControl->SwitchOscillatorSource(bsp::LowPowerMode::OscillatorSource::Internal);
-
-            // and switch external RAM clock source
-            if (driverSEMC) {
-                driverSEMC->SwitchToPeripheralClockSource();
-            }
-
-            // turn on power save mode for DCDC inverter
-            lowPowerControl->EnableDcdcPowerSaveMode();
-
-            // disconnect internal the load resistor
-            lowPowerControl->DisconnectInternalLoadResistor();
-        }
-
-        isFrequencyLoweringInProgress = true;
+        cpuGovernor->RemoveSentinel(sentinelName);
     }
 
-    void PowerManager::RegisterNewSentinel(std::shared_ptr<CpuSentinel> newSentinel) const
+    void PowerManager::SetCpuFrequencyRequest(const std::string &sentinelName, bsp::CpuFrequencyMHz request)
     {
-        if (cpuGovernor->RegisterNewSentinel(newSentinel)) {
-            newSentinel->ReadRegistrationData(lowPowerControl->GetCurrentFrequencyLevel(),
-                                              cpuGovernor->GetPermanentFrequencyRequested().isActive);
-        }
+        cpuGovernor->SetCpuFrequencyRequest(sentinelName, request);
+        // TODO:
+        // - move other UpdateCpuFrequency usages too
+        // - if it's ok to trigger algorithms on leave?
+        auto ret = UpdateCpuFrequency();
+        cpuStatistics.TrackChange(ret);
     }
 
-    void PowerManager::RemoveSentinel(std::string sentinelName) const
+    void PowerManager::ResetCpuFrequencyRequest(const std::string &sentinelName)
     {
-        cpuGovernor->RemoveSentinel(sentinelName);
+        cpuGovernor->ResetCpuFrequencyRequest(sentinelName);
+        //  TODO - same as above
+        auto ret = UpdateCpuFrequency();
+        cpuStatistics.TrackChange(ret);
     }
 
-    void PowerManager::SetCpuFrequencyRequest(std::string sentinelName,
-                                              bsp::CpuFrequencyMHz request,
-                                              bool permanentBlock)
+    bool PowerManager::IsCpuPernamentFrequency()
     {
-        cpuGovernor->SetCpuFrequencyRequest(std::move(sentinelName), request, permanentBlock);
+        return cpuAlgorithms->get(sys::cpu::AlgoID::FrequencyHold) != nullptr;
     }
 
-    void PowerManager::ResetCpuFrequencyRequest(std::string sentinelName, bool permanentBlock)
+    void PowerManager::SetPernamentFrequency(bsp::CpuFrequencyMHz freq)
     {
-        cpuGovernor->ResetCpuFrequencyRequest(std::move(sentinelName), permanentBlock);
+        cpuAlgorithms->emplace(sys::cpu::AlgoID::FrequencyHold, std::make_unique<sys::cpu::FrequencyHold>(freq,powerProfile));
     }
 
-    void PowerManager::SetCpuFrequency(bsp::CpuFrequencyMHz freq)
+    void PowerManager::ResetPernamentFrequency()
     {
-        UpdateCpuFrequencyMonitor(lowPowerControl->GetCurrentFrequencyLevel());
-        lowPowerControl->SetCpuFrequency(freq);
-        cpuGovernor->InformSentinelsAboutCpuFrequencyChange(freq);
+        cpuAlgorithms->remove(sys::cpu::AlgoID::FrequencyHold);
     }
 
-    void PowerManager::ResetFrequencyShiftCounter()
+    void PowerManager::SetCpuFrequency(bsp::CpuFrequencyMHz freq)
     {
-        aboveThresholdCounter = 0;
-        belowThresholdCounter = 0;
+       UpdateCpuFrequencyMonitor(lowPowerControl->GetCurrentFrequencyLevel());
+        while ( lowPowerControl->GetCurrentFrequencyLevel() != freq ) {
+            lowPowerControl->SetCpuFrequency(freq);
+            cpuGovernor->InformSentinelsAboutCpuFrequencyChange(freq);
+        }
     }
 
     [[nodiscard]] auto PowerManager::getExternalRamDevice() const noexcept -> std::shared_ptr<devices::Device>

@@ 84,7 84,7 @@ namespace sys
     }
 
     SystemManagerCommon::SystemManagerCommon(std::vector<std::unique_ptr<BaseServiceCreator>> &&creators)
-        : Service(service::name::system_manager, "", systemManagerStack), systemServiceCreators{std::move(creators)}
+        : Service(service::name::system_manager, "", systemManagerStack, ServicePriority::Low), systemServiceCreators{std::move(creators)}
     {
         // Specify list of channels which System Manager is registered to
         bus.channels            = {BusChannel::SystemManagerRequests};


@@ 192,6 192,7 @@ namespace sys
         // Power off request (pending)
         PowerOff();
 
+        // TODO these were globals = never cleared, is it needed now?
         powerManager.reset();
         cpuStatistics.reset();
         deviceManager.reset();


@@ 241,8 242,8 @@ namespace sys
 
     void SystemManagerCommon::StartSystem(InitFunction sysInit, InitFunction appSpaceInit, DeinitFunction sysDeinit)
     {
-        powerManager  = std::make_unique<PowerManager>();
         cpuStatistics = std::make_unique<CpuStatistics>();
+        powerManager  = std::make_unique<PowerManager>(*cpuStatistics);
         deviceManager = std::make_unique<DeviceManager>();
 
         systemInit   = std::move(sysInit);


@@ 256,7 257,7 @@ namespace sys
         freqTimer.start();
 
         powerManagerEfficiencyTimer = sys::TimerFactory::createPeriodicTimer(
-            this, "logPowerManagerEfficiency", constants::powerManagerLogsTimerInterval, [](sys::Timer &) {
+            this, "logPowerManagerEfficiency", constants::powerManagerLogsTimerInterval, [this](sys::Timer &) {
                 powerManager->LogPowerManagerEfficiency();
             });
         powerManagerEfficiencyTimer.start();


@@ 633,7 634,9 @@ namespace sys
         connect(typeid(sys::HoldCpuFrequencyMessage), [this](sys::Message *message) -> sys::MessagePointer {
             auto msg = static_cast<sys::HoldCpuFrequencyMessage *>(message);
             powerManager->SetCpuFrequencyRequest(msg->getName(), msg->getRequest());
-
+            if ( msg->getHandle() != nullptr ) {
+                xTaskNotifyGive(msg->getHandle());
+            }
             return sys::MessageNone{};
         });
 


@@ 644,17 647,19 @@ namespace sys
             return sys::MessageNone{};
         });
 
+        connect(typeid(sys::IsCpuPernament), [this](sys::Message *message) -> sys::MessagePointer {
+            return std::make_shared<sys::IsCpuPernamentResponse>(powerManager->IsCpuPernamentFrequency());
+        });
+
         connect(typeid(sys::HoldCpuFrequencyPermanentlyMessage), [this](sys::Message *message) -> sys::MessagePointer {
             auto msg = static_cast<sys::HoldCpuFrequencyPermanentlyMessage *>(message);
-            powerManager->SetCpuFrequencyRequest(msg->getName(), msg->getRequest(), true);
-
-            return sys::MessageNone{};
+            powerManager->SetPernamentFrequency(msg->request);
+            return std::make_shared<sys::HoldCpuFrequencyPermanentlyResponse>();
         });
 
         connect(typeid(sys::ReleaseCpuPermanentFrequencyMessage), [this](sys::Message *message) -> sys::MessagePointer {
-            auto msg = static_cast<sys::ReleaseCpuPermanentFrequencyMessage *>(message);
-            powerManager->ResetCpuFrequencyRequest(msg->getName(), true);
-            return sys::MessageNone{};
+            powerManager->ResetPernamentFrequency();
+            return std::make_shared<sys::HoldCpuFrequencyPermanentlyResponse>();
         });
 
         connect(typeid(app::manager::CheckIfStartAllowedMessage), [this](sys::Message *) -> sys::MessagePointer {


@@ 685,7 690,8 @@ namespace sys
         deviceManager->RegisterNewDevice(powerManager->getExternalRamDevice());
 
         cpuSentinel = std::make_shared<sys::CpuSentinel>(
-            service::name::system_manager, this, [this](bsp::CpuFrequencyMHz newFrequency) {
+            service::name::system_manager
+            , this, [this](bsp::CpuFrequencyMHz newFrequency) {
                 UpdateResourcesAfterCpuFrequencyChange(newFrequency);
             });
         powerManager->RegisterNewSentinel(cpuSentinel);


@@ 781,8 787,7 @@ namespace sys
             freqTimer.restart(constants::timerPeriodInterval);
         }
 
-        cpuStatistics->UpdatePercentageCpuLoad();
-        auto ret = powerManager->UpdateCpuFrequency(cpuStatistics->GetPercentageCpuLoad());
+        auto ret = powerManager->UpdateCpuFrequency();
         cpuStatistics->TrackChange(ret);
     }
 


@@ 800,8 805,4 @@ namespace sys
     std::vector<std::shared_ptr<app::ApplicationCommon>> SystemManagerCommon::applicationsList;
     cpp_freertos::MutexStandard SystemManagerCommon::serviceDestroyMutex;
     cpp_freertos::MutexStandard SystemManagerCommon::appDestroyMutex;
-    std::unique_ptr<PowerManager> SystemManagerCommon::powerManager;
-    std::unique_ptr<CpuStatistics> SystemManagerCommon::cpuStatistics;
-    std::unique_ptr<DeviceManager> SystemManagerCommon::deviceManager;
-
 } // namespace sys

@@ 0,0 1,33 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "AlgorithmFactory.hpp"
+#include "SystemManager/cpu/algorithm/FrequencyHold.hpp"
+#include <memory>
+
+namespace sys::cpu {
+
+    AlgorithmFactory::PutResult AlgorithmFactory::emplace(sys::cpu::AlgoID id, std::unique_ptr<Algorithm>&&algorithm)
+    {
+        if ( algorithms.find(id) != std::end(algorithms)) {
+            algorithms[id] = std::move(algorithm);
+            return  PutResult::Replaced;
+        } 
+        algorithms.emplace(id, std::move(algorithm));
+        return PutResult::Added;
+    }
+
+
+    Algorithm* AlgorithmFactory::get(sys::cpu::AlgoID id)
+    {
+        if ( auto el = algorithms.find(id); el != std::end(algorithms)) {
+            return el->second.get();
+        }
+        return nullptr;
+    }
+
+    void AlgorithmFactory::remove(sys::cpu::AlgoID id)
+    {
+        algorithms.erase(id);
+    }
+} // namespace sys::cpu

@@ 0,0 1,26 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "algorithm/Algorithm.hpp"
+#include <map>
+#include <memory>
+
+
+namespace sys::cpu {
+
+    class AlgorithmFactory
+    {
+        std::map<sys::cpu::AlgoID, std::unique_ptr<Algorithm>> algorithms;
+
+      public:
+        Algorithm* get(sys::cpu::AlgoID);
+        enum class PutResult {
+            Added,
+            Replaced,
+        };
+        PutResult emplace(sys::cpu::AlgoID id, std::unique_ptr<Algorithm>&&algorithm);
+        void remove(sys::cpu::AlgoID id);
+    };
+};

@@ 0,0 1,17 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "Algorithm.hpp"
+
+namespace sys::cpu
+{
+    bsp::CpuFrequencyMHz Algorithm::calculate(const AlgorithmData &data)
+    {
+        return calculateImplementation(data);
+    }
+
+    void Algorithm::reset()
+    {
+        resetImplementation();
+    }
+} // namespace sys::cpu

@@ 0,0 1,32 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "AlgorithmID.hpp"
+#include "SystemManager/SentinelView.hpp"
+#include "common.hpp"
+
+namespace sys::cpu
+{
+    struct AlgorithmData
+    {
+        unsigned int CPUload                 = 0;
+        bsp::CpuFrequencyMHz curentFrequency = bsp::CpuFrequencyMHz::Level_6;
+        sentinel::View sentinel;
+    };
+
+    class Algorithm
+    {
+      private:
+        [[nodiscard]] virtual bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData &) = 0;
+        virtual void resetImplementation()
+        {}
+
+      public:
+        [[nodiscard]] virtual bsp::CpuFrequencyMHz calculate(const AlgorithmData &) final;
+        virtual void reset() final;
+
+        virtual ~Algorithm() = default;
+    };
+} // namespace sys::cpu

@@ 0,0 1,15 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once 
+
+namespace sys::cpu
+{
+    enum class AlgoID
+    {
+        None,
+        FrequencyHold,
+        ImmediateUpscale,
+        FrequencyStepping,
+    };
+}

@@ 0,0 1,17 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "FrequencyHold.hpp"
+#include <algorithm>
+
+namespace sys::cpu
+{
+    FrequencyHold::FrequencyHold(bsp::CpuFrequencyMHz toHold, const bsp::PowerProfile &profile)
+        : toHold(toHold), profile(profile)
+    {}
+
+    bsp::CpuFrequencyMHz FrequencyHold::calculateImplementation(const AlgorithmData&data)
+    {
+        return std::max(toHold, profile.minimalFrequency);
+    }
+};

@@ 0,0 1,18 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "Algorithm.hpp"
+#include "lpm/PowerProfile.hpp"
+
+namespace sys::cpu
+{
+    class FrequencyHold : public Algorithm {
+            bsp::CpuFrequencyMHz toHold;
+            const bsp::PowerProfile &profile;
+            [[nodiscard]] bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData& data) override;
+        public:
+          explicit FrequencyHold(bsp::CpuFrequencyMHz toHold, const bsp::PowerProfile &profile);
+    };
+}

@@ 0,0 1,86 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "FrequencyStepping.hpp"
+#include "SystemManager/CpuGovernor.hpp"
+
+namespace sys::cpu
+{
+
+    FrequencyStepping::FrequencyStepping(const bsp::PowerProfile &powerProfile, CpuGovernor &cpuGovernor)
+        : powerProfile(powerProfile), cpuGovernor(cpuGovernor)
+    {}
+
+    bsp::CpuFrequencyMHz stepDown(bsp::CpuFrequencyMHz freq, const bsp::PowerProfile &profile)
+    {
+        switch (freq) {
+        case bsp::CpuFrequencyMHz::Level_6:
+            return bsp::CpuFrequencyMHz::Level_5;
+        case bsp::CpuFrequencyMHz::Level_5:
+            return bsp::CpuFrequencyMHz::Level_4;
+        case bsp::CpuFrequencyMHz::Level_4:
+            return bsp::CpuFrequencyMHz::Level_3;
+        case bsp::CpuFrequencyMHz::Level_3:
+            return bsp::CpuFrequencyMHz::Level_2;
+        case bsp::CpuFrequencyMHz::Level_2:
+            [[fallthrough]];
+        case bsp::CpuFrequencyMHz::Level_1:
+            [[fallthrough]];
+        case bsp::CpuFrequencyMHz::Level_0:
+            return profile.minimalFrequency;
+        }
+        return freq;
+    }
+
+    bsp::CpuFrequencyMHz  FrequencyStepping::calculateImplementation(const AlgorithmData&data)
+    {
+        auto load           = data.CPUload;
+        auto startFrequency = data.curentFrequency;
+        const auto min      = cpuGovernor.GetMinimumFrequencyRequested();
+
+        if (load > powerProfile.frequencyShiftUpperThreshold && startFrequency < bsp::CpuFrequencyMHz::Level_6) {
+            aboveThresholdCounter++;
+            belowThresholdCounter = 0;
+        }
+        else if (load < powerProfile.frequencyShiftLowerThreshold &&
+                 startFrequency > powerProfile.minimalFrequency) {
+            belowThresholdCounter++;
+            aboveThresholdCounter = 0;
+        }
+        else {
+            reset();
+        }
+
+        if (belowThresholdCounter == 0u) {
+            isFrequencyLoweringInProgress = false;
+        }
+
+        if (min.frequency > startFrequency) {}
+        else if (aboveThresholdCounter >= powerProfile.maxAboveThresholdCount) {
+            if (startFrequency < bsp::CpuFrequencyMHz::Level_4) {
+                reset();
+                return bsp::CpuFrequencyMHz::Level_4;
+            }
+            else {
+                reset();
+                return bsp::CpuFrequencyMHz::Level_6;
+            }
+        }
+        else {
+            if (belowThresholdCounter >= (isFrequencyLoweringInProgress ? powerProfile.maxBelowThresholdInRowCount
+                                                                        : powerProfile.maxBelowThresholdCount) &&
+                startFrequency > min.frequency) {
+                reset();
+                return stepDown(startFrequency, powerProfile);
+            }
+        }
+
+        return startFrequency;
+    }
+
+    void FrequencyStepping::resetImplementation()
+    {
+        aboveThresholdCounter = 0;
+        belowThresholdCounter = 0;
+    }
+}

@@ 0,0 1,26 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "Algorithm.hpp"
+#include "lpm/PowerProfile.hpp"
+
+namespace sys {
+    class CpuGovernor;
+}
+
+namespace sys::cpu
+{
+    class FrequencyStepping : public Algorithm {
+            const bsp::PowerProfile &powerProfile;
+            CpuGovernor &cpuGovernor;
+            unsigned int aboveThresholdCounter = 0;
+            unsigned int belowThresholdCounter = 0;
+            bool isFrequencyLoweringInProgress = true;
+        public:
+            FrequencyStepping(const bsp::PowerProfile&powerProfile, CpuGovernor &cpuGovernor);
+            [[nodiscard]] bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData&data) override;
+            void resetImplementation() override;
+    };
+}

@@ 0,0 1,14 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "ImmediateUpscale.hpp"
+
+namespace sys::cpu
+{
+    bsp::CpuFrequencyMHz ImmediateUpscale::calculateImplementation(const AlgorithmData&data)
+    {
+        auto now = data.sentinel.frequency;
+        auto was = data.curentFrequency; 
+        return std::max(now,was);
+    }
+} // namespace sys::cpu

@@ 0,0 1,17 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "Algorithm.hpp"
+#include "lpm/PowerProfile.hpp"
+#include <functional>
+#include <utility>
+
+namespace sys::cpu
+{
+    class ImmediateUpscale : public Algorithm {
+        public:
+          bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData&data) override;
+    };
+}

@@ 6,26 6,11 @@
 #include <memory>
 #include <vector>
 #include "SystemManager/CpuSentinel.hpp"
+#include "SentinelView.hpp"
 
 namespace sys
 {
 
-    namespace sentinel
-    {
-        struct Data
-        {
-            UBaseType_t ownerTCBNumber = 0;
-            /// name of sentinel thread responsible for curent minimum load
-            std::string name;
-            /// curent minimum frequency set in sentinel
-            bsp::CpuFrequencyMHz frequency = bsp::CpuFrequencyMHz::Level_0;
-            /// please do not use this task handle to perform actions, it's just for reference sake
-            TaskHandle_t task;
-            /// textual information on what actually happens
-            std::string reason;
-        };
-    }; // namespace sentinel
-
     using SentinelPointer = std::weak_ptr<CpuSentinel>;
 
     class GovernorSentinel


@@ 55,22 40,19 @@ namespace sys
         [[nodiscard]] auto GetNumberOfRegisteredSentinels() const noexcept -> uint32_t;
         void PrintAllSentinels() const noexcept;
 
-        void SetCpuFrequencyRequest(std::string sentinelName,
-                                    bsp::CpuFrequencyMHz request,
-                                    bool permanentBlock = false);
-        void ResetCpuFrequencyRequest(std::string sentinelName, bool permanentBlock = false);
+        void SetCpuFrequencyRequest(const std::string& sentinelName,
+                                    bsp::CpuFrequencyMHz request
+                                    );
+        void ResetCpuFrequencyRequest(const std::string& sentinelName);
 
-        [[nodiscard]] auto GetMinimumFrequencyRequested() const noexcept -> sentinel::Data;
+        [[nodiscard]] auto GetMinimumFrequencyRequested() const noexcept -> sentinel::View;
         void InformSentinelsAboutCpuFrequencyChange(bsp::CpuFrequencyMHz newFrequency) const noexcept;
 
-        [[nodiscard]] auto GetPermanentFrequencyRequested() const noexcept -> PermanentFrequencyToHold;
-
       private:
         static void PrintName(const GovernorSentinelPointer &element);
 
         /// this could be set - set is sorted :)
         GovernorSentinelsVector sentinels;
-        PermanentFrequencyToHold permanentFrequencyToHold{false, bsp::CpuFrequencyMHz::Level_0};
     };
 
 } // namespace sys

@@ 17,6 17,7 @@ namespace sys::cpu
           public:
             virtual void printSysUsage(struct task_prof_data *data, size_t size) = 0;
             virtual void printCPUChange(const cpu::UpdateResult &ret)            = 0;
+            virtual void printPowerConsumption()     = 0;
         };
 
         class NullPrinter : public Printer


@@ 25,6 26,7 @@ namespace sys::cpu
             {}
             void printCPUChange(const cpu::UpdateResult &ret)
             {}
+            void printPowerConsumption() {}
         };
 
         class LogPrinter : public Printer


@@ 32,6 34,7 @@ namespace sys::cpu
           public:
             void printSysUsage(struct task_prof_data *data, size_t size) override;
             void printCPUChange(const cpu::UpdateResult &ret) override;
+            void printPowerConsumption() override;
         };
 
         class PackPrinter : public Printer


@@ 39,6 42,7 @@ namespace sys::cpu
           public:
             void printSysUsage(struct task_prof_data *data, size_t size) override;
             void printCPUChange(const cpu::UpdateResult &ret) override;
+            void printPowerConsumption() override;
         };
     }; // namespace stats
 };     // namespace sys::cpu

@@ 13,12 13,6 @@
 
 namespace sys
 {
-    struct PermanentFrequencyToHold
-    {
-        bool isActive;
-        bsp::CpuFrequencyMHz frequencyToHold;
-    };
-
     /// Each sentinel manages the requests, i.e. when it is needed it sends messages to CpuGovernor with the required
     /// minimum CPU frequency to perform the task (e.g. screen redraw). Furthermore, every sentinel is informed
     /// immediately after changing the frequency. This allows it to invoke a callback to the service to update their


@@ 30,29 24,24 @@ namespace sys
         explicit CpuSentinel(std::string name,
                              sys::Service *service,
                              std::function<void(bsp::CpuFrequencyMHz)> callback = nullptr);
-        ~CpuSentinel() = default;
 
         [[nodiscard]] auto GetName() const noexcept -> std::string;
         void HoldMinimumFrequency(bsp::CpuFrequencyMHz frequencyToHold);
-        bool HoldMinimumFrequencyAndWait(bsp::CpuFrequencyMHz frequencyToHold,
-                                         TaskHandle_t taskToNotify,
-                                         uint32_t timeout);
         void ReleaseMinimumFrequency();
 
+        // TODO actually sentinel provides api here so i.e. in case of irq is useless
         void HoldFrequencyPermanently(bsp::CpuFrequencyMHz frequencyToHold);
         [[nodiscard]] auto GetFrequency() const noexcept -> bsp::CpuFrequencyMHz;
-        void ReleasePermanentFrequency();
-        [[nodiscard]] bool isPermanentFrequencyActive();
 
         void CpuFrequencyHasChanged(bsp::CpuFrequencyMHz newFrequency);
-        void ReadRegistrationData(bsp::CpuFrequencyMHz frequencyHz, bool permanentFrequency);
+
+        void ReadRegistrationData(bsp::CpuFrequencyMHz frequencyHz);
         TaskHandle_t getTask();
         std::string getReason();
 
       protected:
         const std::string name;
         bsp::CpuFrequencyMHz currentFrequencyToHold{bsp::CpuFrequencyMHz::Level_0};
-        PermanentFrequencyToHold permanentFrequencyToHold{false, bsp::CpuFrequencyMHz::Level_0};
         std::atomic<bsp::CpuFrequencyMHz> currentFrequency{bsp::CpuFrequencyMHz::Level_0};
         sys::Service *owner{nullptr};
 


@@ 61,7 50,6 @@ namespace sys
         /// critical section or mutex support necessary
         std::function<void(bsp::CpuFrequencyMHz)> callback;
 
-        TaskHandle_t taskWaitingForFrequency = nullptr;
         std::string currentReason;
     };
 

@@ 24,11 24,11 @@ namespace sys
         /// stores system usage, should be called before any CPU frequency change
         /// this way we know what services were in use and for how long before it happened
         void StoreSysUsage();
-        [[nodiscard]] uint32_t GetPercentageCpuLoad() const noexcept;
-        void UpdatePercentageCpuLoad();
+        [[nodiscard]] uint32_t GetPercentageCpuLoad();
         void TrackChange(const cpu::UpdateResult &ret);
 
       private:
+        void UpdatePercentageCpuLoad();
         /// used to print stored data in CpuStatistics on change
         std::unique_ptr<cpu::stats::Printer> printer;
         uint32_t ComputeIncrease(uint32_t currentCount, uint32_t lastCount) const;

@@ 13,8 13,15 @@
 #include <bsp/lpm/PowerProfile.hpp>
 #include <vector>
 
+namespace sys::cpu
+{
+    class AlgorithmFactory;
+}
+
 namespace sys
 {
+    class CpuStatistics;
+
     class CpuFrequencyMonitor
     {
       public:


@@ 31,9 38,8 @@ namespace sys
 
     class PowerManager
     {
-
       public:
-        PowerManager();
+        explicit PowerManager(CpuStatistics& stats);
         ~PowerManager();
 
         int32_t PowerOff();


@@ 47,44 53,38 @@ namespace sys
         /// limit (frequencyShiftUpperThreshold), CPU frequency is increased; if for the last 'maxBelowThresholdCount'
         /// periods the current CPU usage was below the lower limit (frequencyShiftLowerThreshold), CPU frequency is
         /// reduced frequency
-        /// @param current cpu load
-        [[nodiscard]] cpu::UpdateResult UpdateCpuFrequency(uint32_t cpuLoad);
+        [[nodiscard]] cpu::UpdateResult UpdateCpuFrequency();
 
         [[nodiscard]] auto getExternalRamDevice() const noexcept -> std::shared_ptr<devices::Device>;
 
         void RegisterNewSentinel(std::shared_ptr<CpuSentinel> newSentinel) const;
         void RemoveSentinel(std::string sentinelName) const;
-        void SetCpuFrequencyRequest(std::string sentinelName,
-                                    bsp::CpuFrequencyMHz request,
-                                    bool permanentBlock = false);
-        void ResetCpuFrequencyRequest(std::string sentinelName, bool permanentBlock = false);
+        void SetCpuFrequencyRequest(const std::string &sentinelName, bsp::CpuFrequencyMHz request);
+        void ResetCpuFrequencyRequest(const std::string &sentinelName);
+        bool IsCpuPernamentFrequency();
+        void SetPernamentFrequency(bsp::CpuFrequencyMHz freq);
+        void ResetPernamentFrequency();
+
         void LogPowerManagerEfficiency();
         void SetBootSuccess();
 
       private:
-        /// called when the CPU frequency needs to be increased
-        void IncreaseCpuFrequency(bsp::CpuFrequencyMHz newFrequency);
-
-        /// called when the CPU frequency needs to be reduced
-        /// @note the frequency is always reduced by one step
-        void DecreaseCpuFrequency();
-
         void ResetFrequencyShiftCounter();
         void SetCpuFrequency(bsp::CpuFrequencyMHz freq);
 
         void UpdateCpuFrequencyMonitor(bsp::CpuFrequencyMHz currentFreq);
 
-        uint32_t belowThresholdCounter{0};
-        uint32_t aboveThresholdCounter{0};
         TickType_t lastCpuFrequencyChangeTimestamp{0};
-        bool isFrequencyLoweringInProgress{false};
 
         std::vector<CpuFrequencyMonitor> cpuFrequencyMonitor;
 
-        std::unique_ptr<bsp::LowPowerMode> lowPowerControl;
         std::shared_ptr<drivers::DriverSEMC> driverSEMC;
+        std::unique_ptr<bsp::LowPowerMode> lowPowerControl;
         std::unique_ptr<CpuGovernor> cpuGovernor;
         const bsp::PowerProfile powerProfile;
+
+        std::unique_ptr<sys::cpu::AlgorithmFactory> cpuAlgorithms;
+        CpuStatistics& cpuStatistics;
     };
 
 } // namespace sys

@@ 0,0 1,28 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include "common.hpp"
+#include "FreeRTOS.h"
+#include "portmacro.h"
+#include <string>
+
+namespace sys
+{
+
+    namespace sentinel
+    {
+        struct View
+        {
+            UBaseType_t ownerTCBNumber = 0;
+            /// name of sentinel thread responsible for curent minimum load
+            std::string name;
+            /// curent minimum frequency set in sentinel
+            bsp::CpuFrequencyMHz frequency = bsp::CpuFrequencyMHz::Level_0;
+            /// textual information on what actually happens
+            std::string reason;
+        };
+    }; // namespace sentinel
+}
+

@@ 3,13 3,21 @@
 
 #pragma once
 
-#include "SystemManager/CpuGovernor.hpp"
+#include "SystemManager/cpu/algorithm/AlgorithmID.hpp"
+#include "SystemManager/SentinelView.hpp"
+
 namespace sys::cpu
 {
     struct UpdateResult
     {
-        bool changed                      = false;
+        enum class Result {
+            UpScaled,   /// frequency risen
+            Downscaled, /// frequency downscaled
+            NoChange    /// nothing to do
+        };
+        Result changed                      = Result::NoChange;
         bsp::CpuFrequencyMHz frequencySet = bsp::CpuFrequencyMHz::Level_0;
-        sentinel::Data data{};
+        sentinel::View data{};
+        AlgoID id = AlgoID::None;
     };
 }; // namespace sys::cpu

@@ 211,9 211,9 @@ namespace sys
         static std::vector<std::shared_ptr<app::ApplicationCommon>> applicationsList;
         static cpp_freertos::MutexStandard serviceDestroyMutex;
         static cpp_freertos::MutexStandard appDestroyMutex;
-        static std::unique_ptr<PowerManager> powerManager;
-        static std::unique_ptr<CpuStatistics> cpuStatistics;
-        static std::unique_ptr<DeviceManager> deviceManager;
+        std::unique_ptr<CpuStatistics> cpuStatistics;
+        std::unique_ptr<PowerManager> powerManager;
+        std::unique_ptr<DeviceManager> deviceManager;
     };
 } // namespace sys
 

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


@@ 11,9 11,9 @@ namespace sys
         Failure,
         Timeout,
         ServiceDoesntExist,
-        // This is used in application's template in base class messages handler. The meaning is that
-        // message that was processed by base class implementation of the DataReceivedHandler was not processed
-        // and it should be handled by the class next in hierarchy.
+        /// This is used in application's template in base class messages handler. The meaning is that
+        /// message that was processed by base class implementation of the DataReceivedHandler was not processed
+        /// and it should be handled by the class next in hierarchy.
         Unresolved
     };
 }

@@ 1,20 1,23 @@
-// Copyright (c) 2017-2021, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
 
+#include "FreeRTOS.h"
+#include "task.h"
 #include <Service/Message.hpp>
 #include <bsp/common.hpp>
 
 namespace sys
 {
 
+    /// TODO move data to struct...
     class HoldCpuFrequencyMessage : public sys::DataMessage
     {
       public:
-        HoldCpuFrequencyMessage(std::string sentinelName, bsp::CpuFrequencyMHz request)
+        HoldCpuFrequencyMessage(std::string sentinelName, bsp::CpuFrequencyMHz request, TaskHandle_t handle)
             : sys::DataMessage(MessageType::SystemManagerCpuFrequency), sentinelName(std::move(sentinelName)),
-              frequencyRequested(request)
+              frequencyRequested(request), handle(handle)
         {}
 
         [[nodiscard]] auto getRequest() const noexcept


@@ 27,9 30,15 @@ namespace sys
             return sentinelName;
         };
 
+        [[nodiscard]] TaskHandle_t getHandle() const
+        {
+            return handle;
+        }
+
       private:
         std::string sentinelName;
         bsp::CpuFrequencyMHz frequencyRequested = bsp::CpuFrequencyMHz::Level_0;
+        TaskHandle_t handle;
     };
 
     class ReleaseCpuFrequencyMessage : public sys::DataMessage


@@ 48,19 57,36 @@ namespace sys
         std::string sentinelName;
     };
 
-    class HoldCpuFrequencyPermanentlyMessage : public HoldCpuFrequencyMessage
+    struct IsCpuPernament : public sys::DataMessage
     {
       public:
-        HoldCpuFrequencyPermanentlyMessage(std::string sentinelName, bsp::CpuFrequencyMHz request)
-            : HoldCpuFrequencyMessage(sentinelName, request)
+        explicit IsCpuPernament() 
         {}
     };
 
-    class ReleaseCpuPermanentFrequencyMessage : public ReleaseCpuFrequencyMessage
+    struct IsCpuPernamentResponse : public sys::ResponseMessage
     {
       public:
-        ReleaseCpuPermanentFrequencyMessage(std::string sentinelName) : ReleaseCpuFrequencyMessage(sentinelName)
+        explicit IsCpuPernamentResponse(bool pernament) : pernament(pernament)
         {}
+        const bool pernament = false;
     };
 
+
+
+    struct HoldCpuFrequencyPermanentlyMessage : public sys::DataMessage
+    {
+      public:
+        explicit HoldCpuFrequencyPermanentlyMessage(bsp::CpuFrequencyMHz request) : request(request)
+        {}
+        const bsp::CpuFrequencyMHz request;
+    };
+
+    class HoldCpuFrequencyPermanentlyResponse : public sys::ResponseMessage
+    {
+    };
+
+    class ReleaseCpuPermanentFrequencyMessage : public sys::DataMessage
+    {};
+
 } // namespace sys