From 3a5f668c981db13ba5e31cabb528c43728a8bf2e Mon Sep 17 00:00:00 2001
From: Lefucjusz <marcin.lyda@mudita.com>
Date: Thu, 4 May 2023 21:33:02 +0200
Subject: [PATCH] [BH-1583] Fix SNVS LP lockup after debugging

* Fix of the issue that manifested after
running MuditaOS on Harmony via JLink,
which resulted in OS freezing when
trying to set time in normally booted
(via the bootloader) device.
* Minor RTC code cleanup.
* Partial T6 JLink script cleanup.
---
 evkbimxrt1050_sdram_init_T6.jlinkscript       | 445 +++++++++---------
 .../bellpx/bsp/rtc/rtc_configuration.cpp      |   8 +-
 module-bsp/board/rt1051/bsp/rtc/rtc.cpp       | 125 +++--
 module-bsp/board/rt1051/common/board.cpp      |   5 +-
 module-bsp/bsp/rtc/rtc.hpp                    |   3 +-
 5 files changed, 296 insertions(+), 290 deletions(-)

diff --git a/evkbimxrt1050_sdram_init_T6.jlinkscript b/evkbimxrt1050_sdram_init_T6.jlinkscript
index c8c7846397474f9c943639f32ec98bea8d09bb4c..a8bf48f9c582e454deec75957d065590e41e9051 100644
--- a/evkbimxrt1050_sdram_init_T6.jlinkscript
+++ b/evkbimxrt1050_sdram_init_T6.jlinkscript
@@ -26,243 +26,252 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
- 
-void Load_Dcdc_Trim()
-{
-  unsigned int ocotp_base;  
-  unsigned int dcdc_base;  
-  unsigned int ocotp_fuse_bank0_base;  
-  unsigned int reg;
-  unsigned int index;
-  unsigned int trim_value;
-  unsigned int dcdc_trim_loaded;
-  ocotp_base = 0x401F4000;
-  ocotp_fuse_bank0_base = 0x401F4000 + 0x400;
-  dcdc_base = 0x40080000;
-  dcdc_trim_loaded = 0;
-  reg = MEM_ReadU32(ocotp_fuse_bank0_base + 0x90);
-  if (reg & (1<<10))
-  {
-      trim_value = (reg & (0x1F << 11)) >> 11;
-      reg = (MEM_ReadU32(dcdc_base + 0x4) & ~(0x1F << 24)) | (trim_value << 24);
-      MEM_WriteU32(dcdc_base + 0x4, reg);
-      dcdc_trim_loaded = 1;
-  }
-  // DCDC_VOLT_CHANG_EN
-  reg = MEM_ReadU32(ocotp_fuse_bank0_base + 0x80);
-  if (reg & (1<<30))
-  {
-    index = (reg & (3 << 28)) >> 28;
-    if (index < 4)
-    {
-      reg = (MEM_ReadU32(dcdc_base + 0xC) & ~(0x1F)) | (0xF + index);
-      MEM_WriteU32(dcdc_base + 0xC, reg);
-      dcdc_trim_loaded = 1;
-    }
-  }
-  if (dcdc_trim_loaded)
-  {
-      // delay 1ms for dcdc to get stable
-      SYS_Sleep(1);
-      JLINK_SYS_Report("DCDC trim value loaded.");
-  }
-}
- 
+
 void Clock_Init() 
 {
-    //
-  MEM_WriteU32(0x400FC068,0xffffffff);
-  MEM_WriteU32(0x400FC06C,0xffffffff);
+    unsigned int CCM_CCGR0;
+    unsigned int CCM_CCGR1;
+    unsigned int CCM_CCGR2;
+    unsigned int CCM_CCGR3;
+    unsigned int CCM_CCGR4;
+    unsigned int CCM_CCGR5;
+    unsigned int CCM_CCGR6;
+    unsigned int CCM_ANALOG_PLL_SYS;
+    unsigned int CCM_ANALOG_PFD_528;
+    unsigned int CCM_CBCDR;
+
+    CCM_CCGR0 = 0x400FC068;
+    CCM_CCGR1 = 0x400FC06C;
+    CCM_CCGR2 = 0x400FC070;
+    CCM_CCGR3 = 0x400FC074;
+    CCM_CCGR4 = 0x400FC078;
+    CCM_CCGR5 = 0x400FC07C;
+    CCM_CCGR6 = 0x400FC080;
+    CCM_ANALOG_PLL_SYS = 0x400D8030;
+    CCM_ANALOG_PFD_528 = 0x400D8100;
+    CCM_CBCDR = 0x400FC014;
+
+    /* Enable clocking for all peripherals */
+    MEM_WriteU32(CCM_CCGR0, 0xFFFFFFFF);
+    MEM_WriteU32(CCM_CCGR1, 0xFFFFFFFF);
+    MEM_WriteU32(CCM_CCGR2, 0xFFFFFFFF);
+    MEM_WriteU32(CCM_CCGR3, 0xFFFFFFFF);
+    MEM_WriteU32(CCM_CCGR4, 0xFFFFFFFF);
+    MEM_WriteU32(CCM_CCGR5, 0xFFFFFFFF);
+    MEM_WriteU32(CCM_CCGR6, 0xFFFFFFFF);
+
+    /* Configure PLL_SYS:
+     * 24MHz OSC as source;
+     * output enabled;
+     * Fout = Fref * 22
+     * Fout = 24MHz * 22 = 528MHz */
+    MEM_WriteU32(CCM_ANALOG_PLL_SYS, 0x00002001);
+
+    /* Configure PFD_528:
+     * set PFD2_FRAC = 29;
+     * Fout = 528MHz * 18/29 = 327.724MHz */
+    MEM_WriteU32(CCM_ANALOG_PFD_528, 0x001D0000);
 
-  MEM_WriteU32(0x400FC070,0xffffffff);
-  MEM_WriteU32(0x400FC074,0xffffffff);
-  MEM_WriteU32(0x400FC078,0xffffffff);
-  MEM_WriteU32(0x400FC07C,0xffffffff);
-  MEM_WriteU32(0x400FC080,0xffffffff);
-  MEM_WriteU32(0x400D8030,0x00002001); // CCM_ANALOG_PLL_SYS
-  MEM_WriteU32(0x400D8100,0x001d0000);  // CCM_ANALOG_PFD_528
-  MEM_WriteU32(0x400FC014,0x00010D40); // CCM_CBCDR
-  JLINK_SYS_Report("Clock Init Done");
+    /* Configure CCM Bus Clock Dividers:
+     * set SEMC alternative clock as SEMC clock root;
+     * set PLL2 PFD2 as alternative clock for SEMC root clock;
+     * set IPG_PODF divider to 2;
+     * set AHB_PODF divider to 4;
+     * set SEMC_PODF divider to 2;
+     * set peripheral main clock to pre_periph_clk_sel;
+     * set PERIPH_CLK2_PODF divider to 1 */
+    MEM_WriteU32(CCM_CBCDR, 0x00010D40);
+
+    JLINK_SYS_Report("Clock Init Done");
 }
