From 32c6769cb684b99d6154c41fa72a421ce1e23c0c Mon Sep 17 00:00:00 2001
From: Lefucjusz <marcin.lyda@mudita.com>
Date: Mon, 8 Jan 2024 13:47:02 +0100
Subject: [PATCH] [BH-1657][BH-1833][BH-1854] Add WFI and SDRAM self-refresh
 mode

* Added mechanism enabling CPU to
enter WFI mode when the OS is
in idle, what results in large
power consumption reduction.
* Added mechanism to switch SDRAM to
self-refresh mode before entering
WFI, what resulted in further power
consumption reduction.
---
 board/rt1051/ldscripts/sections.ld            |  26 +-
 harmony_changelog.md                          |   4 +-
 .../windows/advanced/CPUModeTestWindow.cpp    |   2 +-
 module-bsp/board/linux/lpm/LinuxLPM.cpp       |  23 ++
 module-bsp/board/linux/lpm/LinuxLPM.h         |   8 +
 module-bsp/board/rt1051/bellpx/CMakeLists.txt |   2 +
 module-bsp/board/rt1051/bellpx/board.cpp      |  20 +-
 module-bsp/board/rt1051/bellpx/brownout.cpp   |  23 +-
 .../rt1051/bellpx/bsp/lpm/EnterSleepMode.c    |  29 ++
 .../rt1051/bellpx/bsp/lpm/EnterSleepMode.h    |  13 +
 .../board/rt1051/bellpx/bsp/lpm/RT1051LPM.cpp |  21 ++
 .../board/rt1051/bellpx/bsp/lpm/RT1051LPM.hpp |   5 +
 .../rt1051/bellpx/bsp/lpm/WfiController.cpp   | 175 ++++++++++++
 .../rt1051/bellpx/bsp/lpm/WfiController.hpp   |  14 +
 .../rt1051/bellpx/bsp/switches/switches.cpp   |   6 +-
 .../board/rt1051/bellpx/clock_config.cpp      |   3 +
 module-bsp/board/rt1051/bellpx/irq_gpio.cpp   |   4 -
 .../board/rt1051/bsp/lpm/RT1051LPMCommon.cpp  |  29 +-
 .../board/rt1051/bsp/lpm/RT1051LPMCommon.hpp  |   5 +-
 module-bsp/board/rt1051/common/board.cpp      | 254 +++++++++---------
 .../rt1051/common/startup_mimxrt1052.cpp      |  18 +-
 .../board/rt1051/os/fsl_runtimestat_gpt.c     |  18 +-
 .../rt1051/os/include/fsl_runtimestat_gpt.h   |  19 ++
 module-bsp/board/rt1051/os/include/macros.h   |   9 +-
 module-bsp/board/rt1051/puretx/board.cpp      |  15 +-
 .../board/rt1051/puretx/bsp/lpm/RT1051LPM.cpp |  17 ++
 .../board/rt1051/puretx/bsp/lpm/RT1051LPM.hpp |   5 +
 module-bsp/bsp/bsp.hpp                        |  17 +-
 module-bsp/bsp/lpm/bsp_lpm.hpp                |   7 +
 module-platform/rt1051/src/RT1051Platform.cpp |   8 +-
 .../service-bluetooth/ServiceBluetooth.cpp    |   2 +-
 .../service-cellular/ServiceCellular.cpp      |   2 +-
 module-services/service-eink/ServiceEink.cpp  |   8 +-
 module-sys/SystemManager/CpuGovernor.cpp      |  39 +++
 module-sys/SystemManager/CpuSentinel.cpp      |  24 +-
 module-sys/SystemManager/PowerManager.cpp     |  54 +++-
 .../SystemManager/SystemManagerCommon.cpp     |  14 +-
 .../include/SystemManager/CpuGovernor.hpp     |   5 +
 .../include/SystemManager/CpuSentinel.hpp     |   5 +
 .../include/SystemManager/PowerManager.hpp    |  18 +-
 .../include/system/messages/BlockWfiMode.hpp  |  39 +++
 products/BellHybrid/BellHybridMain.cpp        |   9 -
 .../MeditationTimer.cpp                       |  12 +-
 .../MeditationTimer.hpp                       |   1 +
 .../ApplicationBellOnBoarding.cpp             |   2 +-
 .../ApplicationBellPowerNap.cpp               |  13 +-
 .../ApplicationBellPowerNap.hpp               |   1 +
 .../ApplicationBellRelaxation.cpp             |  23 +-
 .../ApplicationBellRelaxation.hpp             |   1 +
 .../HomeScreenLayoutClassicWithDate.cpp       |   3 +-
 .../common/src/layouts/HomeScreenLayouts.cpp  |   3 +-
 .../migration/settings_bell/0/up.sql          |   2 +-
 third-party/freeRTOS-kernel/freeRTOS-kernel   |   2 +-
 53 files changed, 825 insertions(+), 256 deletions(-)
 create mode 100644 module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.c
 create mode 100644 module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.h
 create mode 100644 module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.cpp
 create mode 100644 module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.hpp
 create mode 100644 module-bsp/board/rt1051/os/include/fsl_runtimestat_gpt.h
 create mode 100644 module-sys/common/include/system/messages/BlockWfiMode.hpp

diff --git a/board/rt1051/ldscripts/sections.ld b/board/rt1051/ldscripts/sections.ld
index d8eb843a4aa282ca04f1f056777d23c8b5d14030..4dd52ebecb50841d1015873d1efc589243443786 100644
--- a/board/rt1051/ldscripts/sections.ld
+++ b/board/rt1051/ldscripts/sections.ld
@@ -17,6 +17,8 @@ __ocram_noncached_end = ORIGIN(SRAM_OC) + LENGTH(SRAM_OC);
 __dtcm_ram_start = ORIGIN(SRAM_DTC);
 __dtcm_ram_end = ORIGIN(SRAM_DTC) + LENGTH(SRAM_DTC);
 
+__sdram_start = ORIGIN(BOARD_SDRAM_TEXT);
+
 __sdram_cached_start = ORIGIN(BOARD_SDRAM_HEAP);
 __sdram_cached_end = ORIGIN(BOARD_SDRAM_HEAP) + LENGTH(BOARD_SDRAM_HEAP);
 
@@ -49,7 +51,7 @@ SECTIONS
     .text : ALIGN(4)
     {
         FILL(0x00)
-        __vectors_start__ = ABSOLUTE(.) ;
+        __vectors_start__ = ABSOLUTE(.);
         KEEP(*(.isr_vector))
         /* Global Section Table */
         . = ALIGN(4) ;
@@ -64,6 +66,11 @@ SECTIONS
         LONG(    ADDR(.data));
         LONG(  SIZEOF(.data));
         __data_section_table_end = .;
+        __ocramtext_section_table = .;
+        LONG(LOADADDR(.ocramtext));
+        LONG(ADDR(.ocramtext));
+        LONG(SIZEOF(.ocramtext));
+        __ocramtext_section_table_end = .;
         __bss_section_table = .;
         /* System bss */
         LONG(    ADDR(.sysbss));
@@ -105,10 +112,18 @@ SECTIONS
         *libusb_stack.a:*(.data*)
     } > SRAM_DTC AT > BOARD_SDRAM_TEXT
 
+    /* Place WFI and SDRAM driver code in OCRAM to enable switching SDRAM to self-refresh before executing WFI */
+    .ocramtext : ALIGN(4)
+    {
+        *(.wficode*)
+        *fsl_semc.c.obj(.text*)
+    } > SRAM_OC AT > BOARD_SDRAM_TEXT
+
     /* MAIN TEXT SECTION */
     .text : ALIGN(4)
     {
-        *(.text*)
+        /* Place here all .text* sections except for the ones in fsl_semc.c.obj file */
+        *(EXCLUDE_FILE (*fsl_semc.c.obj) .text*)
         *(.rodata .rodata.* .constdata .constdata.*)
         . = ALIGN(4);
 
@@ -191,13 +206,6 @@ SECTIONS
         *(.sdram.*)
     } >BOARD_SDRAM_HEAP
 
-    /* DMA buffers */
-    .intramnoncacheable (NOLOAD) : ALIGN(4)
-    {
-        *(.intramnoncacheable)
-        *(.intramnoncacheable.*)
-    } > SRAM_OC
-
     /* Place system stack at the very end of DTCM */
     _StackSize = 0x1000;
     .stack ORIGIN(SRAM_DTC) + LENGTH(SRAM_DTC) - _StackSize - 0 : ALIGN(4)
diff --git a/harmony_changelog.md b/harmony_changelog.md
index 688c6fe8ab9f48804b59554bf230ba375611311e..e7a51da6a98b473db66fc1f3e29cd7ce71053a04 100644
--- a/harmony_changelog.md
+++ b/harmony_changelog.md
@@ -9,6 +9,8 @@
 * Added setting onboarding year to build date year
 * Added low battery notification on the home screen
 * Added low battery notification before using the application
+* Added entering WFI when CPU is idle to reduce power consumption
+* Added switching SDRAM to self-refresh before entering WFI for further power consumption reduction
 
 ### Changed / Improved
 
@@ -17,7 +19,6 @@
 ### Added
 * Added gradual alarm volume increase
 * Added progress bar for all volume control windows
-* Improved factory reset procedure to remove user files
 
 ### Changed / Improved
 * Increased clock font in Relaxation, Meditation, Power nap mode
@@ -26,6 +27,7 @@
 * Extended volume scale from 10 to 15 point scale
 * Modified volume control characteristic for better user experience
 * Reduced power consumption for Meditation and Power Nap applications
+* Improved factory reset procedure to remove user files
 
 ## [2.3.0 2023-12-20]
 
diff --git a/module-apps/application-settings/windows/advanced/CPUModeTestWindow.cpp b/module-apps/application-settings/windows/advanced/CPUModeTestWindow.cpp
index 15e9bc22b91c0f3c60b9da46a639e9b7aa8153d8..2a4150a803f4936d39da56003514190da1f87f9d 100644
--- a/module-apps/application-settings/windows/advanced/CPUModeTestWindow.cpp
+++ b/module-apps/application-settings/windows/advanced/CPUModeTestWindow.cpp
@@ -37,7 +37,7 @@ namespace gui
         });
 
         auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(cpuModeTester);
-        application->bus.sendUnicastSync(sentinelRegistrationMsg, service::name::system_manager, 30);
+        application->bus.sendUnicastSync(std::move(sentinelRegistrationMsg), service::name::system_manager, 30);
 
         AppWindow::buildInterface();
 
diff --git a/module-bsp/board/linux/lpm/LinuxLPM.cpp b/module-bsp/board/linux/lpm/LinuxLPM.cpp
index 046cec64ae62c3a3cef9bc34f445e82a34fde612..d1f2dd7ee0a4e7001869748e5aab288ef71d4c7b 100644
--- a/module-bsp/board/linux/lpm/LinuxLPM.cpp
+++ b/module-bsp/board/linux/lpm/LinuxLPM.cpp
@@ -33,4 +33,27 @@ namespace bsp
 
     void LinuxLPM::DisableDcdcPowerSaveMode()
     {}
+
+    void LinuxLPM::AllowEnteringWfiMode()
+    {}
+
+    void LinuxLPM::BlockEnteringWfiMode()
+    {}
+
+    std::uint32_t LinuxLPM::EnterWfiModeIfAllowed()
+    {
+        return 0;
+    }
+
+    std::uint32_t LinuxLPM::GetLastTimeSpentInWfi()
+    {
+        return 0;
+    }
+
+    void LinuxLPM::DisableSysTick()
+    {}
+
+    void LinuxLPM::EnableSysTick()
+    {}
+
 } // namespace bsp
diff --git a/module-bsp/board/linux/lpm/LinuxLPM.h b/module-bsp/board/linux/lpm/LinuxLPM.h
index 57238788b5a830fb086d01aee348e5b9e8a5fff4..08b3e6b0d8bea1a60bccba14782403399901cee4 100644
--- a/module-bsp/board/linux/lpm/LinuxLPM.h
+++ b/module-bsp/board/linux/lpm/LinuxLPM.h
@@ -18,5 +18,13 @@ namespace bsp
 
         void EnableDcdcPowerSaveMode() final;
         void DisableDcdcPowerSaveMode() final;
