17 files changed, 230 insertions(+), 47 deletions(-)

M module-sys/SystemManager/CpuStatistics.cpp
M module-sys/SystemManager/PowerManager.cpp
M module-sys/SystemManager/cpu/AlgorithmFactory.cpp
M module-sys/SystemManager/cpu/AlgorithmFactory.hpp
M module-sys/SystemManager/cpu/algorithm/Algorithm.cpp
M module-sys/SystemManager/cpu/algorithm/Algorithm.hpp
A module-sys/SystemManager/cpu/algorithm/AlgorithmChange.hpp
M module-sys/SystemManager/cpu/algorithm/FrequencyHold.cpp
M module-sys/SystemManager/cpu/algorithm/FrequencyHold.hpp
M module-sys/SystemManager/cpu/algorithm/FrequencyStepping.cpp
M module-sys/SystemManager/cpu/algorithm/FrequencyStepping.hpp
M module-sys/SystemManager/cpu/algorithm/ImmediateUpscale.cpp
M module-sys/SystemManager/cpu/algorithm/ImmediateUpscale.hpp
M module-sys/SystemManager/include/SystemManager/SysCpuUpdateResult.hpp
M module-sys/SystemManager/tests/CMakeLists.txt
A module-sys/SystemManager/tests/test-cpu-algorithms.cpp
M test/Catch2

@@ 51,7 51,7 @@ namespace sys
 
     void CpuStatistics::TrackChange(const cpu::UpdateResult &ret)
     {
-        if (ret.changed != sys::cpu::UpdateResult::Result::NoChange) {
+        if (ret.changed != sys::cpu::algorithm::Change::NoChange) {
             printer->printCPUChange(ret);
 #if PROF_ON
             printer->printSysUsage(data, data_size);

@@ 90,38 90,25 @@ namespace sys
     [[nodiscard]] cpu::UpdateResult PowerManager::UpdateCpuFrequency()
     {
         uint32_t cpuLoad = cpuStatistics.GetPercentageCpuLoad();
-        cpu::UpdateResult result;
+        cpu::UpdateResult retval;
         cpu::AlgorithmData data{
             cpuLoad, lowPowerControl->GetCurrentFrequencyLevel(), cpuGovernor->GetMinimumFrequencyRequested()};
 
-        auto _ = gsl::finally([&result, this, data] {
-            result.frequencySet = lowPowerControl->GetCurrentFrequencyLevel();
-            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;
+        auto _ = gsl::finally([&retval, this, data] {
+            retval.frequencySet = lowPowerControl->GetCurrentFrequencyLevel();
+            retval.data         = data.sentinel;
         });
 
         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;
-                }
-            }
+        auto result = cpuAlgorithms->calculate(algorithms, data, &retval.id);
+        if (result.change == cpu::algorithm::Change::NoChange or result.change == cpu::algorithm::Change::Hold) {
+            return retval;
         }
-
-        return result;
+        SetCpuFrequency(result.value);
+        cpuAlgorithms->reset(algorithms);
+        return retval;
     }
 
     void PowerManager::RegisterNewSentinel(std::shared_ptr<CpuSentinel> newSentinel) const

@@ 30,4 30,31 @@ namespace sys::cpu
     {
         algorithms.erase(id);
     }
+
+    AlgorithmResult AlgorithmFactory::calculate(const std::list<sys::cpu::AlgoID> &algorithms,
+                                                cpu::AlgorithmData &data,
+                                                AlgoID *used)
+    {
+        for (auto id : algorithms) {
+            if (auto algo = get(id); algo != nullptr) {
+                if (auto result = algo->calculate(data); result.change != sys::cpu::algorithm::Change::NoChange) {
+                    if (used != nullptr) {
+                        *used = id;
+                    }
+                    return result;
+                }
+            }
+        }
+        return sys::cpu::AlgorithmResult{sys::cpu::algorithm::Change::NoChange, bsp::CpuFrequencyMHz::Level_6};
+    }
+
+    void AlgorithmFactory::reset(const std::list<sys::cpu::AlgoID> &algorithms)
+    {
+        for (auto again : algorithms) {
+            if (auto al = get(again); al != nullptr) {
+                al->reset();
+            }
+        }
+    }
+
 } // namespace sys::cpu

@@ 4,6 4,7 @@
 #pragma once
 
 #include "algorithm/Algorithm.hpp"
+#include <list>
 #include <map>
 #include <memory>
 


@@ 22,6 23,15 @@ namespace sys::cpu
             Replaced,
         };
         PutResult emplace(sys::cpu::AlgoID id, std::unique_ptr<Algorithm> &&algorithm);
+
+        /// use algorithms from factory depending on list to calculate frequency
+        /// return used algorithm in used paramter
+        AlgorithmResult calculate(const std::list<sys::cpu::AlgoID> &algorithms,
+                                  cpu::AlgorithmData &data,
+                                  AlgoID *used = nullptr);
+        /// reset internal algorithms data
+        void reset(const std::list<sys::cpu::AlgoID> &algorithms);
+
         void remove(sys::cpu::AlgoID id);
     };
 }; // namespace sys::cpu