+
 void SDRAM_WaitIpCmdDone(void) 
 {
-  unsigned int reg;
-  do
-  {
-      reg = MEM_ReadU32(0x402F003C);
-  }while((reg & 0x3) == 0);
-  
-  MEM_WriteU32(0x402F003C,0x00000003); // clear IPCMDERR and IPCMDDONE bits
+    unsigned int SEMC_INTR;
+    unsigned int reg_val;
+
+    SEMC_INTR = 0x402F003C;
+
+    do
+    {
+        reg_val = MEM_ReadU32(SEMC_INTR);
+    } while((reg_val & 0x00000003) == 0);
+
+    MEM_WriteU32(SEMC_INTR, 0x00000003); // Clear IPCMDERR and IPCMDDONE bits
 }
-void SDRAM_Init() {
-  // Config IOMUX for SDRAM
-  MEM_WriteU32(0x401F8014,0x00000000); // EMC_00
-  MEM_WriteU32(0x401F8018,0x00000000); // EMC_01
-  MEM_WriteU32(0x401F801C,0x00000000); // EMC_02
-  MEM_WriteU32(0x401F8020,0x00000000); // EMC_03
-  MEM_WriteU32(0x401F8024,0x00000000); // EMC_04
-  MEM_WriteU32(0x401F8028,0x00000000); // EMC_05
-  MEM_WriteU32(0x401F802C,0x00000000); // EMC_06
-  MEM_WriteU32(0x401F8030,0x00000000); // EMC_07
-  MEM_WriteU32(0x401F8034,0x00000000); // EMC_08
-  MEM_WriteU32(0x401F8038,0x00000000); // EMC_09
-  MEM_WriteU32(0x401F803C,0x00000000); // EMC_10
-  MEM_WriteU32(0x401F8040,0x00000000); // EMC_11
-  MEM_WriteU32(0x401F8044,0x00000000); // EMC_12
-  MEM_WriteU32(0x401F8048,0x00000000); // EMC_13
-  MEM_WriteU32(0x401F804C,0x00000000); // EMC_14
-  MEM_WriteU32(0x401F8050,0x00000000); // EMC_15
-  MEM_WriteU32(0x401F8054,0x00000000); // EMC_16
-  MEM_WriteU32(0x401F8058,0x00000000); // EMC_17
-  MEM_WriteU32(0x401F805C,0x00000000); // EMC_18
-  MEM_WriteU32(0x401F8060,0x00000000); // EMC_19
-  MEM_WriteU32(0x401F8064,0x00000000); // EMC_20
-  MEM_WriteU32(0x401F8068,0x00000000); // EMC_21
-  MEM_WriteU32(0x401F806C,0x00000000); // EMC_22
-  MEM_WriteU32(0x401F8070,0x00000000); // EMC_23
-  MEM_WriteU32(0x401F8074,0x00000000); // EMC_24
-  MEM_WriteU32(0x401F8078,0x00000000); // EMC_25
-  MEM_WriteU32(0x401F807C,0x00000000); // EMC_26
-  MEM_WriteU32(0x401F8080,0x00000000); // EMC_27
-  MEM_WriteU32(0x401F8084,0x00000000); // EMC_28
-  MEM_WriteU32(0x401F8088,0x00000000); // EMC_29
-  MEM_WriteU32(0x401F808C,0x00000000); // EMC_30
-  MEM_WriteU32(0x401F8090,0x00000000); // EMC_31
-  MEM_WriteU32(0x401F8094,0x00000000); // EMC_32
-  MEM_WriteU32(0x401F8098,0x00000000); // EMC_33
-  MEM_WriteU32(0x401F809C,0x00000000); // EMC_34
-  MEM_WriteU32(0x401F80A0,0x00000000); // EMC_35
-  MEM_WriteU32(0x401F80A4,0x00000000); // EMC_36
-  MEM_WriteU32(0x401F80A8,0x00000000); // EMC_37
-  MEM_WriteU32(0x401F80AC,0x00000000); // EMC_38
-  MEM_WriteU32(0x401F80B0,0x00000010); // EMC_39
-  
-  // PAD ctrl
-  // drive strength = 0x7 to increase drive strength
-  // otherwise the data7 bit may fail.
-  MEM_WriteU32(0x401F8204,0x000110F9); // EMC_00
-  MEM_WriteU32(0x401F8208,0x000110F9); // EMC_01
-  MEM_WriteU32(0x401F820C,0x000110F9); // EMC_02
-  MEM_WriteU32(0x401F8210,0x000110F9); // EMC_03
-  MEM_WriteU32(0x401F8214,0x000110F9); // EMC_04
-  MEM_WriteU32(0x401F8218,0x000110F9); // EMC_05
-  MEM_WriteU32(0x401F821C,0x000110F9); // EMC_06
-  MEM_WriteU32(0x401F8220,0x000110F9); // EMC_07
-  MEM_WriteU32(0x401F8224,0x000110F9); // EMC_08
-  MEM_WriteU32(0x401F8228,0x000110F9); // EMC_09
-  MEM_WriteU32(0x401F822C,0x000110F9); // EMC_10
-  MEM_WriteU32(0x401F8230,0x000110F9); // EMC_11
-  MEM_WriteU32(0x401F8234,0x000110F9); // EMC_12
-  MEM_WriteU32(0x401F8238,0x000110F9); // EMC_13
-  MEM_WriteU32(0x401F823C,0x000110F9); // EMC_14
-  MEM_WriteU32(0x401F8240,0x000110F9); // EMC_15
-  MEM_WriteU32(0x401F8244,0x000110F9); // EMC_16
-  MEM_WriteU32(0x401F8248,0x000110F9); // EMC_17
-  MEM_WriteU32(0x401F824C,0x000110F9); // EMC_18
-  MEM_WriteU32(0x401F8250,0x000110F9); // EMC_19
-  MEM_WriteU32(0x401F8254,0x000110F9); // EMC_20
-  MEM_WriteU32(0x401F8258,0x000110F9); // EMC_21
-  MEM_WriteU32(0x401F825C,0x000110F9); // EMC_22
-  MEM_WriteU32(0x401F8260,0x000110F9); // EMC_23
-  MEM_WriteU32(0x401F8264,0x000110F9); // EMC_24
-  MEM_WriteU32(0x401F8268,0x000110F9); // EMC_25
-  MEM_WriteU32(0x401F826C,0x000110F9); // EMC_26
-  MEM_WriteU32(0x401F8270,0x000110F9); // EMC_27
-  MEM_WriteU32(0x401F8274,0x000110F9); // EMC_28
-  MEM_WriteU32(0x401F8278,0x000110F9); // EMC_29
-  MEM_WriteU32(0x401F827C,0x000110F9); // EMC_30
-  MEM_WriteU32(0x401F8280,0x000110F9); // EMC_31
-  MEM_WriteU32(0x401F8284,0x000110F9); // EMC_32
-  MEM_WriteU32(0x401F8288,0x000110F9); // EMC_33
-  MEM_WriteU32(0x401F828C,0x000110F9); // EMC_34
-  MEM_WriteU32(0x401F8290,0x000110F9); // EMC_35
-  MEM_WriteU32(0x401F8294,0x000110F9); // EMC_36
-  MEM_WriteU32(0x401F8298,0x000110F9); // EMC_37
-  MEM_WriteU32(0x401F829C,0x000110F9); // EMC_38
-  MEM_WriteU32(0x401F82A0,0x000110F9); // EMC_39
 
-  // Config SDR Controller Registers/
-  MEM_WriteU32(0x402F0000,0x10000004); // MCR
-  MEM_WriteU32(0x402F0008,0x00030524); // BMCR0
-  MEM_WriteU32(0x402F000C,0x06030524); // BMCR1
-  MEM_WriteU32(0x402F0010,0x80000019); // BR0, 16MB OK
-  MEM_WriteU32(0x402F0040,0x00000F31); // SDRAMCR0 OK
-  MEM_WriteU32(0x402F0044,0x00652922); // SDRAMCR1
-  MEM_WriteU32(0x402F0048,0x000a0b0d); // SDRAMCR2
-  MEM_WriteU32(0x402F004C,0x0f0f0a00); // SDRAMCR3
-  MEM_WriteU32(0x402F0090,0x80000000); // IPCR0 OK
-  MEM_WriteU32(0x402F0094,0x00000002); // IPCR1 OK
-  MEM_WriteU32(0x402F0098,0x00000000); // IPCR2 OK
-  MEM_WriteU32(0x402F009C,0xA55A000F); // IPCMD, SD_CC_IPREA
-  SDRAM_WaitIpCmdDone();
-  MEM_WriteU32(0x402F009C,0xA55A000C); // SD_CC_IAF
-  SDRAM_WaitIpCmdDone();
-  MEM_WriteU32(0x402F009C,0xA55A000C); // SD_CC_IAF
-  SDRAM_WaitIpCmdDone();
-  MEM_WriteU32(0x402F00A0,0x00000033); // IPTXDAT OK
-  MEM_WriteU32(0x402F009C,0xA55A000A); // SD_CC_IMS
-  SDRAM_WaitIpCmdDone();
-  MEM_WriteU32(0x402F004C,0x08080A01 ); // enable sdram self refresh again after initialization done.
+void SDRAM_Init()
+{
+    // Config IOMUX for SDRAM
+    MEM_WriteU32(0x401F8014, 0x00000000); // EMC_00
+    MEM_WriteU32(0x401F8018, 0x00000000); // EMC_01
+    MEM_WriteU32(0x401F801C, 0x00000000); // EMC_02
+    MEM_WriteU32(0x401F8020, 0x00000000); // EMC_03
+    MEM_WriteU32(0x401F8024, 0x00000000); // EMC_04
+    MEM_WriteU32(0x401F8028, 0x00000000); // EMC_05
+    MEM_WriteU32(0x401F802C, 0x00000000); // EMC_06
+    MEM_WriteU32(0x401F8030, 0x00000000); // EMC_07
+    MEM_WriteU32(0x401F8034, 0x00000000); // EMC_08
+    MEM_WriteU32(0x401F8038, 0x00000000); // EMC_09
+    MEM_WriteU32(0x401F803C, 0x00000000); // EMC_10
+    MEM_WriteU32(0x401F8040, 0x00000000); // EMC_11
+    MEM_WriteU32(0x401F8044, 0x00000000); // EMC_12
+    MEM_WriteU32(0x401F8048, 0x00000000); // EMC_13
+    MEM_WriteU32(0x401F804C, 0x00000000); // EMC_14
+    MEM_WriteU32(0x401F8050, 0x00000000); // EMC_15
+    MEM_WriteU32(0x401F8054, 0x00000000); // EMC_16
+    MEM_WriteU32(0x401F8058, 0x00000000); // EMC_17
+    MEM_WriteU32(0x401F805C, 0x00000000); // EMC_18
+    MEM_WriteU32(0x401F8060, 0x00000000); // EMC_19
+    MEM_WriteU32(0x401F8064, 0x00000000); // EMC_20
+    MEM_WriteU32(0x401F8068, 0x00000000); // EMC_21
+    MEM_WriteU32(0x401F806C, 0x00000000); // EMC_22
+    MEM_WriteU32(0x401F8070, 0x00000000); // EMC_23
+    MEM_WriteU32(0x401F8074, 0x00000000); // EMC_24
+    MEM_WriteU32(0x401F8078, 0x00000000); // EMC_25
+    MEM_WriteU32(0x401F807C, 0x00000000); // EMC_26
+    MEM_WriteU32(0x401F8080, 0x00000000); // EMC_27
+    MEM_WriteU32(0x401F8084, 0x00000000); // EMC_28
+    MEM_WriteU32(0x401F8088, 0x00000000); // EMC_29
+    MEM_WriteU32(0x401F808C, 0x00000000); // EMC_30
+    MEM_WriteU32(0x401F8090, 0x00000000); // EMC_31
+    MEM_WriteU32(0x401F8094, 0x00000000); // EMC_32
+    MEM_WriteU32(0x401F8098, 0x00000000); // EMC_33
+    MEM_WriteU32(0x401F809C, 0x00000000); // EMC_34
+    MEM_WriteU32(0x401F80A0, 0x00000000); // EMC_35
+    MEM_WriteU32(0x401F80A4, 0x00000000); // EMC_36
+    MEM_WriteU32(0x401F80A8, 0x00000000); // EMC_37
+    MEM_WriteU32(0x401F80AC, 0x00000000); // EMC_38
+    MEM_WriteU32(0x401F80B0, 0x00000010); // EMC_39
+
+    // PAD ctrl
+    // drive strength = 0x7 to increase drive strength
+    // otherwise the data7 bit may fail.
+    MEM_WriteU32(0x401F8204, 0x000110F9); // EMC_00
+    MEM_WriteU32(0x401F8208, 0x000110F9); // EMC_01
+    MEM_WriteU32(0x401F820C, 0x000110F9); // EMC_02
+    MEM_WriteU32(0x401F8210, 0x000110F9); // EMC_03
+    MEM_WriteU32(0x401F8214, 0x000110F9); // EMC_04
+    MEM_WriteU32(0x401F8218, 0x000110F9); // EMC_05
+    MEM_WriteU32(0x401F821C, 0x000110F9); // EMC_06
+    MEM_WriteU32(0x401F8220, 0x000110F9); // EMC_07
+    MEM_WriteU32(0x401F8224, 0x000110F9); // EMC_08
+    MEM_WriteU32(0x401F8228, 0x000110F9); // EMC_09
+    MEM_WriteU32(0x401F822C, 0x000110F9); // EMC_10
+    MEM_WriteU32(0x401F8230, 0x000110F9); // EMC_11
+    MEM_WriteU32(0x401F8234, 0x000110F9); // EMC_12
+    MEM_WriteU32(0x401F8238, 0x000110F9); // EMC_13
+    MEM_WriteU32(0x401F823C, 0x000110F9); // EMC_14
+    MEM_WriteU32(0x401F8240, 0x000110F9); // EMC_15
+    MEM_WriteU32(0x401F8244, 0x000110F9); // EMC_16
+    MEM_WriteU32(0x401F8248, 0x000110F9); // EMC_17
+    MEM_WriteU32(0x401F824C, 0x000110F9); // EMC_18
+    MEM_WriteU32(0x401F8250, 0x000110F9); // EMC_19
+    MEM_WriteU32(0x401F8254, 0x000110F9); // EMC_20
+    MEM_WriteU32(0x401F8258, 0x000110F9); // EMC_21
+    MEM_WriteU32(0x401F825C, 0x000110F9); // EMC_22
+    MEM_WriteU32(0x401F8260, 0x000110F9); // EMC_23
+    MEM_WriteU32(0x401F8264, 0x000110F9); // EMC_24
+    MEM_WriteU32(0x401F8268, 0x000110F9); // EMC_25
+    MEM_WriteU32(0x401F826C, 0x000110F9); // EMC_26
+    MEM_WriteU32(0x401F8270, 0x000110F9); // EMC_27
+    MEM_WriteU32(0x401F8274, 0x000110F9); // EMC_28
+    MEM_WriteU32(0x401F8278, 0x000110F9); // EMC_29
+    MEM_WriteU32(0x401F827C, 0x000110F9); // EMC_30
+    MEM_WriteU32(0x401F8280, 0x000110F9); // EMC_31
+    MEM_WriteU32(0x401F8284, 0x000110F9); // EMC_32
+    MEM_WriteU32(0x401F8288, 0x000110F9); // EMC_33
+    MEM_WriteU32(0x401F828C, 0x000110F9); // EMC_34
+    MEM_WriteU32(0x401F8290, 0x000110F9); // EMC_35
+    MEM_WriteU32(0x401F8294, 0x000110F9); // EMC_36
+    MEM_WriteU32(0x401F8298, 0x000110F9); // EMC_37
+    MEM_WriteU32(0x401F829C, 0x000110F9); // EMC_38
+    MEM_WriteU32(0x401F82A0, 0x000110F9); // EMC_39
 
-  JLINK_SYS_Report("SDRAM Init Done");
+    // Config SDR Controller Registers
+    MEM_WriteU32(0x402F0000, 0x10000004); // MCR
+    MEM_WriteU32(0x402F0008, 0x00030524); // BMCR0
+    MEM_WriteU32(0x402F000C, 0x06030524); // BMCR1
+    MEM_WriteU32(0x402F0010, 0x80000019); // BR0, 16MB OK
+    MEM_WriteU32(0x402F0040, 0x00000F31); // SDRAMCR0 OK
+    MEM_WriteU32(0x402F0044, 0x00652922); // SDRAMCR1
+    MEM_WriteU32(0x402F0048, 0x000a0b0d); // SDRAMCR2
+    MEM_WriteU32(0x402F004C, 0x0f0f0a00); // SDRAMCR3
+    MEM_WriteU32(0x402F0090, 0x80000000); // IPCR0 OK
+    MEM_WriteU32(0x402F0094, 0x00000002); // IPCR1 OK
+    MEM_WriteU32(0x402F0098, 0x00000000); // IPCR2 OK
+    MEM_WriteU32(0x402F009C, 0xA55A000F); // IPCMD, SD_CC_IPREA
+    SDRAM_WaitIpCmdDone();
+    MEM_WriteU32(0x402F009C, 0xA55A000C); // SD_CC_IAF
+    SDRAM_WaitIpCmdDone();
+    MEM_WriteU32(0x402F009C, 0xA55A000C); // SD_CC_IAF
+    SDRAM_WaitIpCmdDone();
+    MEM_WriteU32(0x402F00A0, 0x00000033); // IPTXDAT OK
+    MEM_WriteU32(0x402F009C, 0xA55A000A); // SD_CC_IMS
+    SDRAM_WaitIpCmdDone();
+    MEM_WriteU32(0x402F004C, 0x08080A01); // enable sdram self refresh again after initialization done.
+
+    JLINK_SYS_Report("SDRAM Init Done");
 }
-void INTRAM_Init() {
-    unsigned int gpr14_addr;
-    unsigned int gpr16_addr;
-    unsigned int gpr17_addr;
-    unsigned int ret;
-    
-    gpr14_addr = 0x400AC038;
-    gpr16_addr = 0x400AC040;
-    gpr17_addr = 0x400AC044;
-    ret = 0;
-    
-    // 448 KBytes of DTCM
-    MEM_WriteU32(gpr17_addr,0x5AAAAAAA);
-    
-    ret = MEM_ReadU32(gpr16_addr);
-    
-    // Turn off DTCM
-    //ret &= ~0x02;
-    // Turn off ITCM
-    ret &= ~0x01;
-    MEM_WriteU32(gpr16_addr,ret);
-    
+
+void INTRAM_Init()
+{
+    unsigned int IOMUXC_GPR_GPR14;
+    unsigned int IOMUXC_GPR_GPR16;
+    unsigned int IOMUXC_GPR_GPR17;
+    unsigned int reg_val;
     
-    // Configure DTCM/ITCM size
-    ret = MEM_ReadU32(gpr14_addr);
-    // Mask ITCM/DTCM size bits
-    ret &= ~0xFF0000;
-    // Set DTCM size to 512KBytes
-    ret |= 0xA00000;
-    MEM_WriteU32(gpr14_addr,ret);
+    IOMUXC_GPR_GPR14 = 0x400AC038;
+    IOMUXC_GPR_GPR16 = 0x400AC040;
+    IOMUXC_GPR_GPR17 = 0x400AC044;
     
-    ret = MEM_ReadU32(gpr16_addr);
+    /* 448 KBytes of DTCM */
+    MEM_WriteU32(IOMUXC_GPR_GPR17, 0x5AAAAAAA);
+
+    /* Turn off ITCM */
+    reg_val = MEM_ReadU32(IOMUXC_GPR_GPR16);
+    reg_val &= ~0x00000001;
+    MEM_WriteU32(IOMUXC_GPR_GPR16, reg_val);
     
-    // FlexRAM_BANK_CFG_SEL
-    ret &= ~0x04;
-    ret |= 0x04;
-    MEM_WriteU32(gpr16_addr,ret);
+    /* Configure DTCM/ITCM size */
+    reg_val = MEM_ReadU32(IOMUXC_GPR_GPR14);
+    reg_val &= ~0x00FF0000; // Mask ITCM/DTCM size bits
+    reg_val |= 0x00A00000;  // Set DTCM size to 512KBytes
+    MEM_WriteU32(IOMUXC_GPR_GPR14, reg_val);
+
     JLINK_SYS_Report("INTRAM Init Done");
 }
 
-void WDOG_Disable(){
-    unsigned int gpio_b1_13;
+void WDOG_Disable()
+{
+    unsigned int MUX_CTL_PAD_GPIO_B1_13;
+    MUX_CTL_PAD_GPIO_B1_13 = 0x401F81B0;
+
+    /* Change pin mux from WDOG_B to GPIO to prevent resetting */
+    MEM_WriteU32(MUX_CTL_PAD_GPIO_B1_13, 0x00000005);
+}
+
+void SNVS_SSM_Init(void)
+{
+    unsigned int SNVS_HPCOMR;
+    unsigned int SNVS_HPCOMR_SW_SV_MASK;
+    unsigned int reg_val;
 
-    gpio_b1_13 = 0x401F81B0;
+    SNVS_HPCOMR = 0x400D4004;
+    SNVS_HPCOMR_SW_SV_MASK = (1 << 8);
 
-    // Switch off WDOG1
-    MEM_WriteU32(gpio_b1_13, 1);
+    /* Force enter non-secure state by setting software security violation bit */
+    reg_val = MEM_ReadU32(SNVS_HPCOMR);
+    reg_val |= SNVS_HPCOMR_SW_SV_MASK;
+    MEM_WriteU32(SNVS_HPCOMR, reg_val);
 }
 
 /* SetupTarget */
-int AfterResetTarget(void) {
-  JLINK_SYS_Report("Enabling i.MXRT SDRAM");
-  /*Load_Dcdc_Trim();*/
-  WDOG_Disable();
-  Clock_Init();
-  SDRAM_Init();
-  INTRAM_Init();
-  JLINK_SYS_Report("Enable Power Switch On for debug");
-  MEM_WriteU32(0x401BC000, 128);
+int AfterResetTarget(void)
+{
+    WDOG_Disable();
+    Clock_Init();
+    SDRAM_Init();
+    INTRAM_Init();
+    SNVS_SSM_Init();
 }
diff --git a/module-bsp/board/rt1051/bellpx/bsp/rtc/rtc_configuration.cpp b/module-bsp/board/rt1051/bellpx/bsp/rtc/rtc_configuration.cpp
index f32957d4cebc9d2ba10b351ac82c12ed03e7623a..372f7effbf96bea79073f5b8b7f812f1e3c34836 100644
--- a/module-bsp/board/rt1051/bellpx/bsp/rtc/rtc_configuration.cpp
+++ b/module-bsp/board/rt1051/bellpx/bsp/rtc/rtc_configuration.cpp
@@ -1,16 +1,16 @@
-// Copyright (c) 2017-2021, 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 <bsp/rtc/rtc.hpp>
+#include <cstdint>
 
 namespace
 {
-    static constexpr uint32_t RTC_CALIBRATION_VALUE = 0;
+    constexpr std::uint32_t RTC_CALIBRATION_VALUE = 0;
 }
 
 namespace bsp::rtc
 {
-    uint32_t getRtcCalibrationValue()
+    std::uint32_t getRtcCalibrationValue()
     {
         return RTC_CALIBRATION_VALUE;
     }
diff --git a/module-bsp/board/rt1051/bsp/rtc/rtc.cpp b/module-bsp/board/rt1051/bsp/rtc/rtc.cpp
index 099d5b4d77020f20a7df05f4068d1f877c0e3bb6..4d750a6520acf215d625ea522cfb63cc8f6532e7 100644
--- a/module-bsp/board/rt1051/bsp/rtc/rtc.cpp
+++ b/module-bsp/board/rt1051/bsp/rtc/rtc.cpp
@@ -2,62 +2,65 @@
 // For licensing, see https://github.com/mudita/MuditaOS/LICENSE.md
 
 #include "rtc_configuration.hpp"
-
 #include <bsp/rtc/rtc.hpp>
 #include <fsl_snvs_hp.h>
 #include <fsl_snvs_lp.h>
-#include <module-os/RTOSWrapper/include/ticks.hpp>
+#include <ticks.hpp>
+#include <critical.hpp>
 #include <time/time_constants.hpp>
 #include <time.h>
 #include <log/log.hpp>
 
 namespace
 {
-    constexpr std::uint32_t irqTimeout = 100000;
-
-    xQueueHandle qHandleRtcIrq = NULL;
-    snvs_hp_rtc_config_t s_rtcConfig;
+    constexpr std::uint32_t hpTimeAlarmIrqEnableTimeout = 100000;
+    xQueueHandle qHandleRtcIrq                          = nullptr;
 } // namespace
 
 namespace bsp::rtc
 {
-
     ErrorCode init(xQueueHandle qHandle)
     {
         qHandleRtcIrq = qHandle;
+
+        /* Enable HP and LP clocking */
+        CLOCK_EnableClock(kCLOCK_SnvsHp);
         CLOCK_EnableClock(kCLOCK_SnvsLp);
-        SNVS_HP_RTC_GetDefaultConfig(&s_rtcConfig);
 
-        s_rtcConfig.rtcCalValue  = getRtcCalibrationValue();
-        s_rtcConfig.rtcCalEnable = true;
-        SNVS_HP_RTC_Init(SNVS, &s_rtcConfig);
+        /* Init HP RTC */
+        snvs_hp_rtc_config_t hpRtcConfig;
+        SNVS_HP_RTC_GetDefaultConfig(&hpRtcConfig);
+        hpRtcConfig.rtcCalValue  = getRtcCalibrationValue();
+        hpRtcConfig.rtcCalEnable = true;
+        SNVS_HP_RTC_Init(SNVS, &hpRtcConfig);
 
-        (void)SNVS_LPCR_LPTA_EN(1);
+        /* Init LP SRTC */
+        snvs_lp_srtc_config_t lpRtcConfig;
+        SNVS_LP_SRTC_GetDefaultConfig(&lpRtcConfig);
+        SNVS_LP_SRTC_Init(SNVS, &lpRtcConfig);
 
-        bool timedOut     = false;
-        auto timeoutTicks = cpp_freertos::Ticks::GetTicks() + pdMS_TO_TICKS(utils::time::milisecondsInSecond);
-        const auto delay  = 10;
+        constexpr auto timeoutMs    = utils::time::milisecondsInSecond;
+        constexpr auto retryDelayMs = 10;
+        const auto initialTicks     = cpp_freertos::Ticks::GetTicks();
 
+        /* Enable LP SRTC */
         SNVS->LPCR |= SNVS_LPCR_SRTC_ENV_MASK;
-        while (!timedOut) {
-            if ((SNVS->LPCR & SNVS_LPCR_SRTC_ENV_MASK)) {
-                break;
-            }
-            timedOut = cpp_freertos::Ticks::GetTicks() > timeoutTicks;
-            if (timedOut) {
-                LOG_ERROR("RTC init timeout");
+
+        /* Wait until LP SRTC enabled */
+        while ((SNVS->LPCR & SNVS_LPCR_SRTC_ENV_MASK) == 0) {
+            if ((cpp_freertos::Ticks::GetTicks() - initialTicks) > cpp_freertos::Ticks::MsToTicks(timeoutMs)) {
+                LOG_ERROR("RTC init timeout!");
                 return ErrorCode::Error;
             }
-            vTaskDelay(delay);
+            vTaskDelay(retryDelayMs);
         }
 
         SNVS_HP_RTC_TimeSynchronize(SNVS);
 
         NVIC_SetPriority(SNVS_HP_WRAPPER_IRQn, configLIBRARY_LOWEST_INTERRUPT_PRIORITY);
-        // Enable at the NVIC
         NVIC_EnableIRQ(SNVS_HP_WRAPPER_IRQn);
 
-        // Start the timer
+        /* Enable HP RTC */
         SNVS_HP_RTC_StartTimer(SNVS);
 
         LOG_INFO("RTC configured successfully");
@@ -66,21 +69,17 @@ namespace bsp::rtc
 
     ErrorCode setDateTimeFromTimestamp(time_t timestamp)
     {
-        portENTER_CRITICAL();
-
-        SNVS_LP_SRTC_StopTimer(SNVS);
+        cpp_freertos::CriticalSection::Enter();
         SNVS_LP_SRTC_SetSeconds(SNVS, static_cast<std::uint32_t>(timestamp));
-        SNVS_LP_SRTC_StartTimer(SNVS);
         SNVS_HP_RTC_TimeSynchronize(SNVS);
-
-        portEXIT_CRITICAL();
+        cpp_freertos::CriticalSection::Exit();
 
         return ErrorCode::OK;
     }
 
     ErrorCode setDateTime(struct tm *time)
     {
-        if (time == NULL) {
+        if (time == nullptr) {
             return ErrorCode::Error;
         }
 
@@ -93,19 +92,17 @@ namespace bsp::rtc
         rtcDate.minute = time->tm_min;
         rtcDate.second = time->tm_sec;
 
-        portENTER_CRITICAL();
-        SNVS_LP_SRTC_StopTimer(SNVS);
+        cpp_freertos::CriticalSection::Enter();
         SNVS_LP_SRTC_SetDatetime(SNVS, &rtcDate);
-        SNVS_LP_SRTC_StartTimer(SNVS);
         SNVS_HP_RTC_TimeSynchronize(SNVS);
-        portEXIT_CRITICAL();
+        cpp_freertos::CriticalSection::Exit();
 
         return ErrorCode::OK;
     }
 
     ErrorCode getCurrentDateTime(struct tm *datetime)
     {
-        if (datetime == NULL) {
+        if (datetime == nullptr) {
             return ErrorCode::Error;
         }
 
@@ -125,18 +122,18 @@ namespace bsp::rtc
 
     ErrorCode getCurrentTimestamp(time_t *timestamp)
     {
-        if (timestamp == NULL) {
+        if (timestamp == nullptr) {
             return ErrorCode::Error;
         }
 
-        *timestamp = static_cast<time_t>(SNVS_HP_RTC_GetSeconds(SNVS));
+        *timestamp = static_cast<std::time_t>(SNVS_HP_RTC_GetSeconds(SNVS));
 
         return ErrorCode::OK;
     }
 
     ErrorCode setAlarmOnDate(struct tm *datetime)
     {
-        if (datetime == NULL) {
+        if (datetime == nullptr) {
             return ErrorCode::Error;
         }
 
@@ -149,10 +146,11 @@ namespace bsp::rtc
         rtcDate.minute = datetime->tm_min;
         rtcDate.second = datetime->tm_sec;
 
-        enableAlarmIrq();
-        SNVS_HP_RTC_SetAlarm(SNVS, &rtcDate);
+        if (SNVS_HP_RTC_SetAlarm(SNVS, &rtcDate) != kStatus_Success) {
+            return ErrorCode::Error;
+        }
 
-        return ErrorCode::OK;
+        return enableAlarmIrq();
     }
 
     ErrorCode setAlarmOnTimestamp(std::uint32_t secs)
@@ -161,9 +159,7 @@ namespace bsp::rtc
             return ErrorCode::Error;
         }
 
-        enableAlarmIrq();
-
-        return ErrorCode::OK;
+        return enableAlarmIrq();
     }
 
     ErrorCode setAlarmInSecondsFromNow(std::uint32_t secs)
@@ -171,18 +167,16 @@ namespace bsp::rtc
         std::uint32_t seconds = SNVS_HP_RTC_GetSeconds(SNVS);
         seconds += secs;
 
-        enableAlarmIrq();
-
         if (SNVS_HP_RTC_SetAlarmSeconds(SNVS, seconds) != kStatus_Success) {
             return ErrorCode::Error;
         }
 
-        return ErrorCode::OK;
+        return enableAlarmIrq();
     }
 
     ErrorCode getAlarmTimestamp(std::uint32_t *secs)
     {
-        if (secs == NULL) {
+        if (secs == nullptr) {
             return ErrorCode::Error;
         }
 
@@ -193,14 +187,15 @@ namespace bsp::rtc
 
     ErrorCode enableAlarmIrq()
     {
-        std::uint32_t cnt = irqTimeout;
-        SNVS->HPCR |= SNVS_HPCR_HPTA_EN_MASK;
-        while ((!(SNVS->HPCR & SNVS_HPCR_HPTA_EN_MASK)) && cnt) {
-            cnt--;
-        }
+        std::uint32_t timeout = hpTimeAlarmIrqEnableTimeout;
 
-        if (cnt == 0) {
-            return ErrorCode::Error;
+        SNVS->HPCR |= SNVS_HPCR_HPTA_EN_MASK;
+        while ((SNVS->HPCR & SNVS_HPCR_HPTA_EN_MASK) == 0) {
+            --timeout;
+            if (timeout == 0) {
+                LOG_ERROR("Failed to enable alarm IRQ!");
+                return ErrorCode::Error;
+            }
         }
 
         return ErrorCode::OK;
@@ -208,15 +203,15 @@ namespace bsp::rtc
 
     ErrorCode disableAlarmIrq()
     {
-        std::uint32_t cnt = irqTimeout;
+        std::uint32_t timeout = hpTimeAlarmIrqEnableTimeout;
 
         SNVS->HPCR &= ~SNVS_HPCR_HPTA_EN_MASK;
-        while ((SNVS->HPCR & SNVS_HPCR_HPTA_EN_MASK) && cnt) {
-            cnt--;
-        }
-
-        if (cnt == 0) {
-            return ErrorCode::Error;
+        while ((SNVS->HPCR & SNVS_HPCR_HPTA_EN_MASK) != 0) {
+            --timeout;
+            if (timeout == 0) {
+                LOG_ERROR("Failed to disable alarm IRQ!");
+                return ErrorCode::Error;
+            }
         }
 
         return ErrorCode::OK;
@@ -236,7 +231,7 @@ namespace bsp::rtc
 
     ErrorCode setMinuteAlarm(time_t timestamp)
     {
-        std::uint32_t secondsToMinute = 60 - (timestamp % 60);
+        const auto secondsToMinute = 60 - (timestamp % 60);
 
         struct tm date
         {};
diff --git a/module-bsp/board/rt1051/common/board.cpp b/module-bsp/board/rt1051/common/board.cpp
index 113172cce7af7a5d39ed177b4135eabc3b492107..1a94d3b4162f7c56e705f66b7f4da08dd290c2b6 100644
--- a/module-bsp/board/rt1051/common/board.cpp
+++ b/module-bsp/board/rt1051/common/board.cpp
@@ -171,7 +171,6 @@ namespace bsp
         Brownout_init();
         irq_gpio_Init();
 
-        // SNVS init. is required for proper operation of the RTC when Secure Boot is used
         SNVS_LP_Init(SNVS);
         SNVS_HP_Init(SNVS);
         SNVS_HP_ChangeSSMState(SNVS);
@@ -210,8 +209,10 @@ namespace bsp
 
     int register_exit_functions(void (*func)())
     {
-        if (iObject >= sizeof(objects))
+        if (iObject >= sizeof(objects)) {
             return -1;
+        }
+
         objects[iObject].func = func;
         ++iObject;
         return 0;
diff --git a/module-bsp/bsp/rtc/rtc.hpp b/module-bsp/bsp/rtc/rtc.hpp
index cfd2f6a51b63b0ee76a92e7f07d22248df1ec7c5..7cdc91072c89cde8f747ba22b7cc5d33a23c9aa7 100644
--- a/module-bsp/bsp/rtc/rtc.hpp
+++ b/module-bsp/bsp/rtc/rtc.hpp
@@ -8,7 +8,8 @@
 #include <ctime>
 #include <cstdint>
 
-namespace bsp::rtc {
+namespace bsp::rtc
+{
 	enum class IrqNotification
     {
         AlarmOccurred = 0x01