+
+        void AllowEnteringWfiMode() final;
+        void BlockEnteringWfiMode() final;
+        std::uint32_t EnterWfiModeIfAllowed() final;
+        std::uint32_t GetLastTimeSpentInWfi() final;
+
+        void DisableSysTick() final;
+        void EnableSysTick() final;
     };
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bellpx/CMakeLists.txt b/module-bsp/board/rt1051/bellpx/CMakeLists.txt
index ad9c623469e662c3a43bff7ffba67f86bbd6f66c..86b3aee773e37dffb1f9af407f9ee81e96264e25 100644
--- a/module-bsp/board/rt1051/bellpx/CMakeLists.txt
+++ b/module-bsp/board/rt1051/bellpx/CMakeLists.txt
@@ -20,6 +20,8 @@ target_sources(
         bsp/eink/eink_gpio.cpp
         bsp/lpm/PowerProfile.cpp
         bsp/lpm/RT1051LPM.cpp
+        bsp/lpm/WfiController.cpp
+        bsp/lpm/EnterSleepMode.c
         bsp/rotary_encoder/rotary_encoder.cpp
         bsp/rtc/rtc_configuration.cpp
         bsp/switches/switches.cpp
diff --git a/module-bsp/board/rt1051/bellpx/board.cpp b/module-bsp/board/rt1051/bellpx/board.cpp
index 788f5f67b866329c4637075d94a9e40542f9aaa9..0deb776c2b7b9340bc076fe223130652d092b5c1 100644
--- a/module-bsp/board/rt1051/bellpx/board.cpp
+++ b/module-bsp/board/rt1051/bellpx/board.cpp
@@ -51,18 +51,30 @@ namespace
 
 namespace bsp
 {
-    void board_exit(rebootState state)
+    void board_configure()
+    {
+        /* See *
+         * https://patchwork.kernel.org/project/linux-arm-kernel/patch/1471885400-9140-1-git-send-email-Anson.Huang@nxp.com/
+         */
+        CCM->CGPR |= CCM_CGPR_INT_MEM_CLK_LPM_MASK;
+
+        /* ERR050143 */
+        IOMUXC_GPR->GPR1 |= IOMUXC_GPR_GPR1_GINT_MASK;
+    }
+
+    void board_exit(RebootState state)
     {
         switch (state) {
-        case rebootState::none:
+        case RebootState::None:
             break;
-        case rebootState::poweroff:
+        case RebootState::Poweroff:
             power_off();
             break;
-        case rebootState::reboot:
+        case RebootState::Reboot:
             reset();
             break;
         }
+
         while (true) {}
     }
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bellpx/brownout.cpp b/module-bsp/board/rt1051/bellpx/brownout.cpp
index ca74cca8ce8bcab88c48b2c2de86b8c0717b9713..9503cd080d541fb6dd8ad1ef48a8b7b8045ef0cb 100644
--- a/module-bsp/board/rt1051/bellpx/brownout.cpp
+++ b/module-bsp/board/rt1051/bellpx/brownout.cpp
@@ -1,7 +1,6 @@
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
-#include <cstdint>
 #include <fsl_common.h>
 #include <fsl_pmu.h>
 
@@ -9,25 +8,15 @@ namespace bsp
 {
     namespace
     {
-        constexpr std::uint32_t OutputVoltage1P1 = 0x19; // 1.275V
-        constexpr std::uint32_t OutputVoltage2P5 = 0x1F; // 2.875V
-
-        constexpr std::uint32_t OffsetVoltage1P1 = 0x05; // 5*25mv
-        constexpr std::uint32_t OffsetVoltage2P5 = 0x03; // 3*25mv
-    }                                                    // namespace
+        constexpr auto outputVoltage2P5 = 0x1BU; // 2.775V
+        constexpr auto offsetVoltage2P5 = 0x03U; // 3*25mV
+    }                                            // namespace
 
     void Brownout_init()
     {
-        // Config LDO Regulatorsand config Brownout voltage offsets
-        PMU_1P1EnableBrownout(PMU, true);
-        PMU_1P1SetRegulatorOutputVoltage(PMU, OutputVoltage1P1);
-        PMU_1P1SetBrownoutOffsetVoltage(PMU, OffsetVoltage1P1);
-        PMU_1P1EnableOutput(PMU, true);
-
         PMU_2P5nableBrownout(PMU, true);
-        PMU_2P5SetRegulatorOutputVoltage(PMU, OutputVoltage2P5);
-        PMU_2P5SetBrownoutOffsetVoltage(PMU, OffsetVoltage2P5);
+        PMU_2P5SetRegulatorOutputVoltage(PMU, outputVoltage2P5);
+        PMU_2P5SetBrownoutOffsetVoltage(PMU, offsetVoltage2P5);
         PMU_2P5EnableOutput(PMU, true);
     }
-
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.c b/module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.c
new file mode 100644
index 0000000000000000000000000000000000000000..0bb25e71f80e9d5b486a2be07a17c8578b762ec7
--- /dev/null
+++ b/module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.c
@@ -0,0 +1,29 @@
+// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "EnterSleepMode.h"
+#include <macros.h>
+#include <fsl_semc.h>
+
+extern char __sdram_start; // Defined in linker script
+
+/* This function needs to be compiled either as a C code, or as a C++ code with
+ * exception handling disabled (-fno-exceptions), otherwise it is not possible to relocate
+ * its code into OCRAM. Rationale:
+ * https://stackoverflow.com/questions/52637962/arm-cortex-m7-long-branch-get-error-relocation-truncated-to-fit-r-arm-prel31
+ *
+ * As compiling single C++ file with exception handling disabled seems to be impossible
+ * using CMake and selectively disabling exceptions using #pragmas looks ugly, I've decided
+ * to just put it in a separate .c file. */
+WFI_CODE_SECTION(void enterSleepMode(void))
+{
+    /* Wait until SDRAM operations done and switch to self-refresh mode */
+    __DSB();
+    while ((SEMC->STS0 & SEMC_STS0_IDLE_MASK) == 0) {} // Wait until SEMC idle
+    __ISB();
+    SEMC_SendIPCommand(SEMC, kSEMC_MemType_SDRAM, (uint32_t)&__sdram_start, kSEMC_SDRAMCM_SelfRefresh, 0, NULL);
+
+    /* Go to sleep */
+    __WFI();
+    __ISB();
+}
diff --git a/module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.h b/module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.h
new file mode 100644
index 0000000000000000000000000000000000000000..36a259ca09a7589af9edb10e6a200422c628737d
--- /dev/null
+++ b/module-bsp/board/rt1051/bellpx/bsp/lpm/EnterSleepMode.h
@@ -0,0 +1,13 @@
+// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+    void enterSleepMode(void);
+#ifdef __cplusplus
+}
+#endif
diff --git a/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.cpp b/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.cpp
index 912b7a531366269fd764f2a8b54e33221abff17b..bae6bb078c84b060ac760f86fe56b092df4c40d4 100644
--- a/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.cpp
+++ b/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.cpp
@@ -2,6 +2,7 @@
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "RT1051LPM.hpp"
+#include "WfiController.hpp"
 
 namespace bsp
 {
@@ -10,4 +11,24 @@ namespace bsp
 
     void RT1051LPM::DisableDcdcPowerSaveMode()
     {}
+
+    void RT1051LPM::AllowEnteringWfiMode()
+    {
+        allowEnteringWfiMode();
+    }
+
+    void RT1051LPM::BlockEnteringWfiMode()
+    {
+        blockEnteringWfiMode();
+    }
+
+    std::uint32_t RT1051LPM::EnterWfiModeIfAllowed()
+    {
+        return enterWfiModeIfAllowed();
+    }
+
+    std::uint32_t RT1051LPM::GetLastTimeSpentInWfi()
+    {
+        return getLastTimeSpentInWfi();
+    }
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.hpp b/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.hpp
index c565f42be6fc8db9a46772108e68ed6fcfc9e55f..af06bd3b92eef6fb83d2376ccb9b7e4aa364fc2f 100644
--- a/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.hpp
+++ b/module-bsp/board/rt1051/bellpx/bsp/lpm/RT1051LPM.hpp
@@ -12,5 +12,10 @@ namespace bsp
       public:
         void EnableDcdcPowerSaveMode() final;
         void DisableDcdcPowerSaveMode() final;
+
+        void AllowEnteringWfiMode() final;
+        void BlockEnteringWfiMode() final;
+        std::uint32_t EnterWfiModeIfAllowed() final;
+        std::uint32_t GetLastTimeSpentInWfi() final;
     };
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.cpp b/module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..22ca190f216ef15e3f183cf6b297c413786ede13
--- /dev/null
+++ b/module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.cpp
@@ -0,0 +1,175 @@
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "WfiController.hpp"
+#include "EnterSleepMode.h"
+#include <fsl_gpc.h>
+#include <fsl_rtwdog.h>
+#include <fsl_runtimestat_gpt.h>
+#include <Utils.hpp>
+#include <time/time_constants.hpp>
+#include <ticks.hpp>
+#include <timers.h>
+
+namespace bsp
+{
+    namespace
+    {
+        /* RTC wakes up CPU every minute, so go to sleep only if next timer will
+         * trigger after more than minute - this way no event will ever be missed */
+        constexpr auto timersInactivityTimeMs{60 * utils::time::milisecondsInSecond};
+
+        bool wfiModeAllowed = false;
+        std::uint32_t timeSpentInWFI;
+
+        bool isTimerTaskScheduledSoon()
+        {
+            const auto currentTick          = cpp_freertos::Ticks::GetTicks();
+            const auto timersNextWakeUpTick = xTimerGetNextWakeUpTime();
+
+            if (timersNextWakeUpTick > currentTick) {
+                return (cpp_freertos::Ticks::TicksToMs(timersNextWakeUpTick - currentTick) < timersInactivityTimeMs);
+            }
+            return true;
+        }
+
+        bool isWfiModeAllowed()
+        {
+            return wfiModeAllowed;
+        }
+
+        void peripheralEnterDozeMode()
+        {
+            IOMUXC_GPR->GPR8 = IOMUXC_GPR_GPR8_LPI2C1_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPI2C2_IPG_DOZE_MASK |
+                               IOMUXC_GPR_GPR8_LPI2C3_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPI2C4_IPG_DOZE_MASK |
+                               IOMUXC_GPR_GPR8_LPSPI1_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPSPI2_IPG_DOZE_MASK |
+                               IOMUXC_GPR_GPR8_LPSPI3_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPSPI4_IPG_DOZE_MASK |
+                               IOMUXC_GPR_GPR8_LPUART1_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPUART2_IPG_DOZE_MASK |
+                               IOMUXC_GPR_GPR8_LPUART3_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPUART4_IPG_DOZE_MASK |
+                               IOMUXC_GPR_GPR8_LPUART5_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPUART6_IPG_DOZE_MASK |
+                               IOMUXC_GPR_GPR8_LPUART7_IPG_DOZE_MASK | IOMUXC_GPR_GPR8_LPUART8_IPG_DOZE_MASK;
+
+            IOMUXC_GPR->GPR12 = IOMUXC_GPR_GPR12_FLEXIO1_IPG_DOZE_MASK | IOMUXC_GPR_GPR12_FLEXIO2_IPG_DOZE_MASK;
+        }
+
+        void peripheralExitDozeMode()
+        {
+            IOMUXC_GPR->GPR8  = 0x00000000;
+            IOMUXC_GPR->GPR12 = 0x00000000;
+        }
+
+        void setRunModeConfig()
+        {
+            CCM->CLPCR &= ~(CCM_CLPCR_LPM_MASK | CCM_CLPCR_ARM_CLK_DIS_ON_LPM_MASK);
+        }
+
+        void setWaitModeConfig()
+        {
+            std::uint32_t clpcr;
+
+            /*
+             * ERR050143: CCM: When improper low-power sequence is used,
+             * the SoC enters low power mode before the ARM core executes WFI.
+             *
+             * Software workaround:
+             * 1) Software should trigger IRQ #41 (GPR_IRQ) to be always pending
+             *      by setting IOMUXC_GPR_GPR1_GINT.
+             * 2) Software should then unmask IRQ #41 in GPC before setting CCM
+             *      Low-Power mode.
+             * 3) Software should mask IRQ #41 right after CCM Low-Power mode
+             *      is set (set bits 0-1 of CCM_CLPCR).
+             */
+            GPC_EnableIRQ(GPC, GPR_IRQ_IRQn);
+            clpcr      = CCM->CLPCR & (~(CCM_CLPCR_LPM_MASK | CCM_CLPCR_ARM_CLK_DIS_ON_LPM_MASK));
+            CCM->CLPCR = clpcr | CCM_CLPCR_LPM(kCLOCK_ModeWait) | CCM_CLPCR_MASK_SCU_IDLE_MASK |
+                         CCM_CLPCR_MASK_L2CC_IDLE_MASK | CCM_CLPCR_ARM_CLK_DIS_ON_LPM_MASK | CCM_CLPCR_STBY_COUNT_MASK |
+                         CCM_CLPCR_BYPASS_LPM_HS0_MASK | CCM_CLPCR_BYPASS_LPM_HS1_MASK;
+            GPC_DisableIRQ(GPC, GPR_IRQ_IRQn);
+        }
+
+        bool isBrownoutOn2P5Output()
+        {
+            return ((PMU->REG_2P5 & PMU_REG_2P5_BO_VDD2P5_MASK) != 0);
+        }
+
+        void disableSystick()
+        {
+            SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk;
+            NVIC_ClearPendingIRQ(SysTick_IRQn);
+        }
+
+        void enableSystick()
+        {
+            SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;
+        }
+    } // namespace
+
+    void allowEnteringWfiMode()
+    {
+        wfiModeAllowed = true;
+    }
+
+    /* Block WFI mode so that OS wakes up fully and goes to sleep only after
+     * frequency has dropped back to the lowest level */
+    void blockEnteringWfiMode()
+    {
+        wfiModeAllowed = false;
+    }
+
+    std::uint32_t getLastTimeSpentInWfi()
+    {
+        return timeSpentInWFI;
+    }
+
+    std::uint32_t enterWfiModeIfAllowed()
+    {
+        if (!isWfiModeAllowed()) {
+            return 0;
+        }
+        timeSpentInWFI = 0;
+        if (isTimerTaskScheduledSoon()) {
+            blockEnteringWfiMode();
+            return 0;
+        }
+        if (isBrownoutOn2P5Output()) {
+            LOG_WARN("WFI disabled - brownout detected on 2P5 VDD output");
+            blockEnteringWfiMode();
+            return 0;
+        }
+
+        RTWDOG_Refresh(RTWDOG);
+        setWaitModeConfig();
+        peripheralEnterDozeMode();
+
+        disableSystick();
+        const auto enterWfiTicks = ulHighFrequencyTimerTicks();
+
+        const auto savedPrimask = DisableGlobalIRQ();
+        __DSB();
+        __ISB();
+
+        /* Clear the SLEEPDEEP bit to go into sleep mode (WAIT) */
+        SCB->SCR &= ~SCB_SCR_SLEEPDEEP_Msk;
+
+        /* Switch SDRAM to self-refresh and execute WFI */
+        enterSleepMode();
+
+        __NOP();
+        __NOP();
+        __NOP();
+        __NOP();
+
+        const auto exitWfiTicks = ulHighFrequencyTimerTicks();
+        enableSystick();
+
+        peripheralExitDozeMode();
+        RTWDOG_Refresh(RTWDOG);
+        EnableGlobalIRQ(savedPrimask);
+
+        blockEnteringWfiMode();
+        setRunModeConfig();
+
+        timeSpentInWFI = ulHighFrequencyTimerTicksToMs(utils::computeIncrease(exitWfiTicks, enterWfiTicks));
+        return timeSpentInWFI;
+    }
+} // namespace bsp
diff --git a/module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.hpp b/module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a36e9124d82011a3d9a41393497c2a15b852d827
--- /dev/null
+++ b/module-bsp/board/rt1051/bellpx/bsp/lpm/WfiController.hpp
@@ -0,0 +1,14 @@
+// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+
+#include <cstdint>
+
+namespace bsp
+{
+    void allowEnteringWfiMode();
+    void blockEnteringWfiMode();
+    std::uint32_t enterWfiModeIfAllowed();
+    std::uint32_t getLastTimeSpentInWfi();
+} // namespace bsp
diff --git a/module-bsp/board/rt1051/bellpx/bsp/switches/switches.cpp b/module-bsp/board/rt1051/bellpx/bsp/switches/switches.cpp
index 64ecda469d651190eb1a4361c6c7628ebb5a5be9..46afba5919f34d7ae5cc6d44cb83b584146b7adc 100644
--- a/module-bsp/board/rt1051/bellpx/bsp/switches/switches.cpp
+++ b/module-bsp/board/rt1051/bellpx/bsp/switches/switches.cpp
@@ -10,14 +10,10 @@
 #include <timers.h>
 #include <bsp/switches/switches.hpp>
 #include <board/BoardDefinitions.hpp>
-#include <board.h>
 #include <fsl_common.h>
 #include <switches/LatchState.hpp>
 
 #include <chrono>
-#include <stdio.h>
-#include <stdint.h>
-#include <assert.h>
 #include <map>
 #include <vector>
 #include <magic_enum.hpp>
@@ -56,7 +52,7 @@ namespace bsp::bell_switches
         void createTimer(TimerCallback callback, const std::chrono::milliseconds timeout)
         {
             timer =
-                xTimerCreate(magic_enum::enum_name(id).data(), pdMS_TO_TICKS(timeout.count()), false, this, callback);
+                xTimerCreate(magic_enum::enum_name(id).data(), pdMS_TO_TICKS(timeout.count()), pdFALSE, this, callback);
         }
     };
 
