Merge pull request #108 from Horizon-OC/bpmpfw

hocclk: add BPMP-Lite firmware

Source/Atmosphere-Patches/secmon_smc_power_management.cpp

@@ -0,0 +1,571 @@
+/*
+ * Copyright (c) Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <exosphere.hpp>
+#include "../secmon_cache.hpp"
+#include "../secmon_cpu_context.hpp"
+#include "../secmon_error.hpp"
+#include "../secmon_misc.hpp"
+#include "secmon_smc_power_management.hpp"
+#include "secmon_smc_se_lock.hpp"
+
+#include "sc7fw_bin.h"
+
+namespace ams::secmon {
+
+    /* Declare assembly functionality. */
+    void *GetCoreExceptionStackVirtual();
+
+}
+
+namespace ams::secmon::smc {
+
+    /* Declare assembly power-management functionality. */
+    void PivotStackAndInvoke(void *stack, void (*function)());
+    void FinalizePowerOff();
+
+    namespace {
+
+        constexpr inline const uintptr_t PMC       = MemoryRegionVirtualDevicePmc.GetAddress();
+        constexpr inline const uintptr_t APB_MISC  = MemoryRegionVirtualDeviceApbMisc.GetAddress();
+        constexpr inline const uintptr_t GPIO      = MemoryRegionVirtualDeviceGpio.GetAddress();
+        constexpr inline const uintptr_t CLK_RST   = MemoryRegionVirtualDeviceClkRst.GetAddress();
+        constexpr inline const uintptr_t EVP       = secmon::MemoryRegionVirtualDeviceExceptionVectors.GetAddress();
+        constexpr inline const uintptr_t FLOW_CTLR = MemoryRegionVirtualDeviceFlowController.GetAddress();
+        constexpr inline const uintptr_t AHB_ARBC  = MemoryRegionVirtualDeviceSystem.GetAddress();
+
+        constexpr inline uintptr_t CommonSmcStackTop = MemoryRegionVirtualTzramVolatileData.GetEndAddress() - (0x80 * (NumCores - 1));
+
+        enum PowerStateType {
+            PowerStateType_StandBy   = 0,
+            PowerStateType_PowerDown = 1,
+        };
+
+        enum PowerStateId {
+            PowerStateId_Sc7 = 27,
+        };
+
+        /* http://infocenter.arm.com/help/topic/com.arm.doc.den0022d/Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf Page 46 */
+        struct SuspendCpuPowerState {
+            using StateId    = util::BitPack32::Field< 0, 16, PowerStateId>;
+            using StateType  = util::BitPack32::Field<16,  1, PowerStateType>;
+            using PowerLevel = util::BitPack32::Field<24,  2, u32>;
+        };
+
+        constinit bool g_charger_hi_z_mode_enabled = false;
+
+        constinit const reg::BitsMask CpuPowerGateStatusMasks[NumCores] = {
+            PMC_REG_BITS_MASK(PWRGATE_STATUS_CE0),
+            PMC_REG_BITS_MASK(PWRGATE_STATUS_CE1),
+            PMC_REG_BITS_MASK(PWRGATE_STATUS_CE2),
+            PMC_REG_BITS_MASK(PWRGATE_STATUS_CE3),
+        };
+
+        constinit const APBDEV_PMC_PWRGATE_TOGGLE_PARTID CpuPowerGateTogglePartitionIds[NumCores] = {
+            APBDEV_PMC_PWRGATE_TOGGLE_PARTID_CE0,
+            APBDEV_PMC_PWRGATE_TOGGLE_PARTID_CE1,
+            APBDEV_PMC_PWRGATE_TOGGLE_PARTID_CE2,
+            APBDEV_PMC_PWRGATE_TOGGLE_PARTID_CE3,
+        };
+
+        bool IsCpuPoweredOn(const reg::BitsMask mask) {
+            return reg::HasValue(PMC + APBDEV_PMC_PWRGATE_STATUS, REG_BITS_VALUE_FROM_MASK(mask, APBDEV_PMC_PWRGATE_STATUS_STATUS_ON));
+        }
+
+        void PowerOnCpu(const reg::BitsMask mask, u32 toggle_partid) {
+            /* If the cpu is already on, we have nothing to do. */
+            if (IsCpuPoweredOn(mask)) {
+                return;
+            }
+
+            /* Wait until nothing is being powergated. */
+            int timeout = 5000;
+            while (true) {
+                if (reg::HasValue(PMC + APBDEV_PMC_PWRGATE_TOGGLE, PMC_REG_BITS_ENUM(PWRGATE_TOGGLE_START, DISABLE))) {
+                    break;
+                }
+
+                util::WaitMicroSeconds(1);
+                if ((--timeout) < 0) {
+                    /* NOTE: Nintendo doesn't do any error handling here... */
+                    return;
+                }
+            }
+
+            /* Toggle on the cpu partition. */
+            reg::Write(PMC + APBDEV_PMC_PWRGATE_TOGGLE, PMC_REG_BITS_ENUM (PWRGATE_TOGGLE_START,         ENABLE),
+                                                        PMC_REG_BITS_VALUE(PWRGATE_TOGGLE_PARTID, toggle_partid));
+
+            /* Wait up to 5000 us for the powergate to complete. */
+            timeout = 5000;
+            while (true) {
+                if (IsCpuPoweredOn(mask)) {
+                    break;
+                }
+
+                util::WaitMicroSeconds(1);
+                if ((--timeout) < 0) {
+                    /* NOTE: Nintendo doesn't do any error handling here... */
+                    return;
+                }
+            }
+        }
+
+        void ResetCpu(int which_core) {
+            reg::Write(CLK_RST + CLK_RST_CONTROLLER_RST_CPUG_CMPLX_SET, REG_BITS_VALUE(which_core + 0x00, 1, 1),  /* CPURESETn */
+                                                                        REG_BITS_VALUE(which_core + 0x10, 1, 1)); /* CORERESETn */
+        }
+
+        void StartCpu(int which_core) {
+            reg::Write(CLK_RST + CLK_RST_CONTROLLER_RST_CPUG_CMPLX_CLR, REG_BITS_VALUE(which_core + 0x00, 1, 1),  /* CPURESETn */
+                                                                        REG_BITS_VALUE(which_core + 0x10, 1, 1)); /* CORERESETn */
+        }
+
+        void PowerOffCpuImpl() {
+            /* Get the current core id. */
+            const auto core_id = hw::GetCurrentCoreId();
+
+            /* Configure the flow controller to prepare for shutting down the current core. */
+            flow::SetCpuCsr(core_id, FLOW_CTLR_CPUN_CSR_ENABLE_EXT_POWERGATE_CPU_ONLY);
+            flow::SetHaltCpuEvents(core_id, false);
+            flow::SetCc4Ctrl(core_id, 0);
+
+            /* Save the core's context for restoration on next power-on. */
+            SaveDebugRegisters();
+            SetCoreOff();
+
+            /* Ensure there are no pending memory transactions prior to our power-down. */
+            FlushEntireDataCache();
+
+            /* Finalize our powerdown and wait for an interrupt. */
+            FinalizePowerOff();
+        }
+
+        void ValidateSocStateForSuspend() {
+            /* Validate that all other cores are off. */
+            AMS_ABORT_UNLESS(reg::HasValue(PMC + APBDEV_PMC_PWRGATE_STATUS, PMC_REG_BITS_VALUE(PWRGATE_STATUS_CE123, 0)));
+
+            /* Horizon-OC: Halt BPMP here as hoc-clk may be running it, and if it's running there will be a panic. */
+            const bool jtag = IsJtagEnabled();
+            reg::Write(FLOW_CTLR + FLOW_CTLR_HALT_COP_EVENTS, FLOW_REG_BITS_ENUM    (HALT_COP_EVENTS_MODE, FLOW_MODE_STOP),
+                                                               FLOW_REG_BITS_ENUM_SEL(HALT_COP_EVENTS_JTAG, jtag, ENABLED, DISABLED));
+
+            /* Validate that the bpmp is appropriately halted. */
+            AMS_ABORT_UNLESS(reg::Read(FLOW_CTLR + FLOW_CTLR_HALT_COP_EVENTS) == reg::Encode(FLOW_REG_BITS_ENUM    (HALT_COP_EVENTS_MODE, FLOW_MODE_STOP),
+                                                                                             FLOW_REG_BITS_ENUM_SEL(HALT_COP_EVENTS_JTAG, jtag, ENABLED, DISABLED)));
+
+            /* Further validations aren't guaranteed on < 6.0.0. */
+            if (GetTargetFirmware() < TargetFirmware_6_0_0) {
+                return;
+            }
+
+            /* Validate that USB2, APB-DMA, AHB-DMA are held in reset. */
+            AMS_ABORT_UNLESS(reg::HasValue(CLK_RST + CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_REG_BITS_ENUM(RST_DEVICES_H_SWR_USB2_RST,   ENABLE),
+                                                                                       CLK_RST_REG_BITS_ENUM(RST_DEVICES_H_SWR_APBDMA_RST, ENABLE),
+                                                                                       CLK_RST_REG_BITS_ENUM(RST_DEVICES_H_SWR_AHBDMA_RST, ENABLE)));
+
+            /* Validate that USBD is held in reset. */
+            AMS_ABORT_UNLESS(reg::HasValue(CLK_RST + CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_REG_BITS_ENUM(RST_DEVICES_L_SWR_USBD_RST, ENABLE)));
+
+            /* Validate that AHB-DMA, USB, USB2, COP are not allowed to arbitrate on the AHB. */
+            AMS_ABORT_UNLESS(reg::HasValue(AHB_ARBC + AHB_ARBITRATION_DISABLE, AHB_REG_BITS_ENUM(ARBITRATION_DISABLE_COP,    DISABLE),
+                                                                               AHB_REG_BITS_ENUM(ARBITRATION_DISABLE_AHBDMA, DISABLE),
+                                                                               AHB_REG_BITS_ENUM(ARBITRATION_DISABLE_USB,    DISABLE),
+                                                                               AHB_REG_BITS_ENUM(ARBITRATION_DISABLE_USB2,   DISABLE)));
+
+            /* Validate that the GPIO controller has clock enabled. */
+            AMS_ABORT_UNLESS(reg::HasValue(CLK_RST + CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_L_CLK_ENB_GPIO, ENABLE)));
+
+            /* Validate that both FUSE and KFUSE have clock enabled. */
+            AMS_ABORT_UNLESS(reg::HasValue(CLK_RST + CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_H_CLK_ENB_FUSE,  ENABLE),
+                                                                                       CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_H_CLK_ENB_KFUSE, ENABLE)));
+
+            /* Validate that all of IRAM has clock enabled. */
+            AMS_ABORT_UNLESS(reg::HasValue(CLK_RST + CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_U_CLK_ENB_IRAMA, ENABLE),
+                                                                                       CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_U_CLK_ENB_IRAMB, ENABLE),
+                                                                                       CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_U_CLK_ENB_IRAMC, ENABLE),
+                                                                                       CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_U_CLK_ENB_IRAMD, ENABLE)));
+
+            /* Validate that ACTMON has clock enabled. */
+            AMS_ABORT_UNLESS(reg::HasValue(CLK_RST + CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_V_CLK_ENB_ACTMON, ENABLE)));
+
+            /* Validate that ENTROPY has clock enabled. */
+            AMS_ABORT_UNLESS(reg::HasValue(CLK_RST + CLK_RST_CONTROLLER_CLK_OUT_ENB_W, CLK_RST_REG_BITS_ENUM(CLK_OUT_ENB_W_CLK_ENB_ENTROPY, ENABLE)));
+        }
+
+        void GenerateCryptographicallyRandomBytes(void * const dst, int size) {
+            /* Flush the region we're about to fill to ensure consistency with the SE. */
+            hw::FlushDataCache(dst, size);
+            hw::DataSynchronizationBarrierInnerShareable();
+
+            /* Generate random bytes. */
+            se::GenerateRandomBytes(dst, size);
+            hw::DataSynchronizationBarrierInnerShareable();
+
+            /* Flush to ensure the CPU sees consistent data for the region. */
+            hw::FlushDataCache(dst, size);
+            hw::DataSynchronizationBarrierInnerShareable();
+        }
+
+        void SaveSecureContextForErista() {
+            /* Generate a random key source. */
+            util::AlignedBuffer<hw::DataCacheLineSize, se::AesBlockSize> key_source;
+            GenerateCryptographicallyRandomBytes(key_source, se::AesBlockSize);
+
+            const u32 * const key_source_32 = reinterpret_cast<const u32 *>(static_cast<u8 *>(key_source));
+
+            /* Ensure that the key source registers are not locked. */
+            AMS_ABORT_UNLESS(pmc::GetSecureRegisterLockState(pmc::SecureRegister_KeySourceReadWrite) != pmc::LockState::Locked);
+
+            /* Write the key source, lock writes to the key source, and verify that the key source is write-locked. */
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH24, key_source_32[0]);
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH25, key_source_32[1]);
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH26, key_source_32[2]);
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH27, key_source_32[3]);
+            pmc::LockSecureRegister(pmc::SecureRegister_KeySourceWrite);
+            AMS_ABORT_UNLESS(pmc::GetSecureRegisterLockState(pmc::SecureRegister_KeySourceWrite) == pmc::LockState::Locked);
+
+            /* Verify the key source is correct in registers, and read-lock the key source registers. */
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH24) == key_source_32[0]);
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH25) == key_source_32[1]);
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH26) == key_source_32[2]);
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH27) == key_source_32[3]);
+            pmc::LockSecureRegister(pmc::SecureRegister_KeySourceRead);
+
+            /* Ensure that the key source registers are locked. */
+            AMS_ABORT_UNLESS(pmc::GetSecureRegisterLockState(pmc::SecureRegister_KeySourceReadWrite) == pmc::LockState::Locked);
+
+            /* Generate a random kek into keyslot 2. */
+            se::SetRandomKey(pkg1::AesKeySlot_TzramSaveKek);
+
+            /* Verify that the se is in a validate state, context save, and validate again. */
+            {
+                se::ValidateErrStatus();
+                ON_SCOPE_EXIT { se::ValidateErrStatus(); };
+
+                {
+                    /* Transition to non-secure mode for the duration of the context save operation. */
+                    se::SetSecure(false);
+                    ON_SCOPE_EXIT { se::SetSecure(true); };
+
+                    /* Get a pointer to the context storage. */
+                    se::Context * const context = MemoryRegionVirtualDramSecureDataStoreSecurityEngineState.GetPointer<se::Context>();
+                    static_assert(MemoryRegionVirtualDramSecureDataStoreSecurityEngineState.GetSize() == sizeof(*context));
+
+                    /* Save the context. */
+                    se::SaveContext(context);
+
+                    /* Ensure that the cpu sees consistent data. */
+                    hw::FlushDataCache(context, sizeof(*context));
+                    hw::DataSynchronizationBarrierInnerShareable();
+
+                    /* Write the context pointer to pmc scratch, so that the bootrom will restore it on wake. */
+                    reg::Write(PMC + APBDEV_PMC_SCRATCH43, MemoryRegionPhysicalDramSecureDataStoreSecurityEngineState.GetAddress());
+                }
+            }
+
+            /* Clear keyslot 3, and then derive the save key. */
+            se::ClearAesKeySlot(pkg1::AesKeySlot_TzramSaveKey);
+            se::SetEncryptedAesKey256(pkg1::AesKeySlot_TzramSaveKey, pkg1::AesKeySlot_TzramSaveKek, key_source, se::AesBlockSize);
+
+            /* Declare a temporary block to be used as both iv and mac. */
+            u32 temp_block[se::AesBlockSize / sizeof(u32)] = {};
+
+            /* Ensure that the SE sees consistent data for tzram. */
+            const void * const tzram_save_src = MemoryRegionVirtualTzramReadOnlyAlias.GetPointer<u8>() + MemoryRegionVirtualTzramVolatileData.GetSize() + MemoryRegionVirtualTzramVolatileStack.GetSize();
+                  void * const tzram_save_dst = MemoryRegionVirtualIramSc7Work.GetPointer<void>();
+            constexpr size_t TzramSaveSize    = MemoryRegionVirtualDramSecureDataStoreTzram.GetSize();
+
+            hw::FlushDataCache(tzram_save_src, TzramSaveSize);
+            hw::FlushDataCache(tzram_save_dst, TzramSaveSize);
+            hw::DataSynchronizationBarrierInnerShareable();
+
+            /* Encrypt tzram using our random key. */
+            se::EncryptAes256Cbc(tzram_save_dst, TzramSaveSize, pkg1::AesKeySlot_TzramSaveKey, tzram_save_src, TzramSaveSize, temp_block, se::AesBlockSize);
+            hw::FlushDataCache(tzram_save_dst, TzramSaveSize);
+            hw::DataSynchronizationBarrierInnerShareable();
+
+            /* Copy the data from work space to the secure storage destination. */
+            void * const tzram_store_dst = MemoryRegionVirtualDramSecureDataStoreTzram.GetPointer<void>();
+            std::memcpy(tzram_store_dst, tzram_save_dst, TzramSaveSize);
+            hw::FlushDataCache(tzram_store_dst, TzramSaveSize);
+            hw::DataSynchronizationBarrierInnerShareable();
+
+            /* Compute cmac of tzram into our temporary block. */
+            se::ComputeAes256Cmac(temp_block, se::AesBlockSize, pkg1::AesKeySlot_TzramSaveKey, tzram_save_src, TzramSaveSize);
+
+            /* Ensure that the cmac registers are not locked. */
+            AMS_ABORT_UNLESS(pmc::GetSecureRegisterLockState(pmc::SecureRegister_CmacReadWrite) != pmc::LockState::Locked);
+
+            /* Write the cmac, lock writes to the cmac, and verify that the cmac is write-locked. */
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH112, temp_block[0]);
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH113, temp_block[1]);
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH114, temp_block[2]);
+            reg::Write(PMC + APBDEV_PMC_SECURE_SCRATCH115, temp_block[3]);
+            pmc::LockSecureRegister(pmc::SecureRegister_CmacWrite);
+            AMS_ABORT_UNLESS(pmc::GetSecureRegisterLockState(pmc::SecureRegister_CmacWrite) == pmc::LockState::Locked);
+
+            /* Verify the key source is correct in registers, and read-lock the key source registers. */
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH112) == temp_block[0]);
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH113) == temp_block[1]);
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH114) == temp_block[2]);
+            AMS_ABORT_UNLESS(reg::Read(PMC + APBDEV_PMC_SECURE_SCRATCH115) == temp_block[3]);
+            pmc::LockSecureRegister(pmc::SecureRegister_CmacRead);
+
+            /* Ensure that the key source registers are locked. */
+            AMS_ABORT_UNLESS(pmc::GetSecureRegisterLockState(pmc::SecureRegister_CmacReadWrite) == pmc::LockState::Locked);
+        }
+
+        void SaveSecureContextForMariko() {
+            /* Save security engine context to TZRAM SE carveout (inaccessible to cpu). */
+            se::SaveContextAutomatic();
+
+            /* Save TZRAM to shadow-TZRAM in always-on power domain. */
+            se::SaveTzramAutomatic();
+        }
+
+        void SaveSecureContext() {
+            /* Save the appropriate secure context. */
+            const auto soc_type = GetSocType();
+            if (soc_type == fuse::SocType_Erista) {
+                SaveSecureContextForErista();
+            } else /* if (soc_type == fuse::SocType_Mariko) */ {
+                SaveSecureContextForMariko();
+            }
+
+            /* Save the debug code. */
+            #if defined(AMS_BUILD_FOR_DEBUGGING) || defined(AMS_BUILD_FOR_AUDITING)
+            {
+                const void * const debug_code_src = MemoryRegionVirtualDebugCode.GetPointer<void>();
+                      void * const debug_code_dst = MemoryRegionVirtualDramDebugDataStore.GetPointer<void>();
+                std::memcpy(debug_code_dst, debug_code_src, MemoryRegionVirtualDebugCode.GetSize());
+                hw::FlushDataCache(debug_code_dst, MemoryRegionVirtualDebugCode.GetSize());
+                hw::DataSynchronizationBarrierInnerShareable();
+            }
+            #endif
+        }
+
+        void LoadAndStartSc7BpmpFirmware() {
+            /* Set BPMP reset. */
+            reg::Write(CLK_RST + CLK_RST_CONTROLLER_RST_DEV_L_SET, CLK_RST_REG_BITS_ENUM(RST_DEV_L_SET_SET_COP_RST, ENABLE));
+
+            /* Set the PMC as insecure, so that the BPMP firmware can access it. */
+            reg::ReadWrite(APB_MISC + APB_MISC_SECURE_REGS_APB_SLAVE_SECURITY_ENABLE_REG0_0, SLAVE_SECURITY_REG_BITS_ENUM(0, PMC, DISABLE));
+
+            /* Set the exception vectors for the bpmp. RESET should point to RESET, all others should point to generic exception/panic. */
+            constexpr const u32 Sc7FirmwareResetVector = static_cast<u32>(MemoryRegionPhysicalIramSc7Firmware.GetAddress() + 0x0);
+            constexpr const u32 Sc7FirmwarePanicVector = static_cast<u32>(MemoryRegionPhysicalIramSc7Firmware.GetAddress() + 0x4);
+
+            reg::Write(EVP + EVP_COP_RESET_VECTOR,          Sc7FirmwareResetVector);
+            reg::Write(EVP + EVP_COP_UNDEF_VECTOR,          Sc7FirmwarePanicVector);
+            reg::Write(EVP + EVP_COP_SWI_VECTOR,            Sc7FirmwarePanicVector);
+            reg::Write(EVP + EVP_COP_PREFETCH_ABORT_VECTOR, Sc7FirmwarePanicVector);
+            reg::Write(EVP + EVP_COP_DATA_ABORT_VECTOR,     Sc7FirmwarePanicVector);
+            reg::Write(EVP + EVP_COP_RSVD_VECTOR,           Sc7FirmwarePanicVector);
+            reg::Write(EVP + EVP_COP_IRQ_VECTOR,            Sc7FirmwarePanicVector);
+            reg::Write(EVP + EVP_COP_FIQ_VECTOR,            Sc7FirmwarePanicVector);
+
+            /* Disable activity monitor bpmp monitoring, so that we don't panic upon bpmp wake. */
+            actmon::StopMonitoringBpmp();
+
+            /* Load the bpmp firmware. */
+            void * const sc7fw_load_address = MemoryRegionVirtualIramSc7Firmware.GetPointer<void>();
+            std::memcpy(sc7fw_load_address, sc7fw_bin, sc7fw_bin_size);
+            hw::FlushDataCache(sc7fw_load_address, sc7fw_bin_size);
+            hw::DataSynchronizationBarrierInnerShareable();
+
+            /* Ensure that the bpmp firmware was loaded. */
+            AMS_ABORT_UNLESS(crypto::IsSameBytes(sc7fw_load_address, sc7fw_bin, sc7fw_bin_size));
+
+            /* Clear BPMP reset. */
+            reg::Write(CLK_RST + CLK_RST_CONTROLLER_RST_DEV_L_CLR, CLK_RST_REG_BITS_ENUM(RST_DEV_L_CLR_CLR_COP_RST, ENABLE));
+
+            /* Start the bpmp. */
+            reg::Write(FLOW_CTLR + FLOW_CTLR_HALT_COP_EVENTS, FLOW_REG_BITS_ENUM(HALT_COP_EVENTS_MODE, FLOW_MODE_NONE));
+        }
+
+        void SaveSecureContextAndSuspend() {
+            /* Ensure there are no pending memory transactions before we continue */
+            FlushEntireDataCache();
+            hw::DataSynchronizationBarrierInnerShareable();
+
+            /* Save all secure context (security engine state + tzram). */
+            SaveSecureContext();
+
+            /* Load and start the sc7 firmware on the bpmp. */
+            if (GetTargetFirmware() >= TargetFirmware_2_0_0) {
+                LoadAndStartSc7BpmpFirmware();
+            }
+
+            /* Log our suspension. */
+            /* NOTE: Nintendo only does this on dev, but we will always do it. */
+            if (true /* !pkg1::IsProduction() */) {
+                log::Initialize(secmon::GetLogPort(), secmon::GetLogBaudRate(), secmon::GetLogFlags());
+                log::SendText("OYASUMI\n", 8);
+                log::Flush();
+            }
+
+            /* If we're on erista, configure the bootrom to allow our custom warmboot firmware. */
+            if (GetSocType() == fuse::SocType_Erista) {
+                reg::Write(PMC + APBDEV_PMC_SCRATCH31, 0x2202E012);
+                reg::Write(PMC + APBDEV_PMC_SCRATCH32, 0x6001DC28);
+            }
+
+            /* Finalize our powerdown and wait for an interrupt. */
+            FinalizePowerOff();
+        }
+
+        SmcResult SuspendCpuImpl(SmcArguments &args) {
+            /* Decode arguments. */
+            const util::BitPack32 power_state = { static_cast<u32>(args.r[1]) };
+            const uintptr_t       entry_point = args.r[2];
+            const uintptr_t       context_id  = args.r[3];
+
+            const auto state_type = power_state.Get<SuspendCpuPowerState::StateType>();
+            const auto state_id   = power_state.Get<SuspendCpuPowerState::StateId>();
+
+            const auto core_id = hw::GetCurrentCoreId();
+
+            /* Validate arguments. */
+            SMC_R_UNLESS(state_type == PowerStateType_PowerDown, PsciDenied);
+            SMC_R_UNLESS(state_id   == PowerStateId_Sc7,         PsciDenied);
+
+            /* Orchestrate charger transition to Hi-Z mode if needed. */
+            if (IsChargerHiZModeEnabled()) {
+                /* Ensure we can do comms over i2c-1. */
+                clkrst::EnableI2c1Clock();
+
+                /* If the charger isn't in hi-z mode, perform a transition. */
+                if (!charger::IsHiZMode()) {
+                    charger::EnterHiZMode();
+
+                    /* Wait up to 50ms for the transition to complete. */
+                    const auto start_time = util::GetMicroSeconds();
+                    auto current_time = start_time;
+                    while ((current_time - start_time) <= 50'000) {
+                        if (auto intr_status = reg::Read(GPIO + 0x634); (intr_status & 1) == 0) {
+                            /* Wait 256 us to ensure the transition completes. */
+                            util::WaitMicroSeconds(256);
+                            break;
+                        }
+                        current_time = util::GetMicroSeconds();
+                    }
+                }
+
+                /* Disable i2c-1, since we're done communicating over it. */
+                clkrst::DisableI2c1Clock();
+            }
+
+            /* Enable wake event detection. */
+            pmc::EnableWakeEventDetection();
+
+            /* Ensure that i2c-5 is usable for communicating with the pmic. */
+            clkrst::EnableI2c5Clock();
+            i2c::Initialize(i2c::Port_5);
+
+            /* Orchestrate sleep entry with the pmic. */
+            pmic::EnableSleep();
+
+            /* Ensure that the soc is in a state valid for us to suspend. */
+            if (GetTargetFirmware() >= TargetFirmware_2_0_0) {
+                ValidateSocStateForSuspend();
+            }
+
+            /* Configure the pmc for sc7 entry. */
+            pmc::ConfigureForSc7Entry();
+
+            /* Configure the flow controller for sc7 entry. */
+            flow::SetCc4Ctrl(core_id, 0);
+            flow::SetHaltCpuEvents(core_id, false);
+            flow::ClearL2FlushControl();
+            flow::SetCpuCsr(core_id, FLOW_CTLR_CPUN_CSR_ENABLE_EXT_POWERGATE_CPU_TURNOFF_CPURAIL);
+
+            /* Save the entry context. */
+            SetEntryContext(core_id, entry_point, context_id);
+
+            /* Configure the cpu context for reset. */
+            SaveDebugRegisters();
+            SetCoreOff();
+            SetResetExpected(true);
+
+            /* Switch to use the common smc stack (all other cores are off), and perform suspension. */
+            PivotStackAndInvoke(reinterpret_cast<void *>(CommonSmcStackTop), SaveSecureContextAndSuspend);
+
+            /* This code will never be reached. */
+            __builtin_unreachable();
+        }
+
+    }
+
+    void PowerOffCpu() {
+        /* Get the current core id. */
+        const auto core_id = hw::GetCurrentCoreId();
+
+        /* Note that we're expecting a reset for the current core. */
+        SetResetExpected(true);
+
+        /* If we're on the final core, shut down directly. Otherwise, invoke with special stack. */
+        if (core_id == NumCores - 1) {
+            PowerOffCpuImpl();
+        } else {
+            PivotStackAndInvoke(GetCoreExceptionStackVirtual(), PowerOffCpuImpl);
+        }
+
+        /* This code will never be reached. */
+        __builtin_unreachable();
+    }
+
+    SmcResult SmcPowerOffCpu(SmcArguments &args) {
+        AMS_UNUSED(args);
+
+        PowerOffCpu();
+    }
+
+    SmcResult SmcPowerOnCpu(SmcArguments &args) {
+        /* Get and validate the core to power on. */
+        const int which_core = args.r[1];
+        SMC_R_UNLESS(0 <= which_core && which_core < NumCores, PsciInvalidParameters);
+
+        /* Ensure the core isn't already on. */
+        SMC_R_UNLESS(!IsCoreOn(which_core), PsciAlreadyOn);
+
+        /* Save the entry context. */
+        SetEntryContext(which_core, args.r[2], args.r[3]);
+
+        /* Reset the cpu. */
+        ResetCpu(which_core);
+
+        /* Turn on the core. */
+        PowerOnCpu(CpuPowerGateStatusMasks[which_core], CpuPowerGateTogglePartitionIds[which_core]);
+
+        /* Start the core. */
+        StartCpu(which_core);
+
+        return SmcResult::PsciSuccess;
+    }
+
+    SmcResult SmcSuspendCpu(SmcArguments &args) {
+        return LockSecurityEngineAndInvoke(args, SuspendCpuImpl);
+    }
+
+    bool IsChargerHiZModeEnabled() {
+        return g_charger_hi_z_mode_enabled;
+    }
+
+    void SetChargerHiZModeEnabled(bool en) {
+        g_charger_hi_z_mode_enabled = en;
+    }
+
+}