@@ 5,7 5,7 @@
 
 namespace sys::cpu
 {
-    bsp::CpuFrequencyMHz Algorithm::calculate(const AlgorithmData &data)
+    AlgorithmResult Algorithm::calculate(const AlgorithmData &data)
     {
         return calculateImplementation(data);
     }

@@ 4,6 4,7 @@
 #pragma once
 
 #include "AlgorithmID.hpp"
+#include "AlgorithmChange.hpp"
 #include "SystemManager/SentinelView.hpp"
 #include "common.hpp"
 


@@ 16,15 17,23 @@ namespace sys::cpu
         sentinel::View sentinel;
     };
 
+    struct AlgorithmResult
+    {
+        /// is change required by algorithm
+        algorithm::Change change;
+        // value requested by algorithm
+        bsp::CpuFrequencyMHz value;
+    };
+
     class Algorithm
     {
       private:
-        [[nodiscard]] virtual bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData &) = 0;
+        [[nodiscard]] virtual AlgorithmResult calculateImplementation(const AlgorithmData &) = 0;
         virtual void resetImplementation()
         {}
 
       public:
-        [[nodiscard]] virtual bsp::CpuFrequencyMHz calculate(const AlgorithmData &) final;
+        [[nodiscard]] virtual AlgorithmResult calculate(const AlgorithmData &) final;
         virtual void reset() final;
 
         virtual ~Algorithm() = default;

@@ 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::algorithm
+{
+    enum class Change
+    {
+        UpScaled,   /// frequency risen
+        Downscaled, /// frequency downscaled
+        Hold,       /// frequency requested to be held
+        NoChange    /// nothing to do
+    };
+}

@@ 3,6 3,7 @@
 
 #include "FrequencyHold.hpp"
 #include <algorithm>
+#include <list>
 
 namespace sys::cpu
 {


@@ 10,8 11,15 @@ namespace sys::cpu
         : toHold(toHold), profile(profile)
     {}
 
-    bsp::CpuFrequencyMHz FrequencyHold::calculateImplementation(const AlgorithmData &data)
+    AlgorithmResult FrequencyHold::calculateImplementation(const AlgorithmData &data)
     {
-        return std::max(toHold, profile.minimalFrequency);
+        const auto retval = std::max(toHold, profile.minimalFrequency);
+        if (toHold > data.curentFrequency) {
+            return {algorithm::Change::UpScaled, retval};
+        }
+        else if (toHold < data.curentFrequency) {
+            return {algorithm::Change::Downscaled, retval};
+        }
+        return {algorithm::Change::Hold, retval};
     }
 }; // namespace sys::cpu

@@ 12,7 12,7 @@ namespace sys::cpu
     {
         bsp::CpuFrequencyMHz toHold;
         const bsp::PowerProfile &profile;
-        [[nodiscard]] bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData &data) override;
+        [[nodiscard]] AlgorithmResult calculateImplementation(const AlgorithmData &data) override;
 
       public:
         explicit FrequencyHold(bsp::CpuFrequencyMHz toHold, const bsp::PowerProfile &profile);

@@ 32,7 32,7 @@ namespace sys::cpu
         return freq;
     }
 
-    bsp::CpuFrequencyMHz FrequencyStepping::calculateImplementation(const AlgorithmData &data)
+    AlgorithmResult FrequencyStepping::calculateImplementation(const AlgorithmData &data)
     {
         const auto load           = data.CPUload;
         const auto startFrequency = data.curentFrequency;


@@ 58,11 58,11 @@ namespace sys::cpu
         else if (aboveThresholdCounter >= powerProfile.maxAboveThresholdCount) {
             if (startFrequency < bsp::CpuFrequencyMHz::Level_4) {
                 reset();
-                return bsp::CpuFrequencyMHz::Level_4;
+                return {algorithm::Change::UpScaled, bsp::CpuFrequencyMHz::Level_4};
             }
             else {
                 reset();
-                return bsp::CpuFrequencyMHz::Level_6;
+                return {algorithm::Change::UpScaled, bsp::CpuFrequencyMHz::Level_6};
             }
         }
         else {


@@ 70,11 70,11 @@ namespace sys::cpu
                                                                         : powerProfile.maxBelowThresholdCount) &&
                 startFrequency > min.frequency) {
                 reset();
-                return stepDown(startFrequency, powerProfile);
+                return {algorithm::Change::Downscaled, stepDown(startFrequency, powerProfile)};
             }
         }
 