diff --git a/module-bsp/board/rt1051/bellpx/clock_config.cpp b/module-bsp/board/rt1051/bellpx/clock_config.cpp
index ca697ad4a225ee313b25124fde31a0bc5fbbd060..c5e60367841fb3b5161137b11bad5324dfc95060 100644
--- a/module-bsp/board/rt1051/bellpx/clock_config.cpp
+++ b/module-bsp/board/rt1051/bellpx/clock_config.cpp
@@ -453,6 +453,9 @@ void BOARD_BootClockRUN(void)
     /* PRE_PERIPH_CLK <- PLL1/2 = 432MHz */
     CLOCK_SetMux(kCLOCK_PeriphMux, 0); // CBCDR  (25) 0 - pre_periph_clk_sel, 1 - periph_clk2_clk_divided
 
+    /* Enable clock of ARM platform memories when entering LPM */
+    CCM->CGPR |= CCM_CGPR_INT_MEM_CLK_LPM_MASK;
+
     /* Set SystemCoreClock variable. */
     SystemCoreClockUpdate();
 }
diff --git a/module-bsp/board/rt1051/bellpx/irq_gpio.cpp b/module-bsp/board/rt1051/bellpx/irq_gpio.cpp
index b7dde338a32be533869dc0d286f26beae7ca0a59..fc521c568d9b123f491b25309b2292625d8d4026 100644
--- a/module-bsp/board/rt1051/bellpx/irq_gpio.cpp
+++ b/module-bsp/board/rt1051/bellpx/irq_gpio.cpp
@@ -236,10 +236,6 @@ namespace bsp
              * immediately reset board's main DCDC converter
              * using the WDOG_B pin. This will reset the
              * whole board. */
-            if (status & kPMU_1P1BrownoutOnOutput) {
-                WDOG1->WCR &= ~WDOG_WCR_WDA_MASK;
-            }
-
             if (status & kPMU_2P5BrownoutOnOutput) {
                 WDOG1->WCR &= ~WDOG_WCR_WDA_MASK;
             }
diff --git a/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.cpp b/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.cpp
index cd984168c257c7f95de1a19ac462d8708a3f2d7b..4a567b380f755a13013edfea9c839bd149b3c947 100644
--- a/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.cpp
+++ b/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.cpp
@@ -1,8 +1,9 @@
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "RT1051LPMCommon.hpp"
 #include <log/log.hpp>
+#include <fsl_device_registers.h>
 #include <fsl_clock.h>
 #include <bsp/bsp.hpp>
 #include "Oscillator.hpp"
@@ -66,6 +67,9 @@ namespace bsp
     void RT1051LPMCommon::onChangeUp(CpuFrequencyMHz freq, CpuFrequencyMHz newFrequency)
     {
         if ((freq <= CpuFrequencyMHz::Level_1) && (newFrequency > CpuFrequencyMHz::Level_1)) {
+            /* Block entering WFI mode */
+            BlockEnteringWfiMode();
+
             /* Switch to external crystal oscillator */
             SwitchOscillatorSource(LowPowerMode::OscillatorSource::External);
 
@@ -86,6 +90,8 @@ namespace bsp
             if (driverSEMC) {
                 driverSEMC->SwitchToPeripheralClockSource();
             }
+            /* Allow entering WFI mode */
+            AllowEnteringWfiMode();
         }
     }
 
@@ -140,7 +146,7 @@ namespace bsp
             onChangeDown(freq);
         }
 
-        LOG_INFO("CPU frequency changed to %lu", CLOCK_GetFreq(kCLOCK_CpuClk));
+        LOG_INFO("%s", getFrequencyChangedLog().c_str());
         currentFrequency = freq;
     }
 
@@ -161,4 +167,23 @@ namespace bsp
             break;
         }
     }
+
+    void RT1051LPMCommon::DisableSysTick()
+    {
+        SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk;
+    }
+
+    void RT1051LPMCommon::EnableSysTick()
+    {
+        SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;
+    }
+
+    std::string RT1051LPMCommon::getFrequencyChangedLog()
+    {
+        auto logMsg = "CPU frequency changed to " + std::to_string(CLOCK_GetCpuClkFreq());
+        if (currentFrequency <= CpuFrequencyMHz::Level_1) {
+            logMsg += " (WFI time: " + std::to_string(GetLastTimeSpentInWfi()) + " ms)";
+        }
+        return logMsg;
+    }
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.hpp b/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.hpp
index 9241bf8ee0c3beb2084dfc7dfb5905098aab7954..482cac70ffdaac4f8d0b62e6e62e0597c5e641f9 100644
--- a/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.hpp
+++ b/module-bsp/board/rt1051/bsp/lpm/RT1051LPMCommon.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -18,10 +18,13 @@ namespace bsp
         void SetCpuFrequency(CpuFrequencyMHz freq) final;
         [[nodiscard]] std::uint32_t GetCpuFrequency() const noexcept final;
         void SwitchOscillatorSource(OscillatorSource source) final;
+        void DisableSysTick() final;
+        void EnableSysTick() final;
 
       private:
         void onChangeUp(CpuFrequencyMHz freq, CpuFrequencyMHz newFrequency);
         void onChangeDown(CpuFrequencyMHz freq);
+        std::string getFrequencyChangedLog();
 
         std::unique_ptr<bsp::CpuFreqLPM> CpuFreq;
         std::shared_ptr<drivers::DriverSEMC> driverSEMC;
diff --git a/module-bsp/board/rt1051/common/board.cpp b/module-bsp/board/rt1051/common/board.cpp
index 7ee24229a530b30ffb78c2072d295cd52edecd8a..f60d1dc37a60aa403c7f38076c4b5ff6d30b0ed6 100644
--- a/module-bsp/board/rt1051/common/board.cpp
+++ b/module-bsp/board/rt1051/common/board.cpp
@@ -1,12 +1,8 @@
-
 #include "bsp.hpp"
 #include "board.h"