Source/hoc-clk/bpmpfw/Makefile

@@ -0,0 +1,62 @@
+ifeq ($(strip $(DEVKITARM)),)
+$(error "Please set DEVKITARM in your environment. export DEVKITARM=<path to>devkitARM")
+endif
+include $(DEVKITARM)/base_rules
+
+TARGET     := bpmpfw
+BUILDDIR   := build
+OUTPUTDIR  := out
+SOURCEDIR  := source
+
+LIBCDIR := $(SOURCEDIR)/libc
+
+CPP_SOURCES := $(wildcard $(SOURCEDIR)/*.cpp)
+C_SOURCES   := $(wildcard $(LIBCDIR)/src/*.c)
+S_SOURCES   := $(wildcard $(SOURCEDIR)/*.s)
+
+OBJS := $(patsubst $(SOURCEDIR)/%.cpp,$(BUILDDIR)/%.o,$(CPP_SOURCES)) \
+        $(patsubst $(LIBCDIR)/src/%.c,$(BUILDDIR)/libc/%.o,$(C_SOURCES)) \
+        $(patsubst $(SOURCEDIR)/%.s,$(BUILDDIR)/%.o,$(S_SOURCES))
+
+ARCH     := -march=armv4t -mtune=arm7tdmi -mthumb-interwork
+COMMONDIR := ../common/include
+INCLUDE  := -I$(SOURCEDIR) -I$(LIBCDIR)/include -I$(LIBCDIR)/include/fslc -I$(COMMONDIR)
+CFLAGS   := $(ARCH) -O2 -g -ffreestanding -nostdlib -ffunction-sections -fdata-sections \
+            -fomit-frame-pointer -std=gnu11 -Wall -Wextra $(CUSTOMDEFINES)
+CXXFLAGS := $(ARCH) -O2 -g -ffreestanding -nostdlib -fno-exceptions -fno-rtti \
+            -fno-unwind-tables -fno-threadsafe-statics -ffunction-sections -fdata-sections \
+            -fomit-frame-pointer -std=gnu++20 -Wall -Wextra $(CUSTOMDEFINES)
+ASFLAGS  := $(ARCH) -g
+LDFLAGS  := $(ARCH) -nostartfiles -nodefaultlibs -Wl,--nmagic,--gc-sections \
+            -T $(SOURCEDIR)/../bpmpfw.ld -lgcc
+
+BIN_TARGET := $(OUTPUTDIR)/$(TARGET).bin
+
+.PHONY: all clean
+
+all: $(BIN_TARGET)
+
+$(BIN_TARGET): $(BUILDDIR)/$(TARGET).elf
+	@mkdir -p $(OUTPUTDIR)
+	$(OBJCOPY) -S -O binary $< $@
+
+$(BUILDDIR)/$(TARGET).elf: $(OBJS)
+	@$(CXX) $(LDFLAGS) $(OBJS) -o $@
+
+$(BUILDDIR)/%.o: $(SOURCEDIR)/%.cpp | $(BUILDDIR)
+	@$(CXX) $(CXXFLAGS) $(INCLUDE) -c $< -o $@
+
+$(BUILDDIR)/libc/%.o: $(LIBCDIR)/src/%.c | $(BUILDDIR)/libc
+	@$(CC) $(CFLAGS) $(INCLUDE) -c $< -o $@
+
+$(BUILDDIR)/%.o: $(SOURCEDIR)/%.s | $(BUILDDIR)
+	@$(CXX) $(ASFLAGS) $(INCLUDE) -c $< -o $@
+
+$(BUILDDIR):
+	@mkdir -p $(BUILDDIR)
+
+$(BUILDDIR)/libc: | $(BUILDDIR)
+	@mkdir -p $(BUILDDIR)/libc
+
+clean:
+	@rm -rf $(BUILDDIR) $(OUTPUTDIR)

Source/hoc-clk/bpmpfw/bpmpfw.ld

@@ -0,0 +1,151 @@
+OUTPUT_ARCH(arm)
+ENTRY(reset)
+
+MEMORY
+{
+    bpmpfw : ORIGIN = 0x40004000, LENGTH = 32K
+}
+
+SECTIONS
+{
+    PROVIDE(__start__ = ORIGIN(bpmpfw));
+    . = __start__;
+    __code_start = . ;
+
+    .vectors :
+    {
+        KEEP (*(.vectors .vectors.*))
+        . = ALIGN(8);
+    } >bpmpfw
+
+    .text :
+    {
+        *(.text.unlikely .text.*_unlikely .text.unlikely.*)
+        *(.text.exit .text.exit.*)
+        *(.text.startup .text.startup.*)
+        *(.text.hot .text.hot.*)
+        *(.text .stub .text.* .gnu.linkonce.t.*)
+        . = ALIGN(8);
+    } >bpmpfw
+
+    .init :
+    {
+        KEEP( *(.init) )
+        . = ALIGN(8);
+    } >bpmpfw
+
+    .plt :
+    {
+        *(.plt)
+        *(.iplt)
+        . = ALIGN(8);
+    } >bpmpfw
+
+    .fini :
+    {
+        KEEP( *(.fini) )
+        . = ALIGN(8);
+    } >bpmpfw
+
+    . = ALIGN(8);
+    __rodata_start = . ;
+
+    .rodata :
+    {
+        *(.rodata .rodata.* .gnu.linkonce.r.*)
+        . = ALIGN(8);
+    } >bpmpfw
+
+    .eh_frame_hdr     : { __eh_frame_hdr_start = .; *(.eh_frame_hdr) *(.eh_frame_entry .eh_frame_entry.*) __eh_frame_hdr_end = .; } >bpmpfw
+    .eh_frame         : ONLY_IF_RO { KEEP (*(.eh_frame)) *(.eh_frame.*) } >bpmpfw
+    .gcc_except_table : ONLY_IF_RO { *(.gcc_except_table .gcc_except_table.*) } >bpmpfw
+    .gnu_extab        : ONLY_IF_RO { *(.gnu_extab*) } >bpmpfw
+
+    .hash              : { *(.hash) } >bpmpfw
+
+    . = ALIGN(8);
+    __data_start = . ;
+
+    .eh_frame : ONLY_IF_RW { KEEP (*(.eh_frame)) *(.eh_frame.*) } >bpmpfw
+    .gcc_except_table : ONLY_IF_RW { *(.gcc_except_table .gcc_except_table.*) } >bpmpfw
+    .gnu_extab : ONLY_IF_RW { *(.gnu_extab*) } >bpmpfw
+    .exception_ranges : ONLY_IF_RW { *(.exception_ranges .exception_ranges*) } >bpmpfw
+
+    .preinit_array ALIGN(8) :
+    {
+        PROVIDE (__preinit_array_start = .);
+        KEEP (*(.preinit_array))
+        PROVIDE (__preinit_array_end = .);
+    } >bpmpfw
+
+    .init_array ALIGN(8) :
+    {
+        PROVIDE (__init_array_start = .);
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        PROVIDE (__init_array_end = .);
+    } >bpmpfw
+
+    .fini_array ALIGN(8) :
+    {
+        PROVIDE (__fini_array_start = .);
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        PROVIDE (__fini_array_end = .);
+    } >bpmpfw
+
+    .ctors ALIGN(8) :
+    {
+        KEEP (*crtbegin.o(.ctors)) /* MUST be first -- GCC requires it */
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*(.ctors))
+    } >bpmpfw
+
+    .dtors ALIGN(8) :
+    {
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*(.dtors))
+    } >bpmpfw
+
+    __got_start__ = .;
+
+    .got            : { *(.got) *(.igot) } >bpmpfw
+    .got.plt        : { *(.got.plt)  *(.igot.plt) } >bpmpfw
+
+    __got_end__ = .;
+
+    .data ALIGN(8) :
+    {
+        *(.data .data.* .gnu.linkonce.d.*)
+        SORT(CONSTRUCTORS)
+    } >bpmpfw
+
+    __bss_start__ = .;
+    .bss ALIGN(8) :
+    {
+        *(.dynbss)
+        *(.bss .bss.* .gnu.linkonce.b.*)
+        *(COMMON)
+        . = ALIGN(16);
+    } >bpmpfw
+    __bss_end__ = .;
+
+    __end__ = ABSOLUTE(.) ;
+
+    __stack_top = ORIGIN(bpmpfw) + LENGTH(bpmpfw) - 0x10;
+
+    /DISCARD/ : { *(.group .comment .note .interp .ARM.attributes) }
+
+    .stab          0 : { *(.stab) }
+    .stabstr       0 : { *(.stabstr) }
+    .debug_info    0 : { *(.debug_info) }
+    .debug_abbrev  0 : { *(.debug_abbrev) }
+    .debug_line    0 : { *(.debug_line) }
+    .debug_frame   0 : { *(.debug_frame) }
+    .debug_str     0 : { *(.debug_str) }
+    .debug_loc     0 : { *(.debug_loc) }
+    .debug_macinfo 0 : { *(.debug_macinfo) }
+}