-        return startFrequency;
+        return {algorithm::Change::NoChange, startFrequency};
     }
 
     void FrequencyStepping::resetImplementation()

@@ 23,7 23,7 @@ namespace sys::cpu
 
       public:
         FrequencyStepping(const bsp::PowerProfile &powerProfile, CpuGovernor &cpuGovernor);
-        [[nodiscard]] bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData &data) override;
+        [[nodiscard]] AlgorithmResult calculateImplementation(const AlgorithmData &data) override;
         void resetImplementation() override;
     };
 } // namespace sys::cpu

@@ 5,10 5,13 @@
 
 namespace sys::cpu
 {
-    bsp::CpuFrequencyMHz ImmediateUpscale::calculateImplementation(const AlgorithmData &data)
+    AlgorithmResult ImmediateUpscale::calculateImplementation(const AlgorithmData &data)
     {
         const auto now = data.sentinel.frequency;
         const auto was = data.curentFrequency;
-        return std::max(now, was);
+        if (now > was) {
+            return {algorithm::Change::UpScaled, now};
+        }
+        return {algorithm::Change::NoChange, was};
     }
 } // namespace sys::cpu

@@ 13,6 13,6 @@ namespace sys::cpu
     class ImmediateUpscale : public Algorithm
     {
       public:
-        bsp::CpuFrequencyMHz calculateImplementation(const AlgorithmData &data) override;
+        AlgorithmResult calculateImplementation(const AlgorithmData &data) override;
     };
 } // namespace sys::cpu

@@ 3,6 3,7 @@
 
 #pragma once
 
+#include "SystemManager/cpu/algorithm/AlgorithmChange.hpp"
 #include "SystemManager/cpu/algorithm/AlgorithmID.hpp"
 #include "SystemManager/SentinelView.hpp"
 