-#include "fsl_gpio.h"
-#include <stdint.h>
 extern "C"
 {
 #include "fsl_common.h"
-#include "fsl_clock.h"
 #include "fsl_dcdc.h"
 #include "fsl_snvs_hp.h"
 #include "fsl_snvs_lp.h"
@@ -21,17 +17,18 @@ extern "C"
 #include <board/debug_console.hpp>
 
 #include <log/log.hpp>
+#include <magic_enum.hpp>
 
 extern std::uint32_t __sdram_cached_start[];
 
 extern "C"
 {
-    uint32_t boot_reason_get_raw()
+    std::uint32_t boot_reason_get_raw()
     {
         return SNVS->LPGPR[0];
     }
 
-    void boot_reason_set_raw(uint32_t raw)
+    void boot_reason_set_raw(std::uint32_t raw)
     {
         SNVS->LPGPR[0] = raw;
     }
@@ -41,124 +38,140 @@ namespace bsp
 {
     namespace
     {
-        volatile rebootState rebootProgress{rebootState::none};
-    } // namespace
-
-    /* MPU configuration. */
-    static void BOARD_ConfigMPU(void)
-    {
-        /* Disable I cache and D cache */
-        if (SCB_CCR_IC_Msk == (SCB_CCR_IC_Msk & SCB->CCR)) {
-            SCB_DisableICache();
-        }
-        if (SCB_CCR_DC_Msk == (SCB_CCR_DC_Msk & SCB->CCR)) {
-            SCB_DisableDCache();
+        volatile RebootState rebootProgress{RebootState::None};
+
+        struct PlatformExitObject
+        {
+            void (*func)();
+        };
+        unsigned short registeredObjectsCount      = 0;
+        constexpr auto maxRegisteredObjectsCount = 16U;
+
+        PlatformExitObject exitObjects[maxRegisteredObjectsCount];
+
+        void call_platform_exit_functions()
+        {
+            while (registeredObjectsCount > 0) {
+                exitObjects[--registeredObjectsCount].func();
+            }
         }
 
-        /* Disable MPU */
-        ARM_MPU_Disable();
-
-        /* MPU configure:
-         * Use ARM_MPU_RASR(DisableExec, AccessPermission, TypeExtField, IsShareable, IsCacheable, IsBufferable,
-         * SubRegionDisable, Size) API in core_cm7.h. param DisableExec       Instruction access (XN) disable
-         * bit,0=instruction fetches enabled, 1=instruction fetches disabled. param AccessPermission  Data access
-         * permissions, allows you to configure read/write access for User and Privileged mode. Use MACROS defined in
-         * core_cm7.h: ARM_MPU_AP_NONE/ARM_MPU_AP_PRIV/ARM_MPU_AP_URO/ARM_MPU_AP_FULL/ARM_MPU_AP_PRO/ARM_MPU_AP_RO
-         * Combine TypeExtField/IsShareable/IsCacheable/IsBufferable to configure MPU memory access attributes.
-         *  TypeExtField  IsShareable  IsCacheable  IsBufferable   Memory Attribtue    Shareability        Cache
-         *     0             x           0           0             Strongly Ordered    shareable
-         *     0             x           0           1              Device             shareable
-         *     0             0           1           0              Normal             not shareable   Outer and inner
-         * write through no write allocate 0             0           1           1              Normal             not
-         * shareable   Outer and inner write back no write allocate 0             1           1           0 Normal
-         * shareable       Outer and inner write through no write allocate 0             1           1           1
-         * Normal             shareable       Outer and inner write back no write allocate 1             0           0
-         * 0              Normal             not shareable   outer and inner noncache 1             1           0 0
-         * Normal             shareable       outer and inner noncache 1             0           1           1 Normal
-         * not shareable   outer and inner write back write/read acllocate 1             1           1           1
-         * Normal             shareable       outer and inner write back write/read acllocate 2             x 0 0 Device
-         * not shareable Above are normal use settings, if your want to see more details or want to config different
-         * inner/outter cache policy. please refer to Table 4-55 /4-56 in arm cortex-M7 generic user guide
-         * <dui0646b_cortex_m7_dgug.pdf> param SubRegionDisable  Sub-region disable field. 0=sub-region is enabled,
-         * 1=sub-region is disabled. param Size              Region size of the region to be configured. use
-         * ARM_MPU_REGION_SIZE_xxx MACRO in core_cm7.h.
-         */
-
-        /* Region 0 setting: Memory with Device type, not shareable, non-cacheable. */
-        MPU->RBAR = ARM_MPU_RBAR(0, 0xC0000000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_512MB);
-
-        /* Region 1 setting: Memory with Device type, not shareable,  non-cacheable. */
-        MPU->RBAR = ARM_MPU_RBAR(1, 0x80000000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_1GB);
-
-        /* Region 2 setting */
+        /* MPU configuration. */
+        void BOARD_ConfigMPU()
+        {
+            /* Disable I cache and D cache */
+            if (SCB_CCR_IC_Msk == (SCB_CCR_IC_Msk & SCB->CCR)) {
+                SCB_DisableICache();
+            }
+            if (SCB_CCR_DC_Msk == (SCB_CCR_DC_Msk & SCB->CCR)) {
+                SCB_DisableDCache();
+            }
+
+            /* Disable MPU */
+            ARM_MPU_Disable();
+
+            /* MPU configure:
+             * Use ARM_MPU_RASR(DisableExec, AccessPermission, TypeExtField, IsShareable, IsCacheable, IsBufferable,
+             * SubRegionDisable, Size) API in core_cm7.h. param DisableExec       Instruction access (XN) disable
+             * bit,0=instruction fetches enabled, 1=instruction fetches disabled. param AccessPermission  Data access
+             * permissions, allows you to configure read/write access for User and Privileged mode. Use MACROS defined in
+             * core_cm7.h: ARM_MPU_AP_NONE/ARM_MPU_AP_PRIV/ARM_MPU_AP_URO/ARM_MPU_AP_FULL/ARM_MPU_AP_PRO/ARM_MPU_AP_RO
+             * Combine TypeExtField/IsShareable/IsCacheable/IsBufferable to configure MPU memory access attributes.
+             *  TypeExtField  IsShareable  IsCacheable  IsBufferable   Memory Attribtue    Shareability        Cache
+             *     0             x           0           0             Strongly Ordered    shareable
+             *     0             x           0           1              Device             shareable
+             *     0             0           1           0              Normal             not shareable   Outer and inner
+             * write through no write allocate 0             0           1           1              Normal             not
+             * shareable   Outer and inner write back no write allocate 0             1           1           0 Normal
+             * shareable       Outer and inner write through no write allocate 0             1           1           1
+             * Normal             shareable       Outer and inner write back no write allocate 1             0           0
+             * 0              Normal             not shareable   outer and inner noncache 1             1           0 0
+             * Normal             shareable       outer and inner noncache 1             0           1           1 Normal
+             * not shareable   outer and inner write back write/read acllocate 1             1           1           1
+             * Normal             shareable       outer and inner write back write/read acllocate 2             x 0 0 Device
+             * not shareable Above are normal use settings, if your want to see more details or want to config different
+             * inner/outter cache policy. please refer to Table 4-55 /4-56 in arm cortex-M7 generic user guide
+             * <dui0646b_cortex_m7_dgug.pdf> param SubRegionDisable  Sub-region disable field. 0=sub-region is enabled,
+             * 1=sub-region is disabled. param Size              Region size of the region to be configured. use
+             * ARM_MPU_REGION_SIZE_xxx MACRO in core_cm7.h.
+             */
+
+            /* Region 0 setting: Memory with Device type, not shareable, non-cacheable. */
+            MPU->RBAR = ARM_MPU_RBAR(0, 0xC0000000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_512MB);
+
+            /* Region 1 setting: Memory with Device type, not shareable,  non-cacheable. */
+            MPU->RBAR = ARM_MPU_RBAR(1, 0x80000000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_1GB);
+
+            /* Region 2 setting */
 #if defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1)
-        /* Setting Memory with Normal type, not shareable, outer/inner write back. */
-        MPU->RBAR = ARM_MPU_RBAR(2, 0x60000000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_512MB);
+            /* Setting Memory with Normal type, not shareable, outer/inner write back. */
+            MPU->RBAR = ARM_MPU_RBAR(2, 0x60000000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_512MB);
 #else
-        /* Setting Memory with Device type, not shareable, non-cacheable. */
-        // TODO: MPU->RBAR = ARM_MPU_RBAR(2, 0x60000000U);
-        // TODO: MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_512MB);
+            /* Setting Memory with Device type, not shareable, non-cacheable. */
+            // TODO: MPU->RBAR = ARM_MPU_RBAR(2, 0x60000000U);
+            // TODO: MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_512MB);
 #endif
 
-        /* Region 3 setting: Memory with Device type, not shareable, non-cacheable. */
-        MPU->RBAR = ARM_MPU_RBAR(3, 0x00000000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_1GB);
+            /* Region 3 setting: Memory with Device type, not shareable, non-cacheable. */
+            MPU->RBAR = ARM_MPU_RBAR(3, 0x00000000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 2, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_1GB);
 
-        /* Region 4 setting: Memory with Normal type, not shareable, outer/inner write back */
-        MPU->RBAR = ARM_MPU_RBAR(4, 0x00000000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_128KB);
+            /* Region 4 setting: Memory with Normal type, not shareable, outer/inner write back */
+            MPU->RBAR = ARM_MPU_RBAR(4, 0x00000000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_128KB);
 
-        // CPU doesn't use cache when accessing TCM memories
-        /* Region 5 setting: Memory with Normal type, not shareable, outer/inner write back */
-        MPU->RBAR = ARM_MPU_RBAR(5, 0x20000000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_512KB);
+            // CPU doesn't use cache when accessing TCM memories
+            /* Region 5 setting: Memory with Normal type, not shareable, outer/inner write back */
+            MPU->RBAR = ARM_MPU_RBAR(5, 0x20000000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_512KB);
 
-        // OCRAM configured as non-cached segment
-        /* Region 6 setting: Memory with Normal type, not shareable, outer/inner write back */
-        MPU->RBAR = ARM_MPU_RBAR(6, 0x20200000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 1, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_64KB);
+            // OCRAM configured as non-cached segment
+            /* Region 6 setting: Memory with Normal type, not shareable, outer/inner write back */
+            MPU->RBAR = ARM_MPU_RBAR(6, 0x20200000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 1, 0, 0, 0, 0, ARM_MPU_REGION_SIZE_64KB);
 
-        /* Region 7 setting: Memory with Normal type, not shareable, outer/inner write back
-         * BOARD_SDRAM_TEXT
-         */
-        MPU->RBAR = ARM_MPU_RBAR(7, 0x80000000U);
+            /* Region 7 setting: Memory with Normal type, not shareable, outer/inner write back
+             * BOARD_SDRAM_TEXT
+             */
+            MPU->RBAR = ARM_MPU_RBAR(7, 0x80000000U);
 #if defined(HW_SDRAM_64_MB) && (HW_SDRAM_64_MB == 1)
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_RO, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_64MB);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_RO, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_64MB);
 #else
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_RO, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_16MB);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_RO, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_16MB);
 #endif
 
-        /* The define sets the cacheable memory to shareable,
-         * this suggestion is referred from chapter 2.2.1 Memory regions,
-         * types and attributes in Cortex-M7 Devices, Generic User Guide */
+            /* The define sets the cacheable memory to shareable,
+             * this suggestion is referred from chapter 2.2.1 Memory regions,
+             * types and attributes in Cortex-M7 Devices, Generic User Guide */
 #if defined(SDRAM_IS_SHAREABLE)
-        /* Region 7 setting: Memory with Normal type, not shareable, outer/inner write back */
-        MPU->RBAR = ARM_MPU_RBAR(8, 0x80000000U);
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 1, 1, 1, 0, ARM_MPU_REGION_SIZE_32MB);
+            /* Region 7 setting: Memory with Normal type, not shareable, outer/inner write back */
+            MPU->RBAR = ARM_MPU_RBAR(8, 0x80000000U);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 1, 1, 1, 0, ARM_MPU_REGION_SIZE_32MB);
 #else
-        /* Region 9 setting: Memory with Normal type, not shareable, outer/inner write back
-         * BOARD_SDRAM_HEAP
-         */
-        MPU->RBAR = ARM_MPU_RBAR(9, reinterpret_cast<std::uintptr_t>(__sdram_cached_start));
+            /* Region 9 setting: Memory with Normal type, not shareable, outer/inner write back
+             * BOARD_SDRAM_HEAP
+             */
+            MPU->RBAR = ARM_MPU_RBAR(9, reinterpret_cast<std::uintptr_t>(__sdram_cached_start));
 #if defined(HW_SDRAM_64_MB) && (HW_SDRAM_64_MB == 1)
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_64MB);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_64MB);
 #else
-        MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_16MB);
+            MPU->RASR = ARM_MPU_RASR(0, ARM_MPU_AP_FULL, 0, 0, 1, 1, 0, ARM_MPU_REGION_SIZE_16MB);
 #endif // HW_SDRAM_64_MB
 #endif // SDRAM_IS_SHAREABLE
 
-        /* Enable MPU */
-        ARM_MPU_Enable(MPU_CTRL_PRIVDEFENA_Msk);
+            /* Enable MPU */
+            ARM_MPU_Enable(MPU_CTRL_PRIVDEFENA_Msk);
 
-        SCB->SHCSR &= ~(SCB_SHCSR_MEMFAULTENA_Msk | SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk);
+            SCB->SHCSR &= ~(SCB_SHCSR_MEMFAULTENA_Msk | SCB_SHCSR_USGFAULTENA_Msk | SCB_SHCSR_BUSFAULTENA_Msk);
 
-        /* Enable I cache and D cache */
-        SCB_EnableDCache();
-        SCB_EnableICache();
-    }
+            /* Enable I cache and D cache */
+            SCB_EnableDCache();
+            SCB_EnableICache();
+        }
+    } // namespace
 
     void board_init()
     {
@@ -182,6 +195,8 @@ namespace bsp
         PrintSystemClocks();
         clearAndPrintBootReason();
 
+        board_configure();
+
         if (SNVS->LPGPR[1] != 0) {
             LOG_INFO("Device seems to have been reset by RTWDOG! Last instruction address: 0x%08lX",
                      SNVS->LPGPR[1]);
@@ -189,49 +204,32 @@ namespace bsp
         }
     }
 
-    //! Board PowerOff function by cutdown power
+    //! Board PowerOff function by power cutoff
     void board_power_off()
     {
-        rebootProgress = rebootState::poweroff;
+        rebootProgress = RebootState::Poweroff;
     }
 
-    //! Board reboot by the SVNC code
+    //! Board reboot by the SNVS code
     void board_restart()
     {
-        rebootProgress = rebootState::reboot;
+        rebootProgress = RebootState::Reboot;
     }
 