Source/hoc-clk/bpmpfw/source/defs.hpp

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+#include <cstdint>
+
+using u8  = std::uint8_t;
+using u16 = std::uint16_t;
+using u32 = std::uint32_t;
+using u64 = std::uint64_t;
+using s8  = std::int8_t;
+using s16 = std::int16_t;
+using s32 = std::int32_t;
+using s64 = std::int64_t;

Source/hoc-clk/bpmpfw/source/libc/LICENSE

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2015-2016 Jurģis Brigmanis <velko.x@gmail.com>
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Source/hoc-clk/bpmpfw/source/libc/include/assert.h

@@ -0,0 +1,6 @@
+#ifndef _STD_ASSERT_H
+#define _STD_ASSERT_H
+
+#include "fslc/fslc_assert.h"
+
+#endif /* _STD_ASSERT_H */

Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_assert.h

@@ -0,0 +1,35 @@
+#ifndef FSLC_ASSERT_H
+#define FSLC_ASSERT_H
+
+#ifndef ALT_FSLC_NAMES
+
+#define fslc_assert     assert
+
+#endif /* ALT_FSLC_NAMES */
+
+
+#ifdef NDEBUG
+
+#define fslc_assert(expr)     ((void)0)
+
+#else /* NDEBUG */
+
+/* Note: __PRETTY_FUNCTION__ is GCC specific */
+#define fslc_assert(expr)     ((expr) ? (void)0 : __fslc_assert_fail(#expr, __FILE__, __LINE__, __PRETTY_FUNCTION__))
+
+#endif /* NDEBUG */
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+
+    void __fslc_assert_fail(const char *expr, const char *file, unsigned int line, const char *func);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+#endif /* FSLC_ASSERT_H */

Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdio.h

@@ -0,0 +1,74 @@
+#ifndef FSLC_STDIO_H
+#define FSLC_STDIO_H
+
+#include <stdarg.h>
+#include <stddef.h>
+
+#ifndef ALT_FSLC_NAMES
+
+#define FSLC_FILE       FILE
+#define fslc_stdout     stdout
+#define fslc_stdin      stdin
+#define fslc_putchar    putchar
+#define fslc_fputc      fputc
+#define fslc_fputs      fputs
+#define fslc_puts       puts
+
+#define fslc_vfprintf   vfprintf
+#define fslc_vprintf    vprintf
+#define fslc_fprintf    fprintf
+#define fslc_printf     printf
+
+#define fslc_fwrite     fwrite
+#define fslc_fread      fread
+
+#define fslc_getc       getc
+#define fslc_getchar    getchar
+#define fslc_ungetc     ungetc
+#define fslc_fgets      fgets
+
+#endif /* ALT_FSLC_NAMES */
+
+typedef struct _FSLC_FILE FSLC_FILE;
+
+struct _FSLC_FILE
+{
+    void *user_ptr;
+    int (*putc)(int c, FSLC_FILE *stream);
+    int (*getc)(FSLC_FILE *stream);
+    void (*pre_output)(FSLC_FILE *stream);
+    void (*post_output)(FSLC_FILE *stream);
+    int ungetc_buf;
+};
+
+extern FSLC_FILE    *fslc_stdout;
+extern FSLC_FILE    *fslc_stdin;
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+    int fslc_putchar(int c);
+    int fslc_fputc(int c, FSLC_FILE *stream);
+    int fslc_fputs(const char *str, FSLC_FILE *stream);
+    int fslc_puts(const char *str);
+
+    int fslc_vfprintf(FSLC_FILE *stream, const char *format, va_list arg);
+    int fslc_vprintf(const char *format, va_list arg);
+    int fslc_fprintf(FSLC_FILE *stream, const char *format, ...);
+    int fslc_printf(const char *format, ...);
+
+    size_t fslc_fwrite(const void *ptr, size_t size, size_t count, FSLC_FILE *stream);
+    size_t fslc_fread(void *ptr, size_t size, size_t count, FSLC_FILE *stream);
+
+    int fslc_getc(FSLC_FILE *stream);
+    int fslc_getchar(void);
+    int fslc_ungetc(int c, FSLC_FILE *stream);
+    char *fslc_fgets(char *str, int num, FSLC_FILE *stream);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+
+#endif /* FSLC_STDIO_H */

Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdlib.h