@@ 10,13 11,7 @@ namespace sys::cpu
 {
     struct UpdateResult
     {
-        enum class Result
-        {
-            UpScaled,   /// frequency risen
-            Downscaled, /// frequency downscaled
-            NoChange    /// nothing to do
-        };
-        Result changed                    = Result::NoChange;
+        algorithm::Change changed         = algorithm::Change::NoChange;
         bsp::CpuFrequencyMHz frequencySet = bsp::CpuFrequencyMHz::Level_0;
         sentinel::View data{};
         AlgoID id = AlgoID::None;

@@ 15,3 15,12 @@ add_catch2_executable(
     LIBS
         module-sys
 )
+
+add_catch2_executable(
+    NAME
+        cpu-algorithms
+    SRCS
+        test-cpu-algorithms.cpp
+    LIBS
+        module-sys
+)

@@ 0,0 1,120 @@
+// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include <catch2/catch.hpp>
+
+#include "SystemManager/cpu/AlgorithmFactory.hpp"
+#include "SystemManager/cpu/algorithm/Algorithm.hpp"
+#include "SystemManager/cpu/algorithm/FrequencyHold.hpp"
+#include "SystemManager/cpu/algorithm/FrequencyStepping.hpp"
+#include "SystemManager/cpu/algorithm/ImmediateUpscale.hpp"
+#include "SystemManager/CpuGovernor.hpp"
+
+namespace mockup
+{
+    class Governor : public sys::CpuGovernor
+    {
+      public:
+        [[nodiscard]] bsp::CpuFrequencyMHz sentinelsKept() const
+        {
+            return bsp::CpuFrequencyMHz::Level_6;
+        }
+
+        [[nodiscard]] auto GetMinimumFrequencyRequested() const noexcept -> sys::sentinel::View
+        {
+            return {0, "test", sentinelsKept(), "because of test"};
+        }
+    };
+
+    std::unique_ptr<sys::cpu::AlgorithmFactory> getAlgos(
+        mockup::Governor &cpuGovernor,
+        const bsp::PowerProfile &powerProfile,
+        const bsp::CpuFrequencyMHz freq = bsp::CpuFrequencyMHz::Level_3)
+    {
+        auto cpuAlgorithms = std::make_unique<sys::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));
+        cpuAlgorithms->emplace(sys::cpu::AlgoID::FrequencyHold,
+                               std::make_unique<sys::cpu::FrequencyHold>(freq, powerProfile));
+        return cpuAlgorithms;
+    }
+} // namespace mockup
+
+TEST_CASE("ImmediateUpscale")
+{
+    auto cpuGovernor        = std::make_unique<mockup::Governor>();
+    const auto freq_to_hold = bsp::CpuFrequencyMHz::Level_3;
+    const bsp::PowerProfile pp{};
+    std::shared_ptr<sys::cpu::AlgorithmFactory> cpuAlgorithms = mockup::getAlgos(*cpuGovernor, pp, freq_to_hold);
+
+    auto algo = cpuAlgorithms->get(sys::cpu::AlgoID::ImmediateUpscale);
+    sys::cpu::AlgorithmData data{0, bsp::CpuFrequencyMHz::Level_2, cpuGovernor->GetMinimumFrequencyRequested()};
+
+    REQUIRE(algo != nullptr);
+    auto result = algo->calculate(data);
+    /// frequency now is set in data as 0
+    /// sentinelsKept() frequency is lvl 6
+    REQUIRE(result.change == sys::cpu::algorithm::Change::UpScaled);
+    REQUIRE(result.value == cpuGovernor->sentinelsKept());
+}
+
+TEST_CASE("FrequencyHold")
+{
+    auto cpuGovernor        = std::make_unique<mockup::Governor>();
+    const auto freq_to_hold = bsp::CpuFrequencyMHz::Level_3;
+    const bsp::PowerProfile pp{};
+    std::shared_ptr<sys::cpu::AlgorithmFactory> cpuAlgorithms = mockup::getAlgos(*cpuGovernor, pp, freq_to_hold);
+    SECTION("FrequencyHold")
+    {
+        /// frequency to hold is set to 3 in mockup factory
+        auto algo = cpuAlgorithms->get(sys::cpu::AlgoID::FrequencyHold);
+        REQUIRE(algo != nullptr);
+
+        SECTION("hold the frequency as it is")
+        {
+            auto data =
+                sys::cpu::AlgorithmData{0, bsp::CpuFrequencyMHz::Level_3, cpuGovernor->GetMinimumFrequencyRequested()};
+            auto result = algo->calculate(data);
+            REQUIRE(result.value == freq_to_hold);
+            REQUIRE(result.change == sys::cpu::algorithm::Change::Hold);
+        }
+
+        SECTION("upscale from curent frequency")
+        {
+            const bsp::CpuFrequencyMHz freq_now = bsp::CpuFrequencyMHz::Level_0;
+            auto data   = sys::cpu::AlgorithmData{0, freq_now, cpuGovernor->GetMinimumFrequencyRequested()};
+            auto result = algo->calculate(data);
+            REQUIRE(result.value == freq_to_hold);
+            REQUIRE(result.change == sys::cpu::algorithm::Change::UpScaled);
+        }
+
+        SECTION("downscale from curent frequency")
+        {
+            const bsp::CpuFrequencyMHz freq_now = bsp::CpuFrequencyMHz::Level_6;
+            auto data   = sys::cpu::AlgorithmData{0, freq_now, cpuGovernor->GetMinimumFrequencyRequested()};
+            auto result = algo->calculate(data);
+            REQUIRE(result.value == freq_to_hold);
+            REQUIRE(result.change == sys::cpu::algorithm::Change::Downscaled);
+        }
+    }
+}
+
+TEST_CASE("combined algos")
+{
+    auto cpuGovernor        = std::make_unique<mockup::Governor>();
+    const auto freq_to_hold = bsp::CpuFrequencyMHz::Level_3;
+    const bsp::PowerProfile pp{};
+    std::shared_ptr<sys::cpu::AlgorithmFactory> cpuAlgorithms = mockup::getAlgos(*cpuGovernor, pp, freq_to_hold);
+    const bsp::CpuFrequencyMHz freq_now                       = bsp::CpuFrequencyMHz::Level_6;
+
+    auto algorithms = {
+        sys::cpu::AlgoID::FrequencyHold, sys::cpu::AlgoID::ImmediateUpscale, sys::cpu::AlgoID::FrequencyStepping};
+    auto data = sys::cpu::AlgorithmData{0, freq_now, cpuGovernor->GetMinimumFrequencyRequested()};
+
+    sys::cpu::AlgoID id;
+    auto result = cpuAlgorithms->calculate(algorithms, data, &id);
+    REQUIRE(id == sys::cpu::AlgoID::FrequencyHold);
+    REQUIRE(result.value == bsp::CpuFrequencyMHz::Level_3);
+    REQUIRE(result.change == sys::cpu::algorithm::Change::Downscaled);
+}

@@ 1,1 1,1 @@
-Subproject commit ff349a50bfc6214b4081f4ca63c7de35e2162f60
+Subproject commit c4e3767e265808590986d5db6ca1b5532a7f3d13