-    struct PlatformExitObject
+    int register_exit_function(void (*func)())
     {
-        void (*func)();
-    };
-    static unsigned short iObject            = 0;
-    constexpr auto MAX_PLATFORM_EXIT_OBJECTS = 16u;
-
-    static PlatformExitObject objects[MAX_PLATFORM_EXIT_OBJECTS];
-
-    int register_exit_functions(void (*func)())
-    {
-        if (iObject >= sizeof(objects)) {
-            return -1;
+        if (registeredObjectsCount >= sizeof(exitObjects)) {
+            return -ENOMEM;
         }
 
-        objects[iObject].func = func;
-        ++iObject;
+        exitObjects[registeredObjectsCount].func = func;
+        ++registeredObjectsCount;
         return 0;
     }
 
-    void CallPlatformExitFunctions()
-    {
-        while (iObject > 0) {
-            objects[--iObject].func();
-        }
-    }
-
     extern "C" void _platform_exit(void)
     {
-        CallPlatformExitFunctions();
+        call_platform_exit_functions();
         bsp::board_exit(rebootProgress);
     }
-
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/common/startup_mimxrt1052.cpp b/module-bsp/board/rt1051/common/startup_mimxrt1052.cpp
index ff02b6642c4e5eb64b68fa260de6b585cf3a40bb..2a20606bf28116259da4fb04523a613d2c9c70c3 100644
--- a/module-bsp/board/rt1051/common/startup_mimxrt1052.cpp
+++ b/module-bsp/board/rt1051/common/startup_mimxrt1052.cpp
@@ -681,10 +681,10 @@ __attribute__((section(".after_vectors.init_bss"))) void bss_init(unsigned int s
 // contains the load address, execution address and length of each RW data
 // section and the execution and length of each BSS (zero initialized) section.
 //*****************************************************************************
-extern unsigned int __textram_section_table;
-extern unsigned int __textram_section_table_end;
 extern unsigned int __data_section_table;
 extern unsigned int __data_section_table_end;
+extern unsigned int __ocramtext_section_table;
+extern unsigned int __ocramtext_section_table_end;
 extern unsigned int __bss_section_table;
 extern unsigned int __bss_section_table_end;
 
@@ -706,7 +706,7 @@ __attribute__((section(".after_vectors.reset"))) void ResetISR(void)
     // Load base address of Global Section Table
     SectionTableAddr = &__data_section_table;
 
-    // Copy the data sections from flash to SRAM.
+    // Copy the data sections from flash to SRAM
     while (SectionTableAddr < &__data_section_table_end) {
         LoadAddr   = *SectionTableAddr++;
         ExeAddr    = *SectionTableAddr++;
@@ -716,6 +716,18 @@ __attribute__((section(".after_vectors.reset"))) void ResetISR(void)
         }
     }
 
+    // Load address of OCRAM text section entry in GST
+    SectionTableAddr = &__ocramtext_section_table;
+    // Copy code from SDRAM to OCRAM
+    while (SectionTableAddr < &__ocramtext_section_table_end) {
+        LoadAddr   = *SectionTableAddr++;
+        ExeAddr    = *SectionTableAddr++;
+        SectionLen = *SectionTableAddr++;
+        if (LoadAddr != ExeAddr) {
+            data_init(LoadAddr, ExeAddr, SectionLen);
+        }
+    }
+
     // Initialize BSS section
     SectionTableAddr = &__bss_section_table;
     // Zero fill the bss segment
diff --git a/module-bsp/board/rt1051/os/fsl_runtimestat_gpt.c b/module-bsp/board/rt1051/os/fsl_runtimestat_gpt.c
index 0a1f661fda19da22656eae729424190c79cdbc1c..1bc53cccf95f8f6629a09c9fa456d741811e761a 100644
--- a/module-bsp/board/rt1051/os/fsl_runtimestat_gpt.c
+++ b/module-bsp/board/rt1051/os/fsl_runtimestat_gpt.c
@@ -1,7 +1,11 @@
-// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
-#include "fsl_gpt.h"
+#include <fsl_runtimestat_gpt.h>
+#include <fsl_gpt.h>
+
+static const uint32_t millisecondsInSecond = 1000U;
+static const uint32_t runTimeStatsClockHz  = 10000U;
 
 static inline GPT_Type *vPortGetGptBase(void)
 {
@@ -31,7 +35,17 @@ void vConfigureTimerForRunTimeStats(void)
     GPT_StartTimer(pxGptBase);
 }
 
+uint32_t ulHighFrequencyTimerTicksToMs(uint32_t ticks)
+{
+    return (ticks / (runTimeStatsClockHz / millisecondsInSecond));
+}
+
 uint32_t ulHighFrequencyTimerTicks(void)
 {
     return GPT_GetCurrentTimerCount(vPortGetGptBase());
 }
+
+uint32_t ulHighFrequencyTimerMs(void)
+{
+    return ulHighFrequencyTimerTicksToMs(ulHighFrequencyTimerTicks());
+}
diff --git a/module-bsp/board/rt1051/os/include/fsl_runtimestat_gpt.h b/module-bsp/board/rt1051/os/include/fsl_runtimestat_gpt.h
new file mode 100644
index 0000000000000000000000000000000000000000..76f8676b040e74985b80fc92a427f2053b3a65aa
--- /dev/null
+++ b/module-bsp/board/rt1051/os/include/fsl_runtimestat_gpt.h
@@ -0,0 +1,19 @@
+// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#pragma once
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+    void vConfigureTimerForRunTimeStats(void);
+    uint32_t ulHighFrequencyTimerTicks(void);
+    uint32_t ulHighFrequencyTimerMs(void);
+    uint32_t ulHighFrequencyTimerTicksToMs(uint32_t ticks);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/module-bsp/board/rt1051/os/include/macros.h b/module-bsp/board/rt1051/os/include/macros.h
index c455b9fd8ae8e75c2e9ccb5e0feeef11eaab68b0..59f19a971073c13a8f6e0feb71a73c3383a9156e 100644
--- a/module-bsp/board/rt1051/os/include/macros.h
+++ b/module-bsp/board/rt1051/os/include/macros.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -17,11 +17,6 @@
 #define CACHEABLE_SECTION_SDRAM_ALIGN(var, alignbytes)                                                                 \
     __attribute__((section(".sdram"))) __attribute__((aligned(alignbytes))) var
 
-#define NONCACHEABLE_SECTION_DMA(var) __attribute__((section(".intramnoncacheable"))) var
-
-#define NONCACHEABLE_SECTION_DMA_ALIGN(var, alignbytes)                                                                \
-    __attribute__((section(".intramnoncacheable"))) var __attribute__((aligned(alignbytes)))
-
 #define NONCACHEABLE_SECTION_SDRAM(var) var
 
 #define NONCACHEABLE_SECTION_SDRAM_ALIGN(var, alignbytes) var __attribute__((aligned(alignbytes)))
@@ -36,6 +31,8 @@
 #define NONCACHEABLE_SECTION_ALIGN(var, alignbytes)                                                                    \
     __attribute__((section("NonCacheable,\"aw\",%nobits @"))) var __attribute__((aligned(alignbytes)))
 
+#define WFI_CODE_SECTION(var) __attribute__((section(".wficode"), noinline)) var
+
 static inline uint32_t IS_MEM_ADDR_CACHED(void *addr)
 {
     extern uint32_t __ocram_noncached_start[];
diff --git a/module-bsp/board/rt1051/puretx/board.cpp b/module-bsp/board/rt1051/puretx/board.cpp
index 0d534eb09afb803057eb714ff774b9f0b1e2c59a..1d1fd109785069b0b5e12d8cbc8200c201087217 100644
--- a/module-bsp/board/rt1051/puretx/board.cpp
+++ b/module-bsp/board/rt1051/puretx/board.cpp
@@ -15,7 +15,7 @@ namespace
         using namespace drivers;
         auto gpio_power = DriverGPIO::Create(static_cast<GPIOInstances>(BoardDefinitions::POWER_SWITCH_HOLD_GPIO),
                                              DriverGPIOParams{});
-        gpio_power->WritePin(static_cast<uint32_t>(BoardDefinitions::POWER_SWITCH_HOLD_BUTTON), 0);
+        gpio_power->WritePin(static_cast<std::uint32_t>(BoardDefinitions::POWER_SWITCH_HOLD_BUTTON), 0);
     }
 
     void board_reset()
@@ -26,15 +26,20 @@ namespace
 
 namespace bsp
 {
-    void board_exit(rebootState state)
+    void board_configure()
+    {
+        // no-op for Pure
+    }
+
+    void board_exit(RebootState state)
     {
         switch (state) {
-        case rebootState::none:
+        case RebootState::None:
             break;
-        case rebootState::poweroff:
+        case RebootState::Poweroff:
             board_shutdown();
             break;
-        case rebootState::reboot:
+        case RebootState::Reboot:
             board_reset();
             break;
         }
diff --git a/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.cpp b/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.cpp
index 635e48f8fba6174d72111b5c5fa3a992f6f0c4fc..41f36259ee6da6bbac1138d8db04576d9d071a4c 100644
--- a/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.cpp
+++ b/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.cpp
@@ -42,4 +42,21 @@ namespace bsp
     {
         gpio_2->WritePin(static_cast<std::uint32_t>(BoardDefinitions::DCDC_INVERTER_MODE_PIN), 1);
     }
+
+    void RT1051LPM::AllowEnteringWfiMode()
+    {}
+
+    void RT1051LPM::BlockEnteringWfiMode()
+    {}
+
+    std::uint32_t RT1051LPM::EnterWfiModeIfAllowed()
+    {
+        return 0;
+    }
+
+    std::uint32_t RT1051LPM::GetLastTimeSpentInWfi()
+    {
+        return 0;
+    }
+
 } // namespace bsp
diff --git a/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.hpp b/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.hpp
index 7088dc75598b0067c1f6bf992c43695bfca8a945..a1abe4498fb1c5fcf90eff658880cfaa9a1596ab 100644
--- a/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.hpp
+++ b/module-bsp/board/rt1051/puretx/bsp/lpm/RT1051LPM.hpp
@@ -14,6 +14,11 @@ namespace bsp
         void EnableDcdcPowerSaveMode() final;
         void DisableDcdcPowerSaveMode() final;
 
+        void AllowEnteringWfiMode() final;
+        void BlockEnteringWfiMode() final;
+        std::uint32_t EnterWfiModeIfAllowed() final;
+        std::uint32_t GetLastTimeSpentInWfi() final;
+
       private:
         std::shared_ptr<drivers::DriverGPIO> gpio_1;
         std::shared_ptr<drivers::DriverGPIO> gpio_2;
diff --git a/module-bsp/bsp/bsp.hpp b/module-bsp/bsp/bsp.hpp
index f24dd42e73316b055436c0a1732cf2acc810cc23..1f5390ba069c5e86545c3562fb3393a9ade4275d 100644
--- a/module-bsp/bsp/bsp.hpp
+++ b/module-bsp/bsp/bsp.hpp
@@ -4,22 +4,23 @@
 
 namespace bsp
 {
-    enum class rebootState : uintptr_t
+    enum class RebootState : std::uintptr_t
     {
-        none,
-        poweroff,
-        reboot
+        None,
+        Poweroff,
+        Reboot
     };
 
     void board_init();
-
     void board_power_off();
-
     void board_restart();
 
     /// Register platform exit functions
-    int register_exit_functions(void (*func)());
+    int register_exit_function(void (*func)());
+
+    /// Board-specific configuration procedure. Currently used to perform WFI-related config for Harmony.
+    void board_configure();
 
     /// Board-specific exit/reset procedure. It is the main exit point from the system.
-    [[noreturn]] void board_exit(rebootState);
+    [[noreturn]] void board_exit(RebootState state);
 } // namespace bsp
diff --git a/module-bsp/bsp/lpm/bsp_lpm.hpp b/module-bsp/bsp/lpm/bsp_lpm.hpp
index 0bb0bd0c130133467dcd233a2f680a3ee3a24f66..4c3a269baa001a9ceca73d057b7e8a51bde1d757 100644
--- a/module-bsp/bsp/lpm/bsp_lpm.hpp
+++ b/module-bsp/bsp/lpm/bsp_lpm.hpp
@@ -51,6 +51,13 @@ namespace bsp
         virtual void EnableDcdcPowerSaveMode()  = 0;
         virtual void DisableDcdcPowerSaveMode() = 0;
 
+        virtual void AllowEnteringWfiMode() = 0;
+        virtual void BlockEnteringWfiMode() = 0;
+        virtual std::uint32_t EnterWfiModeIfAllowed() = 0;
+        virtual std::uint32_t GetLastTimeSpentInWfi() = 0;
+
+        virtual void DisableSysTick() = 0;
+        virtual void EnableSysTick() = 0;
       protected:
         CpuFrequencyMHz currentFrequency = CpuFrequencyMHz::Level_6;
     };
diff --git a/module-platform/rt1051/src/RT1051Platform.cpp b/module-platform/rt1051/src/RT1051Platform.cpp
index 13137b83ae7d8006c36fcb2bff98ede8f6830f63..08afc8fd36e591708c40b7e0f7d00f920d83f78a 100644
--- a/module-platform/rt1051/src/RT1051Platform.cpp
+++ b/module-platform/rt1051/src/RT1051Platform.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2022, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include <platform/rt1051/RT1051Platform.hpp>
@@ -16,7 +16,7 @@ using platform::rt1051::RT1051Platform;
 RT1051Platform::RT1051Platform()
 {
     bsp::board_init();
-    bsp::register_exit_functions(Log::Logger::destroyInstance);
+    bsp::register_exit_function(Log::Logger::destroyInstance);
 }
 
 void RT1051Platform::init()
@@ -33,7 +33,7 @@ void RT1051Platform::initFilesystem()
     auto blockDeviceFactory = std::make_unique<BlockDeviceFactory>();
     vfs                     = purefs::subsystem::initialize(std::move(blockDeviceFactory));
 
-    if (int err = purefs::subsystem::mount_defaults(); err != 0) {
+    if (const auto err = purefs::subsystem::mount_defaults(); err != 0) {
         throw std::runtime_error("Failed to initiate filesystem: " + std::to_string(err));
     }
 
@@ -42,7 +42,7 @@ void RT1051Platform::initFilesystem()
 void platform::rt1051::RT1051Platform::deinit()
 {
     if (usesFilesystem) {
-        if (int err = purefs::subsystem::unmount_all(); err != 0) {
+        if (const auto err = purefs::subsystem::unmount_all(); err != 0) {
             throw std::runtime_error("Failed to unmount all: " + std::to_string(err));
         }
         usesFilesystem = false;
diff --git a/module-services/service-bluetooth/ServiceBluetooth.cpp b/module-services/service-bluetooth/ServiceBluetooth.cpp
index 0d9d7b68e876202a3ce650d380e640e30ea563f1..00fd496a58c2989d1cad33c70a962c0b238bec3d 100644
--- a/module-services/service-bluetooth/ServiceBluetooth.cpp
+++ b/module-services/service-bluetooth/ServiceBluetooth.cpp
@@ -75,7 +75,7 @@ sys::ReturnCodes ServiceBluetooth::InitHandler()
     cpuSentinel = std::make_shared<sys::CpuSentinel>(service::name::bluetooth, this);
 
     auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(cpuSentinel);
-    bus.sendUnicast(sentinelRegistrationMsg, service::name::system_manager);
+    bus.sendUnicast(std::move(sentinelRegistrationMsg), service::name::system_manager);
 
     connectionTimeoutTimer =
         sys::TimerFactory::createSingleShotTimer(this, "btTimeoutTimer", connectionTimeout, [this](sys::Timer &_) {
diff --git a/module-services/service-cellular/ServiceCellular.cpp b/module-services/service-cellular/ServiceCellular.cpp
index 0c934078e633e256892d04ed89495968c6851feb..3325c1147d2cf777632073c9f6aad82d74554177 100644
--- a/module-services/service-cellular/ServiceCellular.cpp
+++ b/module-services/service-cellular/ServiceCellular.cpp
@@ -276,7 +276,7 @@ sys::ReturnCodes ServiceCellular::InitHandler()
                                      cpuSentinel);
 
     auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(cpuSentinel);
-    bus.sendUnicast(sentinelRegistrationMsg, ::service::name::system_manager);
+    bus.sendUnicast(std::move(sentinelRegistrationMsg), ::service::name::system_manager);
 
     cmux->registerCellularDevice();
 
diff --git a/module-services/service-eink/ServiceEink.cpp b/module-services/service-eink/ServiceEink.cpp
index 7d16a4d683f082c6926ca528b6f8ecdb38a1d153..fd07b525979066613473606298748d45ddb382cb 100644
--- a/module-services/service-eink/ServiceEink.cpp
+++ b/module-services/service-eink/ServiceEink.cpp
@@ -95,11 +95,11 @@ namespace service::eink
         settings->init(service::ServiceProxy(shared_from_this()));
         initStaticData();
 
-        const auto deviceRegistrationMsg = std::make_shared<sys::DeviceRegistrationMessage>(display->getDevice());
-        bus.sendUnicast(deviceRegistrationMsg, service::name::system_manager);
+        auto deviceRegistrationMsg = std::make_shared<sys::DeviceRegistrationMessage>(display->getDevice());
+        bus.sendUnicast(std::move(deviceRegistrationMsg), service::name::system_manager);
 
-        const auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(eInkSentinel);
-        bus.sendUnicast(sentinelRegistrationMsg, service::name::system_manager);
+        auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(eInkSentinel);
+        bus.sendUnicast(std::move(sentinelRegistrationMsg), service::name::system_manager);
 
         eInkSentinel->HoldMinimumFrequency();
 
diff --git a/module-sys/SystemManager/CpuGovernor.cpp b/module-sys/SystemManager/CpuGovernor.cpp
index cb3fb56947cd59e1371f418350d3be07b61432ea..b52f9f99eafe055374dc8c3949ba67e36fdd82fa 100644
--- a/module-sys/SystemManager/CpuGovernor.cpp
+++ b/module-sys/SystemManager/CpuGovernor.cpp
@@ -68,6 +68,16 @@ namespace sys
         requestedFrequency = newFrequency;
     }
 
+    void GovernorSentinel::BlockWfiMode(bool request)
+    {
+        wfiBlocked = request;
+    }
+
+    [[nodiscard]] auto GovernorSentinel::IsWfiBlocked() const noexcept -> bool
+    {
+        return wfiBlocked;
+    }
+
     bool CpuGovernor::RegisterNewSentinel(std::shared_ptr<CpuSentinel> newSentinel)
     {
         if (newSentinel) {
@@ -149,6 +159,25 @@ namespace sys
         SetCpuFrequencyRequest(sentinelName, bsp::CpuFrequencyMHz::Level_0);
     }
 
+    void CpuGovernor::BlockWfiMode(const std::string &sentinelName, bool request)
+    {
+        auto isSentinelRecognized = false;
+        for (auto &sentinel : sentinels) {
+            auto sentinelWeakPointer = sentinel->GetSentinel();
+            if (!sentinelWeakPointer.expired()) {
+                std::shared_ptr<CpuSentinel> sharedResource = sentinelWeakPointer.lock();
+                if (sharedResource->GetName() == sentinelName) {
+                    sentinel->BlockWfiMode(request);
+                    isSentinelRecognized = true;
+                    break;
+                }
+            }
+        }
+        if (!isSentinelRecognized) {
+            LOG_WARN("Sentinel %s is not recognized!", sentinelName.c_str());
+        }
+    }
+
     [[nodiscard]] auto CpuGovernor::GetMinimumFrequencyRequested() noexcept -> sentinel::View
     {
         sentinel::View d;
@@ -174,6 +203,16 @@ namespace sys
         return d;
     }
 
+    [[nodiscard]] auto CpuGovernor::IsWfiBlocked() noexcept -> bool
+    {
+        for (auto &sentinel : sentinels) {
+            if (sentinel->IsWfiBlocked()) {
+                return true;
+            }
+        }
+        return false;
+    }
+
     void CpuGovernor::InformSentinelsAboutCpuFrequencyChange(bsp::CpuFrequencyMHz newFrequency) noexcept
     {
         sentinel_foo foo = [&newFrequency](const std::shared_ptr<CpuSentinel> &s) -> bool {
diff --git a/module-sys/SystemManager/CpuSentinel.cpp b/module-sys/SystemManager/CpuSentinel.cpp
index 27c214f1d286fd914742dd11ed00630661570f8b..e0430518bf7ab76288406944d2f56b1ca555d64b 100644
--- a/module-sys/SystemManager/CpuSentinel.cpp
+++ b/module-sys/SystemManager/CpuSentinel.cpp
@@ -4,6 +4,7 @@
 #include <SystemManager/CpuSentinel.hpp>
 #include "system/messages/RequestCpuFrequencyMessage.hpp"
 #include "system/messages/HoldCpuFrequency.hpp"
+#include "system/messages/BlockWfiMode.hpp"
 #include "system/Constants.hpp"
 #include <Timers/TimerFactory.hpp>
 #include <memory>
@@ -19,7 +20,7 @@ namespace sys
     CpuSentinel::CpuSentinel(std::string name,
                              sys::Service *service,
                              std::function<void(bsp::CpuFrequencyMHz)> callback)
-        : name(name), owner(service), callback(callback)
+        : name(std::move(name)), owner(service), callback(std::move(callback))
     {}
 
     [[nodiscard]] auto CpuSentinel::GetName() const noexcept -> std::string
@@ -36,7 +37,7 @@ namespace sys
             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));
+                            std::to_string(static_cast<int>(frequencyToHold));
             ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
         }
     }
@@ -51,6 +52,16 @@ namespace sys
         }
     }
 
+    void CpuSentinel::BlockWfiMode(bool block)
+    {
+        if (blockWfiMode != block) {
+            auto msg = std::make_shared<sys::BlockWfiModeMessage>(GetName(), block, xTaskGetCurrentTaskHandle());
+            owner->bus.sendUnicast(std::move(msg), service::name::system_manager);
+            blockWfiMode = block;
+            ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
+        }
+    }
+
     [[nodiscard]] auto CpuSentinel::GetFrequency() const noexcept -> bsp::CpuFrequencyMHz
     {
         return currentFrequency;
@@ -85,10 +96,11 @@ namespace sys
     }
 
     TimedCpuSentinel::TimedCpuSentinel(std::string name, sys::Service *service)
-        : CpuSentinel(name, service), timerHandle{sys::TimerFactory::createSingleShotTimer(
-                                          owner, "holdFrequencyTimer", defaultHoldFrequencyTime, [this](sys::Timer &) {
-                                              ReleaseMinimumFrequency();
-                                          })}
+        : CpuSentinel(std::move(name), service), timerHandle{sys::TimerFactory::createSingleShotTimer(
+                                                     owner,
+                                                     "holdFrequencyTimer",
+                                                     defaultHoldFrequencyTime,
+                                                     [this](sys::Timer &) { ReleaseMinimumFrequency(); })}
     {}
 
     TimedCpuSentinel::~TimedCpuSentinel()
diff --git a/module-sys/SystemManager/PowerManager.cpp b/module-sys/SystemManager/PowerManager.cpp
index 83ad0d401b3228ff31a85f04525d1223611edbb7..1bec5d52c6229efe020ef9ecb7e49826c26d7b39 100644
--- a/module-sys/SystemManager/PowerManager.cpp
+++ b/module-sys/SystemManager/PowerManager.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "SystemManager/cpu/algorithm/FrequencyHold.hpp"
@@ -12,6 +12,8 @@
 #include <log/log.hpp>
 #include <Logger.hpp>
 #include <Utils.hpp>
+#include <FreeRTOS.h>
+#include <ticks.hpp>
 
 namespace sys
 {
@@ -20,6 +22,7 @@ namespace sys
         constexpr auto lowestLevelName{"lowestCpuFrequency"};
         constexpr auto middleLevelName{"middleCpuFrequency"};
         constexpr auto highestLevelName{"highestCpuFrequency"};
+        constexpr auto WfiName{"WFI"};
 
         constexpr bsp::CpuFrequencyMHz logDumpFrequencyToHold{bsp::CpuFrequencyMHz::Level_4};
     } // namespace
@@ -72,9 +75,10 @@ namespace sys
         cpuAlgorithms->emplace(sys::cpu::AlgoID::FrequencyStepping,
                                std::make_unique<sys::cpu::FrequencyStepping>(powerProfile));
 
-        cpuFrequencyMonitor.push_back(CpuFrequencyMonitor(lowestLevelName));
-        cpuFrequencyMonitor.push_back(CpuFrequencyMonitor(middleLevelName));
-        cpuFrequencyMonitor.push_back(CpuFrequencyMonitor(highestLevelName));
+        cpuFrequencyMonitors.push_back(CpuFrequencyMonitor(lowestLevelName));
+        cpuFrequencyMonitors.push_back(CpuFrequencyMonitor(middleLevelName));
+        cpuFrequencyMonitors.push_back(CpuFrequencyMonitor(highestLevelName));
+        cpuFrequencyMonitors.push_back(CpuFrequencyMonitor(WfiName));
     }
 
     PowerManager::~PowerManager()
@@ -115,7 +119,7 @@ namespace sys
 
     [[nodiscard]] cpu::UpdateResult PowerManager::UpdateCpuFrequency()
     {
-        std::uint32_t cpuLoad = cpuStatistics.GetPercentageCpuLoad();
+        const std::uint32_t cpuLoad = cpuStatistics.GetPercentageCpuLoad();
         cpu::UpdateResult retval;
         const cpu::AlgorithmData data{
             cpuLoad, lowPowerControl->GetCurrentFrequencyLevel(), GetMinimumCpuFrequencyRequested()};
@@ -147,7 +151,7 @@ namespace sys
 
     void PowerManager::RemoveSentinel(std::string sentinelName) const
     {
-        cpuGovernor->RemoveSentinel(sentinelName);
+        cpuGovernor->RemoveSentinel(std::move(sentinelName));
     }
 
     void PowerManager::SetCpuFrequencyRequest(const std::string &sentinelName, bsp::CpuFrequencyMHz request)
@@ -164,6 +168,11 @@ namespace sys
         cpuStatistics.TrackChange(ret);
     }
 
+    void PowerManager::BlockWfiMode(const std::string &sentinelName, bool block)
+    {
+        cpuGovernor->BlockWfiMode(sentinelName, block);
+    }
+
     bool PowerManager::IsCpuPermanentFrequency()
     {
         return cpuAlgorithms->get(sys::cpu::AlgoID::FrequencyHold) != nullptr;
@@ -210,7 +219,7 @@ namespace sys
                              ? lowestLevelName
                              : (currentFreq == bsp::CpuFrequencyMHz::Level_6 ? highestLevelName : middleLevelName);
 
-        for (auto &level : cpuFrequencyMonitor) {
+        for (auto &level : cpuFrequencyMonitors) {
             if (level.GetName() == levelName) {
                 level.IncreaseTicks(ticks - lastCpuFrequencyChangeTimestamp);
             }
@@ -219,6 +228,35 @@ namespace sys
         lastCpuFrequencyChangeTimestamp = ticks;
     }
 
+    void PowerManager::UpdateCpuFrequencyMonitor(const std::string &name, std::uint32_t tickIncrease)
+    {
+        for (auto &level : cpuFrequencyMonitors) {
+            if (level.GetName() == name) {
+                level.IncreaseTicks(tickIncrease);
+            }
+        }
+    }
+
+    void PowerManager::EnterWfiIfReady()
+    {
+        if (cpuGovernor->IsWfiBlocked()) {
+            return;
+        }
+
+        const auto timeSpentInWFI = lowPowerControl->EnterWfiModeIfAllowed();
+        if (timeSpentInWFI > 0) {
+            /* We increase the frequency immediately after exiting WFI so that the xTaskCatchUpTicks procedure has
+             * time to execute and does not block the button press detection mechanism. */
+            SetCpuFrequency(bsp::CpuFrequencyMHz::Level_4);
+            portENTER_CRITICAL();
+            lowPowerControl->DisableSysTick();
+            xTaskCatchUpTicks(cpp_freertos::Ticks::MsToTicks(timeSpentInWFI));
+            lowPowerControl->EnableSysTick();
+            portEXIT_CRITICAL();
+            UpdateCpuFrequencyMonitor(WfiName, timeSpentInWFI);
+        }
+    }
+
     void PowerManager::LogPowerManagerStatistics()
     {
         const TickType_t tickCount          = xTaskGetTickCount();
@@ -226,7 +264,7 @@ namespace sys
         UpdateCpuFrequencyMonitor(lowPowerControl->GetCurrentFrequencyLevel());
 
         std::string log{"Last period (total): "};
-        for (auto &level : cpuFrequencyMonitor) {
+        for (auto &level : cpuFrequencyMonitors) {
             log.append(level.GetName() + ": " + std::to_string(level.GetPeriodRuntimePercentage(periodTickIncrease)) +
                        "% (" + std::to_string(level.GetTotalRuntimePercentage(tickCount)) + "%) ");
             level.SavePeriodTicks();
diff --git a/module-sys/SystemManager/SystemManagerCommon.cpp b/module-sys/SystemManager/SystemManagerCommon.cpp
index 913825fadcef03073c57e350f9401dbb58724395..f3cf064ecbc5d863cc4faf35e2a55569ff5af326 100644
--- a/module-sys/SystemManager/SystemManagerCommon.cpp
+++ b/module-sys/SystemManager/SystemManagerCommon.cpp
@@ -24,6 +24,7 @@
 #include <system/messages/SentinelRegistrationMessage.hpp>
 #include <system/messages/RequestCpuFrequencyMessage.hpp>
 #include <system/messages/HoldCpuFrequency.hpp>
+#include <system/messages/BlockWfiMode.hpp>
 #include <time/ScopedTime.hpp>
 #include "Timers/TimerFactory.hpp"
 #include <service-appmgr/StartupType.hpp>
@@ -240,7 +241,7 @@ namespace sys
         // Start System manager
         StartService();
 
-        freqTimer = sys::TimerFactory::createPeriodicTimer(
+        freqTimer = sys::TimerFactory::createSingleShotTimer(
             this, "cpuTick", constants::timerInitInterval, [this](sys::Timer &) { FreqUpdateTick(); });
         freqTimer.start();
 
@@ -654,6 +655,15 @@ namespace sys
             return sys::MessageNone{};
         });
 
+        connect(typeid(sys::BlockWfiModeMessage), [this](sys::Message *message) -> sys::MessagePointer {
+            auto msg = static_cast<sys::BlockWfiModeMessage *>(message);
+            powerManager->BlockWfiMode(msg->getName(), msg->getRequest());
+            if (msg->getHandle() != nullptr) {
+                xTaskNotifyGive(msg->getHandle());
+            }
+            return sys::MessageNone{};
+        });
+
         connect(typeid(sys::IsCpuPermanent), [this](sys::Message *message) -> sys::MessagePointer {
             return std::make_shared<sys::IsCpuPermanentResponse>(powerManager->IsCpuPermanentFrequency());
         });
@@ -787,6 +797,8 @@ namespace sys
 
         auto ret = powerManager->UpdateCpuFrequency();
         cpuStatistics->TrackChange(ret);
+        powerManager->EnterWfiIfReady();
+        freqTimer.restart(constants::timerPeriodInterval);
     }
 
     void SystemManagerCommon::UpdateResourcesAfterCpuFrequencyChange(bsp::CpuFrequencyMHz newFrequency)
diff --git a/module-sys/SystemManager/include/SystemManager/CpuGovernor.hpp b/module-sys/SystemManager/include/SystemManager/CpuGovernor.hpp
index 4d641191e692277f4e90e9a0e529b7ca80c4581a..b516f537792bc17c26368684a96acfe26e683a53 100644
--- a/module-sys/SystemManager/include/SystemManager/CpuGovernor.hpp
+++ b/module-sys/SystemManager/include/SystemManager/CpuGovernor.hpp
@@ -20,10 +20,13 @@ namespace sys
         [[nodiscard]] auto GetSentinel() const noexcept -> SentinelPointer;
         [[nodiscard]] auto GetRequestedFrequency() const noexcept -> bsp::CpuFrequencyMHz;
         void SetRequestedFrequency(bsp::CpuFrequencyMHz newFrequency);
+        void BlockWfiMode(bool request);
+        [[nodiscard]] auto IsWfiBlocked() const noexcept -> bool;
 
       private:
         SentinelPointer sentinelPtr;
         bsp::CpuFrequencyMHz requestedFrequency;
+        bool wfiBlocked{false};
     };
 
     using GovernorSentinelPointer = std::unique_ptr<GovernorSentinel>;
@@ -43,8 +46,10 @@ namespace sys
 
         void SetCpuFrequencyRequest(const std::string &sentinelName, bsp::CpuFrequencyMHz request);
         void ResetCpuFrequencyRequest(const std::string &sentinelName);
+        void BlockWfiMode(const std::string &sentinelName, bool request);
 
         [[nodiscard]] auto GetMinimumFrequencyRequested() noexcept -> sentinel::View;
+        [[nodiscard]] auto IsWfiBlocked() noexcept -> bool;
         void InformSentinelsAboutCpuFrequencyChange(bsp::CpuFrequencyMHz newFrequency) noexcept;
 
       private:
diff --git a/module-sys/SystemManager/include/SystemManager/CpuSentinel.hpp b/module-sys/SystemManager/include/SystemManager/CpuSentinel.hpp
index 64720e2934ba2c84b4c240859663360ae5ff72ed..59adc1c0624b5959dc989b5e99df625c3efae824 100644
--- a/module-sys/SystemManager/include/SystemManager/CpuSentinel.hpp
+++ b/module-sys/SystemManager/include/SystemManager/CpuSentinel.hpp
@@ -29,6 +29,10 @@ namespace sys
         virtual void HoldMinimumFrequency(bsp::CpuFrequencyMHz frequencyToHold);
         virtual void ReleaseMinimumFrequency();
 
+        /// @brief function used to block entering WFI mode (CPU stop)
+        /// @param block - boolean flag with blocking command
+        void BlockWfiMode(bool block);
+
         [[nodiscard]] auto GetFrequency() const noexcept -> bsp::CpuFrequencyMHz;
 
         void CpuFrequencyHasChanged(bsp::CpuFrequencyMHz newFrequency);
@@ -47,6 +51,7 @@ namespace sys
         std::atomic<bsp::CpuFrequencyMHz> currentFrequency{bsp::CpuFrequencyMHz::Level_0};
         sys::Service *owner{nullptr};
         TickType_t holdTicks;
+        bool blockWfiMode{false};
 
         /// function called from the PowerManager context
         /// to update resources immediately
diff --git a/module-sys/SystemManager/include/SystemManager/PowerManager.hpp b/module-sys/SystemManager/include/SystemManager/PowerManager.hpp
index 3a93a9bb26a14796d0980babf88ef61ad90d916e..6e5f2d3eb385d79c644a1cc1f3e284f0098a89f2 100644
--- a/module-sys/SystemManager/include/SystemManager/PowerManager.hpp
+++ b/module-sys/SystemManager/include/SystemManager/PowerManager.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #pragma once
@@ -29,10 +29,8 @@ namespace sys
         explicit CpuFrequencyMonitor(const std::string &name);
 
         [[nodiscard]] auto GetName() const noexcept -> std::string;
-        [[nodiscard]] auto GetPeriodRuntimePercentage(const TickType_t periodTicksIncrease) const noexcept
-            -> std::uint32_t;
-        [[nodiscard]] auto GetTotalRuntimePercentage(const TickType_t totalTicksIncrease) const noexcept
-            -> std::uint32_t;
+        [[nodiscard]] auto GetPeriodRuntimePercentage(TickType_t periodTicksIncrease) const noexcept -> std::uint32_t;
+        [[nodiscard]] auto GetTotalRuntimePercentage(TickType_t totalTicksIncrease) const noexcept -> std::uint32_t;
         void IncreaseTicks(TickType_t ticks);
         void SavePeriodTicks();
 
@@ -45,7 +43,7 @@ namespace sys
     class PowerManager
     {
       public:
-        explicit PowerManager(CpuStatistics &cpuStats, TaskStatistics &taskStats);
+        PowerManager(CpuStatistics &cpuStats, TaskStatistics &taskStats);
         ~PowerManager();
 
         std::int32_t PowerOff();
@@ -70,19 +68,20 @@ namespace sys
         bool IsCpuPermanentFrequency();
         void SetPermanentFrequency(bsp::CpuFrequencyMHz freq);
         void ResetPermanentFrequency();
-
+        void BlockWfiMode(const std::string &sentinelName, bool block);
+        void EnterWfiIfReady();
         void LogPowerManagerStatistics();
 
       private:
         void SetCpuFrequency(bsp::CpuFrequencyMHz freq);
-
         void UpdateCpuFrequencyMonitor(bsp::CpuFrequencyMHz currentFreq);
+        void UpdateCpuFrequencyMonitor(const std::string &name, std::uint32_t tickIncrease);
         [[nodiscard]] auto GetMinimumCpuFrequencyRequested() const noexcept -> sentinel::View;
 
         TickType_t lastCpuFrequencyChangeTimestamp{0};
         TickType_t lastLogStatisticsTimestamp{0};
 
-        std::vector<CpuFrequencyMonitor> cpuFrequencyMonitor;
+        std::vector<CpuFrequencyMonitor> cpuFrequencyMonitors;
 
         std::shared_ptr<drivers::DriverSEMC> driverSEMC;
         std::unique_ptr<bsp::LowPowerMode> lowPowerControl;
@@ -94,5 +93,4 @@ namespace sys
         CpuStatistics &cpuStatistics;
         TaskStatistics &taskStatistics;
     };
-
 } // namespace sys
diff --git a/module-sys/common/include/system/messages/BlockWfiMode.hpp b/module-sys/common/include/system/messages/BlockWfiMode.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..582b354736a1df6d31174392b6f4f4edf6aae60d
--- /dev/null
+++ b/module-sys/common/include/system/messages/BlockWfiMode.hpp
@@ -0,0 +1,39 @@
+// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include <Service/Message.hpp>
+#include <bsp/common.hpp>
+
+namespace sys
+{
+    class BlockWfiModeMessage : public sys::DataMessage
+    {
+      public:
+        BlockWfiModeMessage(std::string sentinelName, bool block, TaskHandle_t handle)
+            : sys::DataMessage(MessageType::SystemManagerCpuFrequency), sentinelName(std::move(sentinelName)),
+              block(block), handle(handle)
+        {}
+
+        [[nodiscard]] auto getRequest() const noexcept
+        {
+            return block;
+        };
+
+        [[nodiscard]] auto getName() const
+        {
+            return sentinelName;
+        };
+
+        [[nodiscard]] TaskHandle_t getHandle() const
+        {
+            return handle;
+        }
+
+      private:
+        std::string sentinelName;
+        bool block;
+        TaskHandle_t handle;
+    };
+} // namespace sys
diff --git a/products/BellHybrid/BellHybridMain.cpp b/products/BellHybrid/BellHybridMain.cpp
index a52071de2672d334330a785643efb8b568b97278..c0ac39d4fdbf9e7834e6427b3e230e655cf1a9f8 100644
--- a/products/BellHybrid/BellHybridMain.cpp
+++ b/products/BellHybrid/BellHybridMain.cpp
@@ -23,7 +23,6 @@
 #include <audio/ServiceAudio.hpp>
 #include <db/ServiceDB.hpp>
 #include <evtmgr/EventManager.hpp>
-#include <Service/ServiceCreator.hpp>
 #include <service-appmgr/ServiceApplicationManagerName.hpp>
 #include <service-desktop/ServiceDesktop.hpp>
 #include <service-eink/ServiceEink.hpp>
@@ -31,21 +30,13 @@
 #include <service-time/ServiceTime.hpp>
 #include <service-fileindexer/ServiceFileIndexer.hpp>
 
-#include <Application.hpp>
-#include <ApplicationLauncher.hpp>
-#include <log/log.hpp>
 #include <logdump/logdump.h>
 #include <Logger.hpp>
 #include <product/version.hpp>
 #include <sys/SystemManager.hpp>
-#include <SystemWatchdog/SystemWatchdog.hpp>
-#include <thread.hpp>
 #include <time/AlarmOperations.hpp>
 #include <Paths.hpp>
 
-#include <memory>
-#include <vector>
-
 #if SYSTEM_VIEW_ENABLED
 #include <SEGGER/SEGGER_SYSVIEW.h>
 #endif
diff --git a/products/BellHybrid/apps/application-bell-meditation-timer/MeditationTimer.cpp b/products/BellHybrid/apps/application-bell-meditation-timer/MeditationTimer.cpp
index c962b41a4e4674c91d08c5742079ccbddc2a2d39..91732b18bed1cfc9be2b2cb70c9d81a15308ac59 100644
--- a/products/BellHybrid/apps/application-bell-meditation-timer/MeditationTimer.cpp
+++ b/products/BellHybrid/apps/application-bell-meditation-timer/MeditationTimer.cpp
@@ -25,6 +25,8 @@
 #include <common/windows/SessionPausedWindow.hpp>
 #include <common/windows/AppsBatteryStatusWindow.hpp>
 
+#include <system/messages/SentinelRegistrationMessage.hpp>
+
 namespace app
 {
     MeditationTimer::MeditationTimer(std::string name,
@@ -55,6 +57,11 @@ namespace app
             return ret;
         }
 
+        cpuSentinel                  = std::make_shared<sys::CpuSentinel>(defaultName, this);
+        auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(cpuSentinel);
+        bus.sendUnicast(std::move(sentinelRegistrationMsg), service::name::system_manager);
+        cpuSentinel->BlockWfiMode(true);
+
         audioModel         = std::make_unique<AudioModel>(this);
         chimeIntervalModel = std::make_unique<meditation::models::ChimeInterval>(this);
         chimeVolumeModel   = std::make_unique<meditation::models::ChimeVolume>(*audioModel);
@@ -139,5 +146,8 @@ namespace app
 
         return handleAsyncResponse(resp);
     }
-    MeditationTimer::~MeditationTimer() = default;
+    MeditationTimer::~MeditationTimer()
+    {
+        cpuSentinel->BlockWfiMode(false);
+    }
 } // namespace app
diff --git a/products/BellHybrid/apps/application-bell-meditation-timer/include/application-bell-meditation-timer/MeditationTimer.hpp b/products/BellHybrid/apps/application-bell-meditation-timer/include/application-bell-meditation-timer/MeditationTimer.hpp
index 9855a539c070b5dc9dcabed7f948ab1a07e6c07e..6d2c6fed9866e972d0cb978c3539512e8aa0d263 100644
--- a/products/BellHybrid/apps/application-bell-meditation-timer/include/application-bell-meditation-timer/MeditationTimer.hpp
+++ b/products/BellHybrid/apps/application-bell-meditation-timer/include/application-bell-meditation-timer/MeditationTimer.hpp
@@ -49,6 +49,7 @@ namespace app
         std::unique_ptr<app::meditation::models::StartDelay> startDelayModel;
         std::unique_ptr<AbstractAudioModel> audioModel;
         std::unique_ptr<app::meditation::models::Statistics> statisticsModel;
+        std::shared_ptr<sys::CpuSentinel> cpuSentinel;
         std::unique_ptr<AbstractBatteryModel> batteryModel;
         std::unique_ptr<AbstractLowBatteryInfoModel> lowBatteryInfoModel;
     };