@@ -0,0 +1,25 @@
+#ifndef FSLC_STDLIB_H
+#define FSLC_STDLIB_H
+
+#include <stddef.h>
+
+#ifndef ALT_FSLC_NAMES
+
+#define fslc_bsearch     bsearch
+
+#endif /* ALT_FSLC_NAMES */
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+
+    void *fslc_bsearch(const void *key, const void *base, size_t num, size_t size, int (*cmp_proc)(const void *, const void *));
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+#endif /* FSLC_STDLIB_H */

Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_string.h

@@ -0,0 +1,50 @@
+#ifndef FSLC_STRING_H
+#define FSLC_STRING_H
+
+#include <stddef.h>
+
+#ifndef ALT_FSLC_NAMES
+#define fslc_memset     memset
+#define fslc_memcpy     memcpy
+#define fslc_memmove    memmove
+#define fslc_memcmp     memcmp
+
+#define fslc_strlen     strlen
+#define fslc_strcpy     strcpy
+#define fslc_strncpy    strncpy
+#define fslc_strcmp     strcmp
+#define fslc_strncmp    strncmp
+#define fslc_strchr     strchr
+#define fslc_strstr     strstr
+#define fslc_strpbrk    strpbrk
+#define fslc_strspn     strspn
+#define fslc_strtok_r   strtok_r
+
+#endif /* ALT_FSLC_NAMES */
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+    void *fslc_memset(void *ptr, int value, size_t num);
+    void *fslc_memcpy(void *dest, const void *src, size_t len);
+    void *fslc_memmove(void *dest, const void *src, size_t len);
+    int fslc_memcmp(const void *ptr1, const void *ptr2, size_t num);
+
+    size_t fslc_strlen(const char *str);
+    char *fslc_strcpy(char *dest, const char *src);
+    char *fslc_strncpy(char *dest, const char *src, size_t len);
+    int fslc_strcmp(const char *str1, const char *str2);
+    int fslc_strncmp(const char *str1, const char *str2, size_t num);
+    char *fslc_strchr(const char *str, int c);
+    char *fslc_strstr(const char *search_in, const char *search_for);
+    char *fslc_strpbrk(const char *str, const char *delim);
+    size_t fslc_strspn(const char *str, const char *delim);
+    char *fslc_strtok_r(char *str, const char *delim, char **save_p);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+
+#endif /* FSLC_STRING_H */

Source/hoc-clk/bpmpfw/source/libc/include/fslc/stdlibx.h

@@ -0,0 +1,23 @@
+#ifndef FSLC_STDLIBX_H
+#define FSLC_STDLIBX_H
+
+#include <stddef.h>
+
+#ifndef ALT_FSLC_NAMES
+
+#define fslc_bsearch_i     bsearch_i
+
+#endif /* ALT_FSLC_NAMES */
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+    int fslc_bsearch_i(const void *key, const void *base, size_t num, size_t size, int (*cmp_proc)(const void *, const void *));
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+#endif /* FSLC_STDLIBX_H */

Source/hoc-clk/bpmpfw/source/libc/include/fslc/stringx.h

@@ -0,0 +1,33 @@
+#ifndef FSLC_STRINGX_H
+#define FSLC_STRINGX_H
+
+#include <stddef.h>
+
+#ifndef ALT_FSLC_NAMES
+
+#define fslc_memset_l   memset_l
+#define fslc_strcpy_e   strcpy_e
+#define fslc_strncpy_e  strncpy_e
+
+#endif /* ALT_FSLC_NAMES */
+
+#if __SIZEOF_LONG__ == 4
+#define memset_32    memset_l
+#elif __SIZEOF_LONG__ == 8
+#define memset_32(A, V, S)	memset_l(A, (((unsigned long)(V)<<32)|(V)), S)
+#endif
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+    void *fslc_memset_l(void *ptr, unsigned long value, size_t num);
+    char *fslc_strcpy_e(char *dest, const char *src);
+    char *fslc_strncpy_e(char *dest, const char *src, size_t len);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+
+#endif /* FSLC_STRINGX_H */

Source/hoc-clk/bpmpfw/source/libc/include/stdio.h

@@ -0,0 +1,6 @@
+#ifndef _STD_STDIO_H
+#define _STD_STDIO_H
+
+#include "fslc/fslc_stdio.h"
+
+#endif /* _STD_STDIO_H */

Source/hoc-clk/bpmpfw/source/libc/include/stdlib.h

@@ -0,0 +1,6 @@
+#ifndef _STD_STDLIB_H
+#define _STD_STDLIB_H
+
+#include "fslc/fslc_stdlib.h"
+
+#endif /* _STD_STDLIB_H */

Source/hoc-clk/bpmpfw/source/libc/include/string.h

@@ -0,0 +1,6 @@
+#ifndef _STD_STRING_H
+#define _STD_STRING_H
+
+#include "fslc/fslc_string.h"
+
+#endif /* _STD_STRING_H */

Source/hoc-clk/bpmpfw/source/libc/src/fslc_bsearch.c