diff --git a/products/BellHybrid/apps/application-bell-onboarding/ApplicationBellOnBoarding.cpp b/products/BellHybrid/apps/application-bell-onboarding/ApplicationBellOnBoarding.cpp
index cde155ad0fffe370b5008535f68e11654f7e039c..a380145ebb241cafcee6e760f98fc36115bb3dec 100644
--- a/products/BellHybrid/apps/application-bell-onboarding/ApplicationBellOnBoarding.cpp
+++ b/products/BellHybrid/apps/application-bell-onboarding/ApplicationBellOnBoarding.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "ApplicationBellOnBoarding.hpp"
diff --git a/products/BellHybrid/apps/application-bell-powernap/ApplicationBellPowerNap.cpp b/products/BellHybrid/apps/application-bell-powernap/ApplicationBellPowerNap.cpp
index 5630aa8909e4c7fcc25925fb2b8eb7ee7a66588b..498c37641d1ae203e2f779472b70426c5a88f249 100644
--- a/products/BellHybrid/apps/application-bell-powernap/ApplicationBellPowerNap.cpp
+++ b/products/BellHybrid/apps/application-bell-powernap/ApplicationBellPowerNap.cpp
@@ -9,6 +9,7 @@
 #include <common/models/TimeModel.hpp>
 #include <Paths.hpp>
 #include <common/windows/SessionPausedWindow.hpp>
+#include <system/messages/SentinelRegistrationMessage.hpp>
 
 namespace app
 {
@@ -23,13 +24,23 @@ namespace app
         bus.channels.push_back(sys::BusChannel::ServiceAudioNotifications);
     }
 
-    ApplicationBellPowerNap::~ApplicationBellPowerNap() = default;
+    ApplicationBellPowerNap::~ApplicationBellPowerNap()
+    {
+        cpuSentinel->BlockWfiMode(false);
+    }
+
     sys::ReturnCodes ApplicationBellPowerNap::InitHandler()
     {
         auto ret = Application::InitHandler();
         if (ret != sys::ReturnCodes::Success) {
             return ret;
         }
+
+        cpuSentinel                  = std::make_shared<sys::CpuSentinel>(applicationBellPowerNapName, this);
+        auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(cpuSentinel);
+        bus.sendUnicast(std::move(sentinelRegistrationMsg), service::name::system_manager);
+        cpuSentinel->BlockWfiMode(true);
+
         createUserInterface();
 
         return sys::ReturnCodes::Success;
diff --git a/products/BellHybrid/apps/application-bell-powernap/include/application-bell-powernap/ApplicationBellPowerNap.hpp b/products/BellHybrid/apps/application-bell-powernap/include/application-bell-powernap/ApplicationBellPowerNap.hpp
index 2f0c5dbe73285a98376337824b63e7f76091387e..92c420c53376f322203c209f79f42ddfd6d77c4a 100644
--- a/products/BellHybrid/apps/application-bell-powernap/include/application-bell-powernap/ApplicationBellPowerNap.hpp
+++ b/products/BellHybrid/apps/application-bell-powernap/include/application-bell-powernap/ApplicationBellPowerNap.hpp
@@ -22,6 +22,7 @@ namespace app
     {
       private:
         std::unique_ptr<AbstractAudioModel> audioModel;
+        std::shared_ptr<sys::CpuSentinel> cpuSentinel;
         void onStop() override;
 
       public:
diff --git a/products/BellHybrid/apps/application-bell-relaxation/ApplicationBellRelaxation.cpp b/products/BellHybrid/apps/application-bell-relaxation/ApplicationBellRelaxation.cpp
index 794dae6f8f95bd52d9912fd864d765155649aa60..461273e9e677b75ba7882682f88aee0ce9905acf 100644
--- a/products/BellHybrid/apps/application-bell-relaxation/ApplicationBellRelaxation.cpp
+++ b/products/BellHybrid/apps/application-bell-relaxation/ApplicationBellRelaxation.cpp
@@ -28,6 +28,7 @@
 #include <common/windows/AppsBatteryStatusWindow.hpp>
 #include <audio/AudioMessage.hpp>
 #include <service-db/DBNotificationMessage.hpp>
+#include <system/messages/SentinelRegistrationMessage.hpp>
 
 #include <log/log.hpp>
 
@@ -50,7 +51,11 @@ namespace app
         bus.channels.push_back(sys::BusChannel::ServiceAudioNotifications);
         bus.channels.push_back(sys::BusChannel::ServiceDBNotifications);
     }
-    ApplicationBellRelaxation::~ApplicationBellRelaxation() = default;
+
+    ApplicationBellRelaxation::~ApplicationBellRelaxation()
+    {
+        cpuSentinel->BlockWfiMode(false);
+    }
 
     sys::ReturnCodes ApplicationBellRelaxation::InitHandler()
     {
@@ -59,6 +64,11 @@ namespace app
             return ret;
         }
 
+        cpuSentinel                  = std::make_shared<sys::CpuSentinel>(applicationBellRelaxationName, this);
+        auto sentinelRegistrationMsg = std::make_shared<sys::SentinelRegistrationMessage>(cpuSentinel);
+        bus.sendUnicast(std::move(sentinelRegistrationMsg), service::name::system_manager);
+        cpuSentinel->BlockWfiMode(true);
+
         batteryModel                 = std::make_unique<app::BatteryModel>(this);
         lowBatteryInfoModel          = std::make_unique<app::LowBatteryInfoModel>();
         player                       = std::make_unique<relaxation::RelaxationPlayer>(*audioModel);
@@ -111,12 +121,11 @@ namespace app
                                   return std::make_unique<gui::RelaxationRunningLoopWindow>(app, std::move(presenter));
                               });
 