@@ -0,0 +1,12 @@
+#include "fslc_stdlib.h"
+#include "stdlibx.h"
+
+void *fslc_bsearch(const void *key, const void *base, size_t num, size_t size, int (*cmp_proc)(const void *, const void *))
+{
+    int sidx = fslc_bsearch_i(key, base, num, size, cmp_proc);
+
+    if (sidx < 0)
+        return NULL;
+
+    return (char *)base + (sidx * size);
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_bsearch_i.c

@@ -0,0 +1,31 @@
+#include <stddef.h>
+
+int fslc_bsearch_i(const void *key, const void *base, size_t num, size_t size, int (*cmp_proc)(const void *, const void *))
+{
+    int lo = 0;
+    int hi = num - 1;
+
+    char *cbase = (char *)base;
+
+    while (lo <= hi)
+    {
+        int i = lo + ((hi - lo) >> 1);
+
+        char *ptr = cbase + (i * size);
+        int cmp = cmp_proc(key, ptr);
+
+        if (cmp == 0)
+            return i;
+
+        if (cmp > 0)
+        {
+            lo = i + 1;
+        }
+        else
+        {
+            hi = i - 1;
+        }
+    }
+
+    return ~lo;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_fprintf.c

@@ -0,0 +1,10 @@
+#include "fslc_stdio.h"
+
+int fslc_fprintf(FSLC_FILE *stream, const char *format, ...)
+{
+    va_list args;
+    va_start(args, format);
+    int r = fslc_vfprintf(stream, format, args);
+    va_end(args);
+    return r;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputc.c

@@ -0,0 +1,12 @@
+#include "fslc_stdio.h"
+
+int fslc_fputc(int c, FSLC_FILE *stream)
+{
+    if (stream->pre_output) stream->pre_output(stream);
+    
+    int r = stream->putc(c, stream);
+    
+    if (stream->post_output) stream->post_output(stream);
+    
+    return r;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputs.c

@@ -0,0 +1,15 @@
+#include "fslc_stdio.h"
+
+int _fslc_fputs_impl(const char *str, FSLC_FILE *stream);
+
+int fslc_fputs(const char *str, FSLC_FILE *stream)
+{
+    if (stream->pre_output) stream->pre_output(stream);
+    
+    int r = _fslc_fputs_impl(str, stream);
+    
+    if (stream->post_output) stream->post_output(stream);
+    
+    return r;
+}
+

Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputs_impl.c

@@ -0,0 +1,13 @@
+#include "fslc_stdio.h"
+
+int _fslc_fputs_impl(const char *str, FSLC_FILE *stream)
+{
+    int res = 0;
+    for (;*str; ++str)
+    {
+        int pr = stream->putc(*str, stream);
+        if (pr < 0) return pr;
+        ++res;
+    }
+    return res;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcmp.c

@@ -0,0 +1,38 @@
+#include "fslc_string.h"
+#include <stdint.h>
+
+int fslc_memcmp(const void *ptr1, const void *ptr2, size_t num)
+{
+    unsigned char *b_p1 = (unsigned char *)ptr1;
+    unsigned char *b_p2 = (unsigned char *)ptr2;
+
+    for (;((uintptr_t)b_p1 & (sizeof(long)-1)) && num > 0; ++b_p1, ++b_p2, --num)
+    {
+        if (*b_p1 != *b_p2)
+        {
+            return *b_p1 > *b_p2 ? 1 : -1;
+        }
+    }
+
+    unsigned long *w_p1 = (unsigned long *)b_p1;
+    unsigned long *w_p2 = (unsigned long *)b_p2;
+    for (;num >= sizeof(unsigned long); ++w_p1, ++w_p2, num -= sizeof(unsigned long))
+    {
+        if (*w_p1 != *w_p2)
+        {
+            return *w_p1 > *w_p2 ? 1 : -1;
+        }
+    }
+
+    b_p1 = (unsigned char *)w_p1;
+    b_p2 = (unsigned char *)w_p2;
+    for (; num > 0; ++b_p1, ++b_p2, --num)
+    {
+        if (*b_p1 != *b_p2)
+        {
+            return *b_p1 > *b_p2 ? 1 : -1;
+        }
+    }
+
+    return 0;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcpy.c

@@ -0,0 +1,40 @@
+#include "fslc_string.h"
+#include <stdint.h>
+
+void *fslc_memcpy(void *dest, const void *src, size_t len)
+{
+    /* Copy byte-by-byte up to word-aligned.
+     *
+     * There's a possibility, that src and dest have different offsets
+     * to word boundary and can not both be used efficiently.
+     *
+     * In this case we use align to dest argument, because it then can be
+     * written in one operation, instead of read-modify-write of 2 words.
+     *
+     * Most of the time both parameters should be word-aligned, however.
+     */
+    unsigned char *b_src = (unsigned char *)src;
+    unsigned char *b_dst = (unsigned char *)dest;
+    for (;((uintptr_t)b_dst & (sizeof(unsigned long)-1)) && len > 0; b_src++, b_dst++, len--)
+    {
+        *b_dst = *b_src;
+    }
+
+    /* Then all word-aligned */
+    unsigned long *w_src  = (unsigned long*)b_src;
+    unsigned long *w_dst = (unsigned long*)b_dst;
+    for (;len >= sizeof(unsigned long); w_src++, w_dst++, len -= sizeof(unsigned long))
+    {
+        *w_dst = *w_src;
+    }
+
+    /* And byte-by-byte the rest */
+    b_src  = (unsigned char *)w_src;
+    b_dst = (unsigned char *)w_dst;
+    for (; len > 0; b_src++, b_dst++, len--)
+    {
+        *b_dst = *b_src;
+    }
+
+    return dest;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_memmove.c

@@ -0,0 +1,69 @@
+#include "fslc_string.h"
+#include <stdint.h>
+
+void *fslc_memmove(void *dest, const void *src, size_t len)
+{
+    unsigned char *b_dst = (unsigned char *)dest;
+    unsigned char *b_src = (unsigned char *)src;
+
+    /* Need to check if memory regions overlaps. Then if the dest address is
+     * larger than source, we need to copy backwards (or else source regions
+     * ending will be overwritten by writes to dest, before they are read).
+     *
+     * In other cases it works identically as memcpy(). Same considerations
+     * about address alignment applies here.
+     */
+    if ( b_dst < b_src + len && b_dst > b_src)
+    {
+        b_src += len;
+        b_dst += len;
+
+        /* Byte-by-byte down to word-aligned. */
+        for (;((uintptr_t)b_dst & (sizeof(unsigned long)-1)) && len > 0; )
+        {
+            b_src--, b_dst--, len--;
+            *b_dst = *b_src;
+        }
+
+        /* Then all word-aligned */
+        unsigned long *w_src  = (unsigned long*)b_src;
+        unsigned long *w_dst = (unsigned long*)b_dst;
+        for (;len >= sizeof(unsigned long); )
+        {
+            w_src--, w_dst--, len -= sizeof(unsigned long);
+            *w_dst = *w_src;
+        }
+
+        /* And byte-by-byte the rest */
+        b_src = (unsigned char *)w_src;
+        b_dst = (unsigned char *)w_dst;
+        for (; len > 0; )
+        {
+            b_src--, b_dst--, len--;
+            *b_dst = *b_src;
+        }
+    } else {
+        /* Byte-by-byte up to word-aligned. */
+        for (;((uintptr_t)b_dst & (sizeof(unsigned long)-1)) && len > 0; b_src++, b_dst++, len--)
+        {
+            *b_dst = *b_src;
+        }
+
+        /* Then all word-aligned */
+        unsigned long *w_src  = (unsigned long *)b_src;
+        unsigned long *w_dst = (unsigned long *)b_dst;
+        for (;len >= sizeof(unsigned long); w_src++, w_dst++, len -= sizeof(unsigned long))
+        {
+            *w_dst = *w_src;
+        }
+
+        /* And byte-by-byte the rest */
+        b_src  = (unsigned char *)w_src;
+        b_dst = (unsigned char *)w_dst;
+        for (; len > 0; b_src++, b_dst++, len--)
+        {
+            *b_dst = *b_src;
+        }
+    }
+    return dest;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_memset.c

@@ -0,0 +1,22 @@
+#include "fslc_string.h"
+#include "stringx.h"
+
+void *fslc_memset(void *ptr, int value, size_t num)
+{
+    /* Will use memset_l() to actually fill the memory region. If value is non-zero, we need
+     * to replicate lowest byte to all the long's bytes.
+     */
+    if (value)
+    {
+        unsigned char c_val = value;
+        unsigned long l_val = (c_val << 0) | (c_val << 8) | (c_val << 16) | (c_val << 24);
+
+        #if __SIZEOF_LONG__ == 8
+        l_val |= l_val << 32;
+        #endif
+
+        return fslc_memset_l(ptr, l_val, num);
+    }
+    else
+        return fslc_memset_l(ptr, value, num);
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_memset_l.c

@@ -0,0 +1,31 @@
+#include "stringx.h"
+#include <stdint.h>
+
+void *fslc_memset_l(void *ptr, unsigned long value, size_t num)
+{
+    unsigned char *b_src = (unsigned char *)&value;
+    unsigned char *b_dst = (unsigned char *)ptr;
+
+    /* Byte by byte - to aligned word address */
+    b_src += ((uintptr_t)b_dst & (sizeof(unsigned long)-1));
+    for (;((uintptr_t)b_dst & (sizeof(unsigned long)-1)) && num > 0; b_dst++,b_src++,num--)
+    {
+        *b_dst = *b_src;
+    }
+
+    /* All aligned */
+    unsigned long *w_dst = (unsigned long *)b_dst;
+    for (;num >= sizeof(unsigned long); w_dst++,num -= sizeof(unsigned long))
+    {
+        *w_dst= value;
+    }
+
+    /* Unaligned ending */
+    b_src = (unsigned char *)&value;
+    b_dst = (unsigned char *)w_dst;
+    for (; num > 0; b_dst++,b_src++,num--)
+    {
+        *b_dst = *b_src;
+    }
+    return ptr;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_printf.c

@@ -0,0 +1,10 @@
+#include "fslc_stdio.h"
+
+int fslc_printf(const char *format, ...)
+{
+    va_list args;
+    va_start(args, format);
+    int r = fslc_vfprintf(fslc_stdout, format, args);
+    va_end(args);
+    return r;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_putchar.c

@@ -0,0 +1,6 @@
+#include "fslc_stdio.h"
+
+int fslc_putchar(int c)
+{
+    return fslc_fputc(c, fslc_stdout);
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_puts.c

@@ -0,0 +1,21 @@
+#include "fslc_stdio.h"
+
+int _fslc_fputs_impl(const char *str, FSLC_FILE *stream);
+
+int fslc_puts(const char *str)
+{
+    if (fslc_stdout->pre_output) fslc_stdout->pre_output(fslc_stdout);
+    
+    int r = _fslc_fputs_impl(str, fslc_stdout);
+    
+    if (r >= 0)
+    {
+        int nlr = fslc_stdout->putc('\n', fslc_stdout);
+        if (nlr < 0)
+            r = nlr;
+    }
+    
+    if (fslc_stdout->post_output) fslc_stdout->post_output(fslc_stdout);
+    
+    return r;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_stdout.c

@@ -0,0 +1,3 @@
+#include "fslc_stdio.h"
+
+FSLC_FILE *fslc_stdout;

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strchr.c

@@ -0,0 +1,10 @@
+#include "fslc_string.h"
+
+char *fslc_strchr(const char *str, int c)
+{
+    for (;*str; ++str)
+        if (*str == c)
+            return (char *)str;
+
+    return NULL;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcmp.c

@@ -0,0 +1,8 @@
+#include "fslc_string.h"
+
+int fslc_strcmp(const char *str1, const char *str2)
+{
+    for( ; *str1 && *str1 == *str2; ++str1, ++str2);
+
+    return *((unsigned char *)str1) - *((unsigned char *)str2);
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcpy.c

@@ -0,0 +1,8 @@
+#include "fslc_string.h"
+#include "stringx.h"
+
+char *fslc_strcpy(char *dest, const char *src)
+{
+    fslc_strcpy_e(dest, src);
+    return dest;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcpy_e.c

@@ -0,0 +1,10 @@
+#include "stringx.h"
+
+char *fslc_strcpy_e(char *dest, const char *src)
+{
+    for(; *src; ++src, ++dest)
+        *dest = *src;
+
+    *dest = 0;
+    return dest;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strlen.c

@@ -0,0 +1,9 @@
+#include "fslc_string.h"
+
+size_t fslc_strlen(const char *str)
+{
+    size_t res = 0;
+    for(; *str; ++str) ++res;
+
+    return res;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncmp.c

@@ -0,0 +1,8 @@
+#include "fslc_string.h"
+
+int fslc_strncmp(const char *str1, const char *str2, size_t num)
+{
+    for( ; *str1 && *str1 == *str2 && num > 1; ++str1, ++str2, --num);
+
+    return *((unsigned char *)str1) - *((unsigned char *)str2);
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy.c

@@ -0,0 +1,9 @@
+#include "fslc_string.h"
+
+char *_fslc_strncpy_impl(char *dest, const char *src, size_t len);
+
+char *fslc_strncpy(char *dest, const char *src, size_t len)
+{
+    _fslc_strncpy_impl(dest, src, len);
+    return dest;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy_e.c

@@ -0,0 +1,9 @@
+#include "stringx.h"
+
+char *_fslc_strncpy_impl(char *dest, const char *src, size_t len);
+
+char *fslc_strncpy_e(char *dest, const char *src, size_t len)
+{
+    char *r = _fslc_strncpy_impl(dest, src, len);
+    return *r ? r + 1: r;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy_impl.c

@@ -0,0 +1,10 @@
+#include <stddef.h>
+
+char *_fslc_strncpy_impl(char *dest, const char *src, size_t len)
+{
+    for(; *src && len > 1; ++src, ++dest, --len)
+        *dest = *src;
+
+    *dest = *src;
+    return dest;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strpbrk.c

@@ -0,0 +1,14 @@
+#include "fslc_string.h"
+
+char *fslc_strpbrk(const char *str, const char *delim)
+{
+    for (; *str; ++str)
+    {
+        const char *d;
+        for (d = delim; *d; ++d)
+            if (*str == *d)
+                return (char *)str;
+    }
+
+    return NULL;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strspn.c

@@ -0,0 +1,18 @@
+#include "fslc_string.h"
+
+size_t fslc_strspn(const char *str, const char *delim)
+{
+    size_t r;
+
+    for (r = 0; *str; ++str, ++r)
+    {
+        const char *d;
+        for (d = delim; *d; ++d)
+            if (*str == *d) // skip to next if found
+                break;
+
+        if (*d == 0) // inner loop exited on \0 - char was not there
+            break;
+    }
+    return r;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strstr.c

@@ -0,0 +1,23 @@
+#include "fslc_string.h"
+
+char *fslc_strstr(const char *search_in, const char *search_for)
+{
+    while (*search_in)
+    {
+        char *rc = fslc_strchr(search_in, search_for[0]); // find first char
+
+        if (rc)
+        {
+            const char *si = rc + 1, *sf = search_for + 1; // search strcmp-style from second char
+            for( ; *si && *si == *sf; ++si, ++sf);
+
+            if (*sf == 0) return rc; // end of search-for - found result
+
+            search_in = rc + 1; // not this one, continue from next char
+        }
+        else
+            return NULL; // did not find first char - exit
+    }
+
+    return NULL; // reached end of source - did not find
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_strtok_r.c

@@ -0,0 +1,27 @@
+#include "fslc_string.h"
+
+char *fslc_strtok_r(char *str, const char *delim, char **save_p)
+{
+    if (str)
+        *save_p = str;
+
+    if (*save_p == NULL)
+        return NULL;
+
+    char *r = *save_p + fslc_strspn(*save_p, delim);
+
+    char *p = fslc_strpbrk(r, delim);
+
+    if (p)
+    {
+        *(p++) = 0;
+        *save_p = p;
+    }
+    else
+    {
+        if (*r == 0) r = NULL;
+        *save_p = NULL;
+    }
+
+    return r;
+}

Source/hoc-clk/bpmpfw/source/libc/src/fslc_vfprintf.c

@@ -0,0 +1,347 @@
+#include "fslc_stdio.h"
+
+/* There'a a funny behaviour of va_list and va_arg() macros:
+ *    - on x86_64 compiler generates a branch of some sort for each va_arg() macro. Normally it is not a problem,
+ *      however it kinda screws up branch coverage analysis. Therefore I decided to move va_arg() accesses into
+ *      dedicated functions, minimizing the issue to one untaken branch per access function.
+ *    - passing va_list by value works fine on x86_64, but makes some tests to fail on i386. It seems that modifications
+ *      done by va_arg() is not passed back to caller. Subsequent calls to va_arg() or access functions returns wrong
+ *      values on that platform.
+ *    - OK, then. Let's pass pointer to va_list instead. Not so fast. Works fine on i386, but fails to compile on
+ *      x86_64. Fine! Wrapping va_list into a dedicated struct and passing around pointer to it.
+ *
+ *    Why such a fuss about a few untaken branches in tests? Well, just because! Also, if I ever decide to refactor
+ *    _fslc_vfprintf_impl() - the case statement is getting a bit long and messy, it should make things easier if
+ *    argument list can be passed around freely (as opposed to using it from single function).
+ */
+
+struct va_list_w
+{
+    va_list arg;
+};
+
+/* Prototype for printf's internal implementation */
+static int _fslc_vfprintf_impl(FSLC_FILE *stream, const char *format, struct va_list_w *args);
+
+
+/* Main exported function */
+int fslc_vfprintf(FSLC_FILE *stream, const char *format, va_list arg)
+{
+    if (stream->pre_output) stream->pre_output(stream);
+
+    struct va_list_w argw;
+    va_copy(argw.arg, arg); // can not assign directly, should make a copy
+
+    int res = _fslc_vfprintf_impl(stream, format, &argw);
+
+    va_end(argw.arg); // and free it after use
+
+    if (stream->post_output) stream->post_output(stream);
+
+    return res;
+}
+
+/* Prototype to private FSLIBC function - implemented elsewhere */
+int _fslc_fputs_impl(const char *str, FSLC_FILE *stream);
+
+
+/* Prototypes for internal functions */
+static int _fslc_put_sint_l(signed long v, FSLC_FILE *stream);
+static int _fslc_put_uint_l(unsigned long v, FSLC_FILE *stream);
+static int _fslc_put_hex_l(unsigned long v, FSLC_FILE *stream, char alpha);
+
+static int _get_sint_arg(struct va_list_w *arg);
+static unsigned _get_uint_arg(struct va_list_w *arg);
+static long long _get_slonglong_arg(struct va_list_w *arg);
+static unsigned long long _get_ulonglong_arg(struct va_list_w *arg);
+static void *_get_ptr_arg(struct va_list_w *arg);
+
+/* There are some considerations when formatting Integers for output:
+ *     - on 32-bit systems integers will mostly be 32 bits long, except when one REALLY
+ *       wants to print long long. It should be more optimal to use 32-bit calculations
+ *       when possible and invoke 64-bit only when needed. To support that we provide 2
+ *       separate implementations.
+ *     - on 64-bit systems may be 32 or 64 bits, but there is no real benefit from
+ *       using shorter bit size. Hence common implementation.
+ *     - this library does not aim to support systems with shorter integer sizes (like 16-bit)
+ *       but it it did, it would probably make sense to provide optimized ports for these cases
+ *       as well.
+ * 
+ * Buffer size calculations:
+ *   Max UInt32 fits in 10 bytes (+1 for zero terminator) decimal
+ *   2**32 - 1 == 4294967295z
+ *                FFFFFFFFz
+ *                01234567890
+ * 
+ *   Max UInt64 fits in 10 bytes (and +1 for \0)
+ *   2**64 -1  == 18446744073709551615z
+ *                FFFFFFFFFFFFFFFFz
+ *                012345678901234567890
+ */
+
+#define BUFSIZE_LONG_LONG_DECIMAL   20
+#define BUFSIZE_LONG_HEX            (__SIZEOF_LONG__ * 2)
+#define BUFSIZE_LONG_LONG_HEX       (__SIZEOF_LONG_LONG__ * 2)
+#if __SIZEOF_LONG__ == 4
+
+#define BUFSIZE_LONG_DECIMAL    10
+static int _fslc_put_sint_ll(signed long long v, FSLC_FILE *stream);
+static int _fslc_put_uint_ll(unsigned long long v, FSLC_FILE *stream);
+static int _fslc_put_hex_ll(unsigned long long v, FSLC_FILE *stream, char alpha);
+
+#define _get_slong_arg      _get_sint_arg
+#define _get_ulong_arg      _get_uint_arg
+
+#elif __SIZEOF_LONG__ == 8
+
+#define BUFSIZE_LONG_DECIMAL    BUFSIZE_LONG_LONG_DECIMAL
+#define _fslc_put_sint_ll   _fslc_put_sint_l
+#define _fslc_put_uint_ll   _fslc_put_uint_l
+#define _fslc_put_hex_ll    _fslc_put_hex_l
+
+#define _get_slong_arg      _get_slonglong_arg
+#define _get_ulong_arg      _get_ulonglong_arg
+
+#endif
+
+#define FLAG_LONG       1
+#define FLAG_VERYLONG   3
+
+static int _fslc_vfprintf_impl(FSLC_FILE *stream, const char *format, struct va_list_w *arg)
+{
+    int res = 0;
+    int pr;
+    const char *c;
+    
+    for (c = format; *c; ++c)
+    {
+        if (*c == '%')
+        {
+            int flags = 0;
+            
+            for (;;) 
+            {
+                ++c;
+                switch (*c)
+                {
+                    case 's':
+                        pr = _fslc_fputs_impl((const char *)_get_ptr_arg(arg), stream);
+                        if (pr < 0) return pr;
+                        res += pr;
+                        break;
+
+                    case 'c':
+                        pr = stream->putc(_get_sint_arg(arg), stream);
+                        if (pr < 0) return pr;
+                        ++res;
+                        break;
+                        
+                    case '%':
+                        pr = stream->putc('%', stream);
+                        if (pr < 0) return pr;
+                        ++res;
+                        break;
+                    
+                    case 'i':
+                    case 'd':
+                        if ((flags & FLAG_VERYLONG) == FLAG_VERYLONG)
+                            pr = _fslc_put_sint_ll(_get_slonglong_arg(arg), stream);
+                        else if (flags & FLAG_LONG)
+                            pr = _fslc_put_sint_l(_get_slong_arg(arg), stream);
+                        else
+                            pr = _fslc_put_sint_l(_get_sint_arg(arg), stream);
+                        
+                        if (pr < 0) return pr;
+                        res += pr;
+                        break;
+                    
+                    case 'u':
+                        if ((flags & FLAG_VERYLONG) == FLAG_VERYLONG)
+                            pr = _fslc_put_uint_ll(_get_ulonglong_arg(arg), stream);
+                        else if (flags & FLAG_LONG)
+                            pr = _fslc_put_uint_l(_get_ulong_arg(arg), stream);
+                        else
+                            pr = _fslc_put_uint_l(_get_uint_arg(arg), stream);
+                        
+                        if (pr < 0) return pr;
+                        res += pr;
+                        break;
+                    
+                    case 'x':
+                        if ((flags & FLAG_VERYLONG) == FLAG_VERYLONG)
+                            pr = _fslc_put_hex_ll(_get_ulonglong_arg(arg), stream, 'a');
+                        else if (flags & FLAG_LONG)
+                            pr = _fslc_put_hex_l(_get_ulong_arg(arg), stream, 'a');
+                        else
+                            pr = _fslc_put_hex_l(_get_uint_arg(arg), stream, 'a');
+                        
+                        if (pr < 0) return pr;
+                        res += pr;
+                        break;
+                    
+                    case 'X':
+                        if ((flags & FLAG_VERYLONG) == FLAG_VERYLONG)
+                            pr = _fslc_put_hex_ll(_get_ulonglong_arg(arg), stream, 'A');
+                        else if (flags & FLAG_LONG)
+                            pr = _fslc_put_hex_l(_get_ulong_arg(arg), stream, 'A');
+                        else
+                            pr = _fslc_put_hex_l(_get_uint_arg(arg), stream, 'A');
+                        
+                        if (pr < 0) return pr;
+                        res += pr;
+                        break;
+                        
+                    case 'p':
+                        pr = _fslc_fputs_impl("0x", stream);
+                        if (pr < 0) return pr;
+                        res += pr;
+
+                        pr = _fslc_put_hex_l(_get_ulong_arg(arg), stream, 'a');
+
+                        if (pr < 0) return pr;
+                        res += pr;
+                        break;
+
+                    case 'l':
+                        if (flags & FLAG_LONG) 
+                            flags |= FLAG_VERYLONG;
+                        else
+                            flags |= FLAG_LONG;
+                        continue;
+                }
+                break;
+            }
+        } else {
+            pr = stream->putc(*c, stream);
+            if (pr < 0) return pr;
+            ++res;
+        }
+    }
+    
+    return res;
+}
+
+static int _get_sint_arg(struct va_list_w *arg)
+{
+    return va_arg(arg->arg, signed int);
+}
+
+static unsigned _get_uint_arg(struct va_list_w *arg)
+{
+    return va_arg(arg->arg, unsigned int);
+}
+
+static long long _get_slonglong_arg(struct va_list_w *arg)
+{
+    return va_arg(arg->arg, signed long long);
+}
+
+static unsigned long long _get_ulonglong_arg(struct va_list_w *arg)
+{
+    return va_arg(arg->arg, unsigned long long);
+}
+
+static void *_get_ptr_arg(struct va_list_w *arg)
+{
+    return va_arg(arg->arg, void *);
+}
+
+static int _fslc_put_sint_l(signed long v, FSLC_FILE *stream)
+{
+    if (v < 0)
+    {
+        int pr = stream->putc('-', stream);
+        if (pr < 0) return pr;
+        
+        pr = _fslc_put_uint_l(-v, stream);
+        if (pr < 0) return pr;
+        
+        return pr+1;
+    } else
+        return _fslc_put_uint_l(v, stream);
+}
+
+static int _fslc_put_uint_l(unsigned long v, FSLC_FILE *stream)
+{
+    char dbuff[BUFSIZE_LONG_DECIMAL+1], *p;
+
+    dbuff[BUFSIZE_LONG_DECIMAL] = 0;
+
+    for (p = dbuff + BUFSIZE_LONG_DECIMAL; v ; v /= 10)
+    {
+        *(--p) = (v % 10) + '0';
+    }
+
+    if (*p == 0) *(--p) = '0';
+
+    return _fslc_fputs_impl(p, stream);
+}
+
+static int _fslc_put_hex_l(unsigned long v, FSLC_FILE *stream, char alpha)
+{
+    char dbuff[BUFSIZE_LONG_HEX+1], *p;
+
+    dbuff[BUFSIZE_LONG_HEX] = 0;
+
+    for (p = dbuff + BUFSIZE_LONG_HEX; v ; v >>= 4)
+    {
+        char digit = (v & 0xF);
+        *(--p) = digit < 10 ? digit + '0' : digit - 10 + alpha;
+    }
+
+    if (*p == 0) *(--p) = '0';
+
+    return _fslc_fputs_impl(p, stream);
+}
+
+#if __SIZEOF_LONG__ == 4
+
+static int _fslc_put_sint_ll(signed long long v, FSLC_FILE *stream)
+{
+    if (v < 0)
+    {
+        int pr = stream->putc('-', stream);
+        if (pr < 0) return pr;
+        
+        pr = _fslc_put_uint_ll(-v, stream);
+        if (pr < 0) return pr;
+        
+        return pr+1;
+    } else
+        return _fslc_put_uint_ll(v, stream);
+}
+
+static int _fslc_put_uint_ll(unsigned long long v, FSLC_FILE *stream)
+{
+    char dbuff[BUFSIZE_LONG_LONG_DECIMAL+1], *p;
+
+    dbuff[BUFSIZE_LONG_LONG_DECIMAL] = 0;
+
+    for (p = dbuff + BUFSIZE_LONG_LONG_DECIMAL; v ; v /= 10)
+    {
+        *(--p) = (v % 10) + '0';
+    }
+
+    if (*p == 0) *(--p) = '0';
+
+    return _fslc_fputs_impl(p, stream);
+}
+
+static int _fslc_put_hex_ll(unsigned long long v, FSLC_FILE *stream, char alpha)
+{
+    char dbuff[BUFSIZE_LONG_LONG_HEX+1], *p;
+
+    dbuff[BUFSIZE_LONG_LONG_HEX] = 0;
+
+    for (p = dbuff + BUFSIZE_LONG_LONG_HEX; v ; v >>= 4)
+    {
+        char digit = (v & 0xF);
+        *(--p) = digit < 10 ? digit + '0' : digit - 10 + alpha;
+    }
+
+    if (*p == 0) *(--p) = '0';
+
+    return _fslc_fputs_impl(p, stream);
+}
+
+#endif

Source/hoc-clk/bpmpfw/source/libc/src/fslc_vprintf.c

@@ -0,0 +1,6 @@
+#include "fslc_stdio.h"
+
+int fslc_vprintf(const char *format, va_list arg)
+{
+    return fslc_vfprintf(fslc_stdout, format, arg);
+}

Source/hoc-clk/bpmpfw/source/libc_platform.cpp

@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdio.h>
+
+#include "regs.hpp"
+
+namespace {
+
+    int UartStreamPutc(int c, FILE *) {
+        UartPutc(static_cast<char>(c));
+        return c;
+    }
+
+    void UartStreamPostOutput(FILE *) {
+        UartFlush();
+    }
+
+    FILE g_uartStdout = { nullptr, UartStreamPutc, nullptr, nullptr, UartStreamPostOutput, -1 };
+
+} // namespace
+
+void InitializeLibc() {
+    stdout = &g_uartStdout;
+}

Source/hoc-clk/bpmpfw/source/libc_platform.hpp

@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+void InitializeLibc();

Source/hoc-clk/bpmpfw/source/main.cpp

@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include "libc_platform.hpp"
+#include "regs.hpp"
+#include "sensors.hpp"
+
+#include <hocclk/bpmp.h>
+
+namespace {
+
+    HocClkBpmpSharedInfo &SharedInfo() {
+        return *reinterpret_cast<HocClkBpmpSharedInfo *>(WorkRamStart);
+    }
+
+} // namespace
+
+void MainLoop(HocClkBpmpSharedInfo &info) {
+    for (;;) {
+        const u32 temp1 = MMIO32(SocthermBase + SocthermSensorTemp1);
+        const u32 temp2 = MMIO32(SocthermBase + SocthermSensorTemp2);
+
+        info.tempCpu  = TranslateSocthermTemp(static_cast<u16>(temp1 >> 16));
+        info.tempGpu  = TranslateSocthermTemp(static_cast<u16>(temp1));
+        info.tempMem  = TranslateSocthermTemp(static_cast<u16>(temp2 >> 16));
+        info.tempPllx = TranslateSocthermTemp(static_cast<u16>(temp2));
+
+        msleep(500);
+    }
+}
+
+extern "C" void main() {
+    InitializeLibc();
+
+    printf("[hoc-bpmpfw]: Starting bpmpfw\n");
+
+    HocClkBpmpSharedInfo &info = SharedInfo();
+    memset(&info, 0, sizeof(info));
+    info.status = 0;
+    info.magic = HOCCLK_BPMP_MAGIC; // written last
+
+    /* If the main loop exits then it is a panic or invalid state */
+    MainLoop(info);
+
+    printf("[hoc-bpmpfw]: PANIC\n");
+
+    /* Clear the magic so sysmodule knows of a panic */
+    info.magic = 0x0; 
+
+    /* Give a panic code */
+    info.status = ~0;
+    for(;;)
+        ;
+}

Source/hoc-clk/bpmpfw/source/regs.hpp

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+#include "defs.hpp"
+
+#define MMIO32(addr) (*reinterpret_cast<volatile u32 *>(addr))
+
+constexpr u32 FwRamStart    = 0x40004000;
+constexpr u32 FwRamEnd      = 0x4000C000;
+constexpr u32 WorkRamStart  = FwRamEnd;
+constexpr u32 WorkRamEnd    = 0x40010000;
+
+constexpr u32 FlowCtlrBase        = 0x60007000;
+constexpr u32 FlowCtlrHaltCopEvents = 0x004;
+
+constexpr u32 HaltModeWaitEvent = (2u << 29);
+constexpr u32 HaltMsec          = (1u << 24);
+constexpr u32 HaltUsec          = (1u << 25);
+constexpr u32 HaltMaxCnt        = 0xFF;
+
+[[maybe_unused]] inline void usleep(u32 us) {
+    while (us) {
+        const u32 delay = (us > HaltMaxCnt) ? HaltMaxCnt : us;
+        us -= delay;
+        MMIO32(FlowCtlrBase + FlowCtlrHaltCopEvents) = HaltModeWaitEvent | HaltUsec | delay;
+    }
+}
+[[maybe_unused]] inline void msleep(u32 ms) {
+    while (ms) {
+        const u32 delay = (ms > HaltMaxCnt) ? HaltMaxCnt : ms;
+        ms -= delay;
+        MMIO32(FlowCtlrBase + FlowCtlrHaltCopEvents) = HaltModeWaitEvent | HaltMsec | delay;
+    }
+}
+
+constexpr u32 TmrBase        = 0x60005000;
+constexpr u32 TimerUsCntr1Us = 0x10;
+
+constexpr u32 UartBBase = 0x70006040;
+constexpr u32 UartThr   = 0x00;
+constexpr u32 UartLsr   = 0x14;
+constexpr u32 UartLsrThre = (1u << 5);
+constexpr u32 UartLsrTmty = (1u << 6);
+
+inline void UartPutc(char c) {
+    while (!(MMIO32(UartBBase + UartLsr) & UartLsrThre))
+        ;
+    MMIO32(UartBBase + UartThr) = static_cast<u32>(static_cast<u8>(c));
+}
+
+// Waits for the byte to be fully transmitted
+inline void UartFlush() {
+    while (!(MMIO32(UartBBase + UartLsr) & UartLsrTmty))
+        ;
+}
+
+inline void UartPuts(const char *s) {
+    while (*s) {
+        UartPutc(*s++);
+    }
+}
+
+inline void UartPutHex32(u32 v) {
+    static const char digits[] = "0123456789ABCDEF";
+    for (int i = 28; i >= 0; i -= 4) {
+        UartPutc(digits[(v >> i) & 0xF]);
+    }
+}

Source/hoc-clk/bpmpfw/source/sensors.hpp

@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+#include "regs.hpp"
+
+constexpr u32 SocthermBase       = 0x700E2000;
+constexpr u32 SocthermSensorTemp1 = 0x1C8;
+constexpr u32 SocthermSensorTemp2 = 0x1CC;
+
+inline s32 TranslateSocthermTemp(u16 val) {
+    s32 t = ((val >> 8) & 0xFF) * 1000;
+    if (val & (1u << 7)) {
+        t += 500;
+    }
+    if (val & (1u << 0)) {
+        t = -t;
+    }
+    return t;
+}

Source/hoc-clk/bpmpfw/source/start.s

@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) Atmosphere-NX
+ *
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+.section    .vectors, "ax", %progbits
+.align      3
+.global     reset
+reset:
+    b _start
+    b _panic
+
+.section    .text.start, "ax", %progbits
+.align      3
+.global     _start
+.extern     main
+_start:
+    /* Mask IRQ + FIQ, enter SVC mode. */
+    msr cpsr_f, #0xC0
+    msr cpsr_cf, #0xD3
+
+    /* Set the stack pointer. */
+    ldr sp, =__stack_top
+
+    /* Zero .bss. */
+    ldr r0, =__bss_start__
+    ldr r1, =__bss_end__
+    mov r2, #0
+1:
+    cmp r0, r1
+    bge 2f
+    str r2, [r0], #4
+    b   1b
+2:
+
+    /* Run global constructors. */
+    ldr r4, =__init_array_start
+    ldr r5, =__init_array_end
+3:
+    cmp r4, r5
+    bge 4f
+    ldr r0, [r4], #4
+    mov lr, pc
+    bx  r0
+    b   3b
+4:
+
+    bl  main
+
+.global     _panic
+_panic:
+    ldr r0, =0x70006040 /* UART_B_BASE */
+    adr r3, _panic_msg
+7:
+    ldrb r1, [r3], #1
+    cmp  r1, #0
+    beq  _panic_hang
+8:
+    ldr r2, [r0, #0x14] /* UART_LSR */
+    tst r2, #0x20       /* UART_LSR_THRE */
+    beq 8b
+    str r1, [r0]
+    b   7b
+
+_panic_msg:
+    .asciz "[bpmpfw] PANIC\n"
+    .align 2
+
+_panic_hang:
+    b _panic_hang

Source/hoc-clk/build.sh

@@ -20,6 +20,15 @@ for arg in "$@"; do
 done
 
 echo
+echo "*** Compiling bpmpfw ***"
+pushd "$ROOT_DIR/bpmpfw"
+make -j$CORES
+popd > /dev/null
+
+mkdir -p "$ROOT_DIR/sysmodule/data"
+cp -vf "$ROOT_DIR/bpmpfw/out/bpmpfw.bin" "$ROOT_DIR/sysmodule/data/bpmpfw.bin"
+echo
+
 echo "*** Compiling hoc-clk ***"
 TITLE_ID="$(grep -oP '"title_id":\s*"0x\K(\w+)' "$ROOT_DIR/sysmodule/perms.json")"
 

Source/hoc-clk/common/include/hocclk/board.h

@@ -30,7 +30,10 @@
 #include <stdint.h>
 #include <stddef.h>
 #include <stdbool.h>
+#ifdef __SWITCH__ // Wont be defined in BPMP-FW
 #include <switch/types.h>
+#endif
+
 typedef enum
 {
     HocClkSocType_Erista = 0, // T210, found in Icosa and Copper

Source/hoc-clk/common/include/hocclk/bpmp.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) Souldbminer, Lightos_ and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "board.h"
+
+#define HOCCLK_BPMP_MAGIC 0x42504D50 // 'BPMP'
+
+/*
+ * Shared with BPMP-FW
+ * Any 64 bit data type is not permitted.
+ */
+typedef struct {
+    u32 magic;      // should be HOCCLK_BPMP_MAGIC once bpmpfw is running
+    u32 status;     // Reserved for error codes and such
+    s32 tempCpu;
+    s32 tempGpu;
+    s32 tempMem;
+    s32 tempPllx;
+    u32 reserved[2];
+} HocClkBpmpSharedInfo;
+
+static_assert(sizeof(HocClkBpmpSharedInfo) == 0x20);

Source/hoc-clk/common/include/hocclk/clock_manager.h

@@ -29,6 +29,7 @@
 
 #include <stdint.h>
 #include "board.h"
+#include "bpmp.h"
 
 typedef struct {
 
@@ -84,6 +85,9 @@ typedef struct {
     bool rebootRequired;
     bool isFirstLoad;
 
+    // Populated from bpmpfw's work-RAM via SmcCopyFromIram (see HocClkBpmpSharedInfo above).
+    HocClkBpmpSharedInfo bpmpInfo;
+
     // Reserved for future use
     u8 reserved[0x358];
 } HocClkContext;
@@ -100,4 +104,4 @@ typedef struct
 
 #define HOCCLK_GLOBAL_PROFILE_TID 0xA111111111111111
 
-static_assert(sizeof(HocClkContext) == 0x500);
+static_assert(sizeof(HocClkContext) == 0x520);

Source/hoc-clk/overlay/src/ui/gui/about_gui.cpp

@@ -43,6 +43,7 @@ tsl::elm::ListItem* bqtempitem = NULL;
 tsl::elm::ListItem* aotagTempItem = NULL;
 tsl::elm::ListItem* cTypeItem = NULL;
 tsl::elm::ListItem* creditsItem = NULL;
+tsl::elm::ListItem* bpmpStatusItem = NULL;
 
 #define R_ARROW "\u2192"
 
@@ -147,6 +148,9 @@ void AboutGui::listUI()
     // custRevItem = new tsl::elm::ListItem("CUST revision:");
     // this->listElement->addItem(custRevItem);
 
+    bpmpStatusItem = new tsl::elm::ListItem("BPMP Status:");
+    this->listElement->addItem(bpmpStatusItem);
+
     kipVersionItem = new tsl::elm::ListItem("KIP version:");
     this->listElement->addItem(kipVersionItem);
 
@@ -272,6 +276,7 @@ void AboutGui::refresh()
     bqtempitem->setValue(strings[10]);
 
     cTypeItem->setValue(hocClkFormatConsoleType(this->context->consoleType, true));
+    bpmpStatusItem->setValue(this->context->bpmpInfo.magic == HOCCLK_BPMP_MAGIC ? "Enabled" : "Halted");
 }
 
 class CreditsSubMenu : public AboutGui {

Source/hoc-clk/sysmodule/Makefile

@@ -22,7 +22,7 @@ CONFIG_DIR	:=	horizon-oc
 BUILD		:=	build
 OUTDIR		:=	out
 RESOURCES	:=	res
-SOURCES		:=	src src/hos src/i2c src/ipc src/board src/display src/tsensor src/mgr src/file src/soc src/mapping src/pwr src/util
+SOURCES		:=	src src/hos src/i2c src/ipc src/board src/bpmp src/display src/tsensor src/mgr src/file src/soc src/mapping src/pwr src/util
 DATA		:=	data
 INCLUDES	:=	../common/include src/hos src/soc src/i2c src/util src/pwr src/ipc
 EXEFS_SRC	:=	exefs_src

Source/hoc-clk/sysmodule/perms.json

@@ -3,7 +3,7 @@
 	"title_id": "0x00FF0000636C6BFF",
 	"title_id_range_min": "0x00FF0000636C6BFF",
 	"title_id_range_max": "0x00FF0000636C6BFF",
-	"main_thread_stack_size": "0x0000C000",
+	"main_thread_stack_size": "0x0000F000",
 	"main_thread_priority": 16,
 	"default_cpu_id": 3,
 	"process_category": 1,

Source/hoc-clk/sysmodule/src/board/board.cpp

@@ -35,6 +35,7 @@
 #include <t210.h>
 #include <tmp451.h>
 
+#include "../bpmp/bpmp.hpp"
 #include "../display/display_refresh_rate.hpp"
 #include "../file/file_utils.hpp"
 #include "../hos/apm_ext.h"
@@ -52,7 +53,7 @@
 
 namespace board {
 
-    u64 clkVirtAddr, dsiVirtAddr, apbVirtAddr, fuseVirtAddr;
+    u64 clkVirtAddr, dsiVirtAddr, apbVirtAddr, fuseVirtAddr, sysVirtAddr, actmonVirtAddr;
 
     HocClkSocType gSocType;
     u8 gDramID;
@@ -153,6 +154,10 @@ namespace board {
         rc = QueryMemoryMapping(&fuseVirtAddr, 0x7000F000, 0x1000);
         ASSERT_RESULT_OK(rc, "QueryMemoryMapping (fuse)");
 
+        rc = QueryMemoryMapping(&sysVirtAddr, 0x6000C000, 0x1000);
+        ASSERT_RESULT_OK(rc, "QueryMemoryMapping (sys)");
+        actmonVirtAddr = sysVirtAddr + 0x800;
+
         FetchHardwareInfos();
 
         Result nvCheck = 1;
@@ -173,6 +178,12 @@ namespace board {
 
         tsensor::InitializeSoctherm();  // SOCTHERM must be init before AOTAG
 
+        Result bpmpfwRc = bpmp::StartBpmfwExecution();
+        if (R_FAILED(bpmpfwRc)) {
+            fileUtils::LogLine("[bpmp] StartBpmfwExecution failed: 0x%x", bpmpfwRc);
+        }
+        bpmp::StartSleepMonitorThread();
+
         // PMC exosphere check
         SecmonArgs args = {};
         args.X[0] = 0xF0000002;
@@ -201,6 +212,8 @@ namespace board {
     }
 
     void Exit() {
+        bpmp::StopSleepMonitorThread();
+
         if (HOSSVC_HAS_CLKRST) {
             clkrstExit();
         } else {

Source/hoc-clk/sysmodule/src/board/board.hpp

@@ -42,7 +42,7 @@
 #define HOSSVC_HAS_TC (hosversionAtLeast(5, 0, 0))
 
 namespace board {
-    extern u64 clkVirtAddr, dsiVirtAddr, apbVirtAddr, fuseVirtAddr;
+    extern u64 clkVirtAddr, dsiVirtAddr, apbVirtAddr, fuseVirtAddr, sysVirtAddr, actmonVirtAddr;
     extern HocClkSocType gSocType;
     extern u8 gDramID;
     extern HocClkConsoleType gConsoleType;

Source/hoc-clk/sysmodule/src/bpmp/bpmp.cpp

@@ -0,0 +1,268 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <atomic>
+#include <cstring>
+#include <switch.h>
+
+#include "../board/board.hpp"
+#include "../file/config.hpp"
+#include "../file/file_utils.hpp"
+#include "../mapping/mem_map.hpp"
+#include "../tsensor/soctherm.hpp"
+#include "bpmp.hpp"
+
+#include "bpmpfw_bin.h"
+
+namespace bpmp {
+
+    namespace {
+
+        constexpr u64 FlowCtlrPhysBase = 0x60007000;
+        constexpr u64 EvpPhysBase = 0x6000F000;
+
+        constexpr u32 ClkRstRstDevLSet = 0x300;
+        constexpr u32 ClkRstRstDevLClr = 0x304;
+        constexpr u32 RstDevLCopRstBit = (1u << 1); /* SWR_COP_RST */
+
+        constexpr u32 ApbMiscSlaveSecurityReg0 = 0xC00;
+        constexpr u32 ApbMiscSlaveSecurityReg1 = 0xC04;
+        constexpr u32 ApbMiscSlaveSecurityReg2 = 0xC08;
+
+        /* Relative to actmonVirtAddr */
+        constexpr u32 ActmonGlbPeriodCtrl = 0x004;
+        constexpr u32 ActmonCopCtrl       = 0xC0;
+        constexpr u32 ActmonCopUpperWmark = 0xC4;
+        constexpr u32 ActmonCopIntrStatus = 0xE4;
+
+        constexpr u32 ActmonGlbPeriodCtrlUsec  = (1u << 8);
+        constexpr u32 ActmonCopCtrlStockArmed  = (1u << 31) | (1u << 30) | (1u << 18);
+
+        constexpr u32 EvpCopResetVector         = 0x200;
+        constexpr u32 EvpCopUndefVector         = 0x204;
+        constexpr u32 EvpCopSwiVector           = 0x208;
+        constexpr u32 EvpCopPrefetchAbortVector = 0x20C;
+        constexpr u32 EvpCopDataAbortVector     = 0x210;
+        constexpr u32 EvpCopRsvdVector          = 0x214;
+        constexpr u32 EvpCopIrqVector           = 0x218;
+        constexpr u32 EvpCopFiqVector           = 0x21C;
+
+        constexpr u32 StockBpmpExceptionVector = 0x7D000000;
+
+        constexpr u32 FlowCtlrHaltCopEvents = 0x004;
+        constexpr u32 FlowModeNone          = 0x00000000;
+        constexpr u32 FlowModeStop          = (2u << 29);
+
+        constexpr u64 FwIramPhysBase = 0x40004000;
+        constexpr u32 FwStagingSize  = 0x8000;
+        constexpr u64 WorkRamPhysBase = FwIramPhysBase + FwStagingSize; // matches bpmpfw's WorkRamStart
+
+        /* secmon will return error unless we apply thiss workaround */
+        alignas(4096) u8 s_fwStageBuf[FwStagingSize];
+
+        HocClkBpmpSharedInfo s_sharedInfo = {};
+
+        inline volatile u32 &Mmio(u64 base_va, u32 offset) {
+            return *reinterpret_cast<volatile u32 *>(base_va + offset);
+        }
+
+        // Late in the boot chain, otherwise a fatal will occur from invalid boot state
+        constexpr u32 pscDependencies[] = { PscPmModuleId_Olsc };
+        constexpr PscPmModuleId PscModuleId = (PscPmModuleId)705;
+
+        PscPmModule s_pscModule;
+        bool s_pscPrepared = false;
+        Thread s_pscThread;
+        bool s_pscExit = false;
+
+        std::atomic_bool s_isAwake = true;
+
+        void RestoreStockState() {
+            SmcReadWriteRegister(EvpPhysBase + EvpCopResetVector,         0xFFFFFFFF, StockBpmpExceptionVector);
+            SmcReadWriteRegister(EvpPhysBase + EvpCopUndefVector,         0xFFFFFFFF, StockBpmpExceptionVector);
+            SmcReadWriteRegister(EvpPhysBase + EvpCopSwiVector,           0xFFFFFFFF, StockBpmpExceptionVector);
+            SmcReadWriteRegister(EvpPhysBase + EvpCopPrefetchAbortVector, 0xFFFFFFFF, StockBpmpExceptionVector);
+            SmcReadWriteRegister(EvpPhysBase + EvpCopDataAbortVector,     0xFFFFFFFF, StockBpmpExceptionVector);
+            SmcReadWriteRegister(EvpPhysBase + EvpCopRsvdVector,          0xFFFFFFFF, StockBpmpExceptionVector);
+            SmcReadWriteRegister(EvpPhysBase + EvpCopIrqVector,           0xFFFFFFFF, StockBpmpExceptionVector);
+            SmcReadWriteRegister(EvpPhysBase + EvpCopFiqVector,           0xFFFFFFFF, StockBpmpExceptionVector);
+
+            SmcReadWriteRegister(FlowCtlrPhysBase + FlowCtlrHaltCopEvents, 0xFFFFFFFF, FlowModeStop);
+
+            Mmio(board::actmonVirtAddr, ActmonGlbPeriodCtrl) = ActmonGlbPeriodCtrlUsec;
+            Mmio(board::actmonVirtAddr, ActmonCopUpperWmark) = 0;
+            Mmio(board::actmonVirtAddr, ActmonCopIntrStatus) = Mmio(board::actmonVirtAddr, ActmonCopIntrStatus);
+            Mmio(board::actmonVirtAddr, ActmonCopCtrl) = ActmonCopCtrlStockArmed;
+        }
+
+        void PscThreadFunc(void *) {
+            while (!s_pscExit) {
+                Result rc = eventWait(&s_pscModule.event, 1'000'000'000ULL);
+                if (R_FAILED(rc)) {
+                    continue; // Timeout
+                }
+
+                PscPmState state;
+                u32 flags;
+                rc = pscPmModuleGetRequest(&s_pscModule, &state, &flags);
+                if (R_SUCCEEDED(rc)) {
+                    if (state == PscPmState_Awake) {
+                        svcSleepThread(500'000'000); // 500ms
+
+                        /* I have no clue why this is needed as it wasn't needed before ¯\(ツ)/¯ */
+                        tsensor::InitializeSoctherm();
+
+                        Result wrc = StartBpmfwExecution();
+                        if (R_FAILED(wrc)) {
+                            fileUtils::LogLine("[bpmp] restart after wake failed: 0x%x", wrc);
+                        }
+
+                        s_isAwake = true;
+                    } else if (state == PscPmState_ReadySleep || state == PscPmState_ReadyShutdown) {
+                        RestoreStockState();
+                        s_isAwake = false;
+                    }
+
+                    pscPmModuleAcknowledge(&s_pscModule, state);
+                }
+            }
+        }
+
+    } // namespace
+
+    Result StartBpmfwExecution() {
+        if (!IsPatchedExosphere()) {
+            fileUtils::LogLine("[bpmp] Cannot start without patched exosphere");
+            return (Result)1;
+        }
+
+        const size_t fw_size = bpmpfw_bin_size;
+        if (fw_size == 0 || fw_size > FwStagingSize) {
+            fileUtils::LogLine("[bpmp] StartBpmfwExecution: bpmpfw of %zu bytes is invalid (should not happen)!", fw_size);
+            return (Result)1;
+        }
+
+        std::memcpy(s_fwStageBuf, bpmpfw_bin, fw_size);
+
+        fileUtils::LogLine("[bpmp] StartBpmfwExecution: loading %zu bytes at 0x%llx",
+                            fw_size, static_cast<unsigned long long>(FwIramPhysBase));
+
+        /* Set BPMP reset */
+        Mmio(board::clkVirtAddr, ClkRstRstDevLSet) = RstDevLCopRstBit;
+
+        /* Grant BPMP access to all a bunch of registers that are useful */
+        Mmio(board::apbVirtAddr, ApbMiscSlaveSecurityReg0) = 0;
+        Mmio(board::apbVirtAddr, ApbMiscSlaveSecurityReg1) = 0;
+        Mmio(board::apbVirtAddr, ApbMiscSlaveSecurityReg2) = 0;
+
+        /* Set up exeption vectors for bpmp */
+        const u32 fw_reset_vector = static_cast<u32>(FwIramPhysBase + 0x0);
+        const u32 fw_panic_vector = static_cast<u32>(FwIramPhysBase + 0x4);
+
+        SmcReadWriteRegister(EvpPhysBase + EvpCopResetVector,         0xFFFFFFFF, fw_reset_vector);
+        SmcReadWriteRegister(EvpPhysBase + EvpCopUndefVector,         0xFFFFFFFF, fw_panic_vector);
+        SmcReadWriteRegister(EvpPhysBase + EvpCopSwiVector,           0xFFFFFFFF, fw_panic_vector);
+        SmcReadWriteRegister(EvpPhysBase + EvpCopPrefetchAbortVector, 0xFFFFFFFF, fw_panic_vector);
+        SmcReadWriteRegister(EvpPhysBase + EvpCopDataAbortVector,     0xFFFFFFFF, fw_panic_vector);
+        SmcReadWriteRegister(EvpPhysBase + EvpCopRsvdVector,          0xFFFFFFFF, fw_panic_vector);
+        SmcReadWriteRegister(EvpPhysBase + EvpCopIrqVector,           0xFFFFFFFF, fw_panic_vector);
+        SmcReadWriteRegister(EvpPhysBase + EvpCopFiqVector,           0xFFFFFFFF, fw_panic_vector);
+
+        /* Disable actmon monitoring to avoid hang on FW >4.0.0 */
+        Mmio(board::actmonVirtAddr, ActmonCopCtrl) = 0;
+        Mmio(board::actmonVirtAddr, ActmonCopIntrStatus) = Mmio(board::actmonVirtAddr, ActmonCopIntrStatus);
+
+        /* Load the BPMPFW into iram per page. TODO: determine if this is nessesary */
+        constexpr size_t PageSize = 4096;
+        for (size_t ofs = 0; ofs < fw_size; ofs += PageSize) {
+            const size_t chunk = (fw_size - ofs) < PageSize ? (fw_size - ofs) : PageSize;
+
+            Result copy_rc = SmcCopyToIram(FwIramPhysBase + ofs, s_fwStageBuf + ofs, static_cast<u32>(chunk));
+            if (R_FAILED(copy_rc)) {
+                fileUtils::LogLine("[bpmp] SmcCopyToIram failed at +0x%zx: 0x%x", ofs, copy_rc);
+                return copy_rc;
+            }
+        }
+
+        /* Clear BPMP reset. */
+        Mmio(board::clkVirtAddr, ClkRstRstDevLClr) = RstDevLCopRstBit;
+
+        /* Clear the halt written by bootloader. */
+        SmcReadWriteRegister(FlowCtlrPhysBase + FlowCtlrHaltCopEvents, 0xFFFFFFFF, FlowModeNone);
+
+        return 0;
+    }
+
+    void StartSleepMonitorThread() {
+        if (!IsPatchedExosphere()) {
+            fileUtils::LogLine("[bpmp] Cant start BPMP without exosphere patch");
+            return;
+        }
+
+        Result rc = pscmInitialize();
+        if (R_FAILED(rc)) {
+            fileUtils::LogLine("[bpmp] pscmInitialize failed: 0x%x", rc);
+            return;
+        }
+
+        rc = pscmGetPmModule(&s_pscModule, PscModuleId, pscDependencies, sizeof(pscDependencies) / sizeof(u32), true);
+        if (R_FAILED(rc)) {
+            fileUtils::LogLine("[bpmp] pscmGetPmModule failed: 0x%x", rc);
+            pscmExit();
+            return;
+        }
+        s_pscPrepared = true;
+
+        rc = threadCreate(&s_pscThread, PscThreadFunc, nullptr, NULL, 0x1000, 0x10, 3);
+        if (R_FAILED(rc)) {
+            fileUtils::LogLine("[bpmp] failed to create psc thread: 0x%x", rc);
+            return;
+        }
+        threadStart(&s_pscThread);
+    }
+
+    void StopSleepMonitorThread() {
+        if (!s_pscPrepared) {
+            return;
+        }
+
+        s_pscExit = true;
+        eventFire(&s_pscModule.event);
+        threadWaitForExit(&s_pscThread);
+        threadClose(&s_pscThread);
+
+        pscPmModuleFinalize(&s_pscModule);
+        pscPmModuleClose(&s_pscModule);
+        pscmExit();
+        s_pscPrepared = false;
+    }
+
+    const HocClkBpmpSharedInfo *GetSharedInfo() {
+        if (!IsPatchedExosphere()) {
+            s_sharedInfo = {};
+            return &s_sharedInfo;
+        }
+
+        SmcCopyFromIram(&s_sharedInfo, WorkRamPhysBase, sizeof(s_sharedInfo));
+        return &s_sharedInfo;
+    }
+
+    bool IsAwake() {
+        return s_isAwake;
+    }
+
+} // namespace bpmp

Source/hoc-clk/sysmodule/src/bpmp/bpmp.hpp

@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) Souldbminer, Lightos and Horizon OC Contributors
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+#include <switch.h>
+
+#include <hocclk/bpmp.h>
+
+namespace bpmp {
+    Result StartBpmfwExecution();
+
+    void StartSleepMonitorThread();
+    void StopSleepMonitorThread();
+
+    const HocClkBpmpSharedInfo *GetSharedInfo();
+    
+    // Always true on stock exosphere
+    bool IsAwake();
+}

Source/hoc-clk/sysmodule/src/file/file_utils.cpp

@@ -46,7 +46,6 @@ namespace fileUtils {
         std::atomic_bool g_has_initialized = false;
         bool g_log_enabled = false;
         bool g_uart_enabled = false;
-        std::uint64_t g_last_flag_check = 0;
 
         bool FlagExists(const char *path) {
             FILE *file = fopen(path, "r");
@@ -57,16 +56,9 @@ namespace fileUtils {
             return false;
         }
 
-        void RefreshFlags(bool force) {
-            std::uint64_t now = armTicksToNs(armGetSystemTick());
-            if (!force && (now - g_last_flag_check) < FILE_FLAG_CHECK_INTERVAL_NS) {
-                return;
-            }
-
+        void RefreshFlags() {
             g_log_enabled = FlagExists(FILE_LOG_FLAG_PATH);
             g_uart_enabled = FlagExists(FILE_UART_FLAG_PATH);
-
-            g_last_flag_check = now;
         }
 
         void InitializeThreadFunc(void *args) {
@@ -94,8 +86,6 @@ namespace fileUtils {
             return;
         }
 
-        RefreshFlags(false);
-
         if (!g_log_enabled && !g_uart_enabled) {
             return;
         }
@@ -103,7 +93,7 @@ namespace fileUtils {
         va_list args;
         va_start(args, format);
 
-        char buff[0xfff];
+        char buff[0x200];
         int len = vsnprintf(buff, sizeof(buff), format, args);
         va_end(args);
 
@@ -223,7 +213,7 @@ namespace fileUtils {
         }
 
         if (R_SUCCEEDED(rc)) {
-            RefreshFlags(true);
+            RefreshFlags();
             g_has_initialized = true;
             LogLine("=== hoc-clk " TARGET_VERSION " ===");
             LogLine("by m4xw, natinusala, p-sam, Souldbminer, Lightos_ and Dominatorul");

Source/hoc-clk/sysmodule/src/file/file_utils.hpp

@@ -35,7 +35,6 @@
 #include <vector>
 
 #define FILE_CONFIG_DIR "/config/" CONFIG_DIR
-#define FILE_FLAG_CHECK_INTERVAL_NS (10000ULL * 1000000000ULL)
 #define FILE_CONTEXT_CSV_PATH FILE_CONFIG_DIR "/context.csv"
 #define FILE_LOG_FLAG_PATH FILE_CONFIG_DIR "/log.flag"
 #define FILE_UART_FLAG_PATH FILE_CONFIG_DIR "/uart.flag"

Source/hoc-clk/sysmodule/src/main.cpp

@@ -38,7 +38,7 @@
 #include "mgr/clock_manager.hpp"
 
 
-#define INNER_HEAP_SIZE 0x40000
+#define INNER_HEAP_SIZE 0x48000
 
 extern "C" {
 void virtmemSetup(void);

Source/hoc-clk/sysmodule/src/mapping/mem_map.cpp

@@ -65,4 +65,36 @@ Result SmcCopyToIram(uintptr_t dest, const void *src, u32 size) {
         rc = (26u | ((u32)args.X[0] << 9u));
     }
     return rc;
+}
+
+bool IsPatchedExosphere() {
+    static bool checked = false;
+    static bool patched = false;
+
+    if (!checked) {
+        constexpr u64 EvpPhysBase       = 0x6000F000;
+        constexpr u64 EvpCopResetVector = 0x200;
+
+        SecmonArgs args = {};
+        args.X[0] = 0xF0000002;
+        args.X[1] = EvpPhysBase + EvpCopResetVector;
+        args.X[2] = 0;
+        args.X[3] = 0;
+        svcCallSecureMonitor(&args);
+
+        patched = (args.X[0] == 0);
+        checked = true;
+    }
+
+    return patched;
+}
+
+u32 SmcReadWriteRegister(u64 phys_addr, u32 mask, u32 value) {
+    SecmonArgs args = {};
+    args.X[0] = 0xF0000002;
+    args.X[1] = phys_addr;
+    args.X[2] = mask;
+    args.X[3] = value;
+    svcCallSecureMonitor(&args);
+    return (u32)args.X[1];
 }

Source/hoc-clk/sysmodule/src/mapping/mem_map.hpp

@@ -22,3 +22,5 @@ Result QueryMemoryMapping(u64 *virtaddr, u64 physaddr, u64 size);
 Result MapAddress(u64 &va, const u64 &physAddr, const char *name);
 Result SmcCopyFromIram(void *dest, uintptr_t src, u32 size);
 Result SmcCopyToIram(uintptr_t dest, const void *src, u32 size);
+bool IsPatchedExosphere();
+u32 SmcReadWriteRegister(u64 phys_addr, u32 mask, u32 value);

Source/hoc-clk/sysmodule/src/mgr/clock_manager.cpp

@@ -30,6 +30,7 @@
 #include <i2c.h>
 
 #include "../board/board.hpp"
+#include "../bpmp/bpmp.hpp"
 #include "../display/aula.hpp"
 #include "../display/display_refresh_rate.hpp"
 #include "../file/config.hpp"
@@ -629,6 +630,8 @@ namespace clockManager {
     bool RefreshContext() {
         bool hasChanged = false;
 
+        gContext.bpmpInfo = *bpmp::GetSharedInfo();
+
         std::uint32_t mode = 0;
         Result rc = apmExtGetCurrentPerformanceConfiguration(&mode);
         ASSERT_RESULT_OK(rc, "apmExtGetCurrentPerformanceConfiguration");
@@ -879,6 +882,11 @@ namespace clockManager {
     }
 
     void WaitForNextTick() {
+        if (!bpmp::IsAwake()) {
+            svcSleepThread(250 * 1000000ULL);  // 250ms
+            return;
+        }
+
         if (board::GetHz(HocClkModule_MEM) > 665000000)
             svcSleepThread(config::GetConfigValue(HocClkConfigValue_PollingIntervalMs) * 1000000ULL);
         else

Source/hoc-clk/sysmodule/src/tsensor/soctherm.cpp

@@ -24,6 +24,7 @@
 #include <switch.h>
 
 #include "../board/board.hpp"
+#include "../bpmp/bpmp.hpp"
 #include "../file/file_utils.hpp"
 #include "../mapping/mem_map.hpp"
 #include "soctherm.hpp"
@@ -413,15 +414,6 @@ namespace tsensor {
         bool isMariko;
     }  // namespace
 
-    bool IsDisabledThroughSleep() {
-        return (ReadReg(carVa, CLK_RST_CONTROLLER_RST_DEVICES) & SWR_SOC_THERM_RST) ||
-               !(ReadReg(carVa, CLK_RST_CONTROLLER_CLK_OUT_ENB) & CLK_ENB_SOC_THERM);
-    }
-
-    bool IsSensorEnabled() {
-        return ReadReg(socthermVa, TSENSOR_TSENSOR_CLKEN);
-    }
-
     void EnableSensor(const TSensor *sensor, u32 sensorIdx) {
         u32 val = sensor->config->tall << SENSOR_CONFIG0_TALL_SHIFT;
         WriteReg(socthermVa, sensor->base + SENSOR_CONFIG0, val);
@@ -434,21 +426,6 @@ namespace tsensor {
         WriteReg(socthermVa, sensor->base + SENSOR_CONFIG2, calib[sensorIdx]);
     }
 
-    s32 TranslateTemp(u16 val) {
-        s32 t;
-
-        t = ((val & READBACK_VALUE_MASK) >> READBACK_VALUE_SHIFT) * 1000;
-        if (val & READBACK_ADD_HALF) {
-            t += 500;
-        }
-
-        if (val & READBACK_NEGATE) {
-            t *= -1;
-        }
-
-        return t;
-    }
-
     void StartSensors() {
         u32 pdiv, hotspot;
 
@@ -474,23 +451,15 @@ namespace tsensor {
     }
 
     void ReadTSensors(TSensorTemps &temps) {
-        if (IsDisabledThroughSleep()) {
+        const HocClkBpmpSharedInfo *bpmpInfo = bpmp::GetSharedInfo();
+        if (bpmpInfo->magic != HOCCLK_BPMP_MAGIC) {
             return;
         }
 
-        if (!IsSensorEnabled()) {
-            StartSensors();
-        }
-
-        temps.cpu = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP1) >> 16);
-        temps.gpu = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP1) & SENSOR_TEMP1_GPU_TEMP_MASK);
-        temps.pllx = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP2) & SENSOR_TEMP2_PLLX_TEMP_MASK);
-
-        if (board::GetSocType() == HocClkSocType_Erista) {
-            temps.mem = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP2) >> 16);
-        } else {
-            temps.mem = -1;
-        }
+        temps.cpu = bpmpInfo->tempCpu;
+        temps.gpu = bpmpInfo->tempGpu;
+        temps.pllx = bpmpInfo->tempPllx;
+        temps.mem = (board::GetSocType() == HocClkSocType_Erista) ? bpmpInfo->tempMem : bpmpInfo->tempPllx;
     }
 
     void InitializeSoctherm() {