-        windowsFactory.attach(
-            gui::window::name::relaxationPaused, [this](ApplicationCommon *app, const std::string &name) {
-                auto timeModel = std::make_unique<app::TimeModel>();
-                auto presenter = std::make_unique<relaxation::RelaxationPausedPresenter>(std::move(timeModel));
-                return std::make_unique<gui::RelaxationPausedWindow>(app, std::move(presenter));
-            });
+        windowsFactory.attach(gui::window::name::relaxationPaused, [](ApplicationCommon *app, const std::string &name) {
+            auto timeModel = std::make_unique<app::TimeModel>();
+            auto presenter = std::make_unique<relaxation::RelaxationPausedPresenter>(std::move(timeModel));
+            return std::make_unique<gui::RelaxationPausedWindow>(app, std::move(presenter));
+        });
 
         windowsFactory.attach(gui::popup::window::volume_window,
                               [this](ApplicationCommon *app, const std::string &name) {
diff --git a/products/BellHybrid/apps/application-bell-relaxation/include/application-bell-relaxation/ApplicationBellRelaxation.hpp b/products/BellHybrid/apps/application-bell-relaxation/include/application-bell-relaxation/ApplicationBellRelaxation.hpp
index 93bb774e988c1f2a0c51aec0b356f9d9eaf9c702..59631e6423859700bfc432d520bb22b634e08a00 100644
--- a/products/BellHybrid/apps/application-bell-relaxation/include/application-bell-relaxation/ApplicationBellRelaxation.hpp
+++ b/products/BellHybrid/apps/application-bell-relaxation/include/application-bell-relaxation/ApplicationBellRelaxation.hpp
@@ -36,6 +36,7 @@ namespace app
         std::unique_ptr<AbstractLowBatteryInfoModel> lowBatteryInfoModel;
         std::unique_ptr<relaxation::RelaxationPlayer> player;
         sys::TimerHandle relaxationRebuildTimerHandle{};
+        std::shared_ptr<sys::CpuSentinel> cpuSentinel;
 
         void onStop() override;
         sys::MessagePointer handleSwitchWindow(sys::Message *msgl) override;
diff --git a/products/BellHybrid/apps/common/src/layouts/HomeScreenLayoutClassicWithDate.cpp b/products/BellHybrid/apps/common/src/layouts/HomeScreenLayoutClassicWithDate.cpp
index d90c942ccea068f1ddf8c296d39c4c39a61d177e..2116df964c7b370b96c8f6fa8e7c0ea95bb948c4 100644
--- a/products/BellHybrid/apps/common/src/layouts/HomeScreenLayoutClassicWithDate.cpp
+++ b/products/BellHybrid/apps/common/src/layouts/HomeScreenLayoutClassicWithDate.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "layouts/HomeScreenLayoutClassicWithDate.hpp"
@@ -86,5 +86,4 @@ namespace gui
     {
         return true;
     }
-
 }; // namespace gui
diff --git a/products/BellHybrid/apps/common/src/layouts/HomeScreenLayouts.cpp b/products/BellHybrid/apps/common/src/layouts/HomeScreenLayouts.cpp
index 95c1040909cf1a212bd6b19d8f9c9841388eb6e0..eca51f93a4fa45ebaf843db41a73e6b58221a648 100644
--- a/products/BellHybrid/apps/common/src/layouts/HomeScreenLayouts.cpp
+++ b/products/BellHybrid/apps/common/src/layouts/HomeScreenLayouts.cpp
@@ -1,5 +1,5 @@
 
-// Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+// Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include <common/layouts/HomeScreenLayoutNames.hpp>
@@ -80,5 +80,4 @@ namespace gui::factory
     {
         return gui::layout::ClassicWithBattery;
     }
-
 } // namespace gui::factory
diff --git a/products/BellHybrid/services/db/databases/migration/settings_bell/0/up.sql b/products/BellHybrid/services/db/databases/migration/settings_bell/0/up.sql
index b779aa0cb32f4a9e5629c28297e7aa0126707550..71c1cf7dd5aba1a3eae561bcf946e682fe82d826 100644
--- a/products/BellHybrid/services/db/databases/migration/settings_bell/0/up.sql
+++ b/products/BellHybrid/services/db/databases/migration/settings_bell/0/up.sql
@@ -1,4 +1,4 @@
--- Copyright (c) 2017-2023, Mudita Sp. z.o.o. All rights reserved.
+-- Copyright (c) 2017-2024, Mudita Sp. z.o.o. All rights reserved.
 -- For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 --
diff --git a/third-party/freeRTOS-kernel/freeRTOS-kernel b/third-party/freeRTOS-kernel/freeRTOS-kernel
index f621edbabfb5690dbe4171b4243a5c6d40c7876b..c5d50b33033b8496fcf631b6a183198511454dc0 160000
--- a/third-party/freeRTOS-kernel/freeRTOS-kernel
+++ b/third-party/freeRTOS-kernel/freeRTOS-kernel
@@ -1 +1 @@
-Subproject commit f621edbabfb5690dbe4171b4243a5c6d40c7876b
+Subproject commit c5d50b33033b8496fcf631b6a183198511454dc0