diff --git a/.gitignore b/.gitignore index 76dc4eee..24f4283c 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,5 @@ build Source/hekate dist/* !dist/.gitignore +Source/hoc-clk/sysmodule/data/bpmpfw.bin +Source/hoc-clk/bpmpfw/out/bpmpfw.bin diff --git a/Source/Atmosphere-Patches/secmon_smc_power_management.cpp b/Source/Atmosphere-Patches/secmon_smc_power_management.cpp new file mode 100644 index 00000000..4d2214da --- /dev/null +++ b/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 . + */ +#include +#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 key_source; + GenerateCryptographicallyRandomBytes(key_source, se::AesBlockSize); + + const u32 * const key_source_32 = reinterpret_cast(static_cast(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(); + 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() + MemoryRegionVirtualTzramVolatileData.GetSize() + MemoryRegionVirtualTzramVolatileStack.GetSize(); + void * const tzram_save_dst = MemoryRegionVirtualIramSc7Work.GetPointer(); + 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(); + 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 * const debug_code_dst = MemoryRegionVirtualDramDebugDataStore.GetPointer(); + 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(MemoryRegionPhysicalIramSc7Firmware.GetAddress() + 0x0); + constexpr const u32 Sc7FirmwarePanicVector = static_cast(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(); + 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(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(); + const auto state_id = power_state.Get(); + + 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(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; + } + +} diff --git a/Source/hoc-clk/bpmpfw/Makefile b/Source/hoc-clk/bpmpfw/Makefile new file mode 100644 index 00000000..4cd0257a --- /dev/null +++ b/Source/hoc-clk/bpmpfw/Makefile @@ -0,0 +1,62 @@ +ifeq ($(strip $(DEVKITARM)),) +$(error "Please set DEVKITARM in your environment. export DEVKITARM=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) diff --git a/Source/hoc-clk/bpmpfw/bpmpfw.ld b/Source/hoc-clk/bpmpfw/bpmpfw.ld new file mode 100644 index 00000000..dc63a2ad --- /dev/null +++ b/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) } +} diff --git a/Source/hoc-clk/bpmpfw/out/bpmpfw.bin b/Source/hoc-clk/bpmpfw/out/bpmpfw.bin new file mode 100644 index 00000000..61766ba1 Binary files /dev/null and b/Source/hoc-clk/bpmpfw/out/bpmpfw.bin differ diff --git a/Source/hoc-clk/bpmpfw/source/defs.hpp b/Source/hoc-clk/bpmpfw/source/defs.hpp new file mode 100644 index 00000000..6c2b8730 --- /dev/null +++ b/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 . + * + */ + +#pragma once +#include + +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; diff --git a/Source/hoc-clk/bpmpfw/source/libc/LICENSE b/Source/hoc-clk/bpmpfw/source/libc/LICENSE new file mode 100644 index 00000000..3ea91fc4 --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/LICENSE @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2015-2016 Jurģis Brigmanis + +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. diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/assert.h b/Source/hoc-clk/bpmpfw/source/libc/include/assert.h new file mode 100644 index 00000000..a022cf62 --- /dev/null +++ b/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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_assert.h b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_assert.h new file mode 100644 index 00000000..131498df --- /dev/null +++ b/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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdio.h b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdio.h new file mode 100644 index 00000000..ffdc222d --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdio.h @@ -0,0 +1,74 @@ +#ifndef FSLC_STDIO_H +#define FSLC_STDIO_H + +#include +#include + +#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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdlib.h b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdlib.h new file mode 100644 index 00000000..021b67aa --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_stdlib.h @@ -0,0 +1,25 @@ +#ifndef FSLC_STDLIB_H +#define FSLC_STDLIB_H + +#include + +#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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_string.h b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_string.h new file mode 100644 index 00000000..a869054d --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/fslc_string.h @@ -0,0 +1,50 @@ +#ifndef FSLC_STRING_H +#define FSLC_STRING_H + +#include + +#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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/fslc/stdlibx.h b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/stdlibx.h new file mode 100644 index 00000000..2b86a1a4 --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/stdlibx.h @@ -0,0 +1,23 @@ +#ifndef FSLC_STDLIBX_H +#define FSLC_STDLIBX_H + +#include + +#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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/fslc/stringx.h b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/stringx.h new file mode 100644 index 00000000..193d8913 --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/include/fslc/stringx.h @@ -0,0 +1,33 @@ +#ifndef FSLC_STRINGX_H +#define FSLC_STRINGX_H + +#include + +#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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/stdio.h b/Source/hoc-clk/bpmpfw/source/libc/include/stdio.h new file mode 100644 index 00000000..fb4853f2 --- /dev/null +++ b/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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/stdlib.h b/Source/hoc-clk/bpmpfw/source/libc/include/stdlib.h new file mode 100644 index 00000000..34259177 --- /dev/null +++ b/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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/include/string.h b/Source/hoc-clk/bpmpfw/source/libc/include/string.h new file mode 100644 index 00000000..c35eec9c --- /dev/null +++ b/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 */ diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_bsearch.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_bsearch.c new file mode 100644 index 00000000..1f25f704 --- /dev/null +++ b/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); +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_bsearch_i.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_bsearch_i.c new file mode 100644 index 00000000..52cf54b4 --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_bsearch_i.c @@ -0,0 +1,31 @@ +#include + +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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fprintf.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fprintf.c new file mode 100644 index 00000000..688d5912 --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputc.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputc.c new file mode 100644 index 00000000..2745b827 --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputs.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputs.c new file mode 100644 index 00000000..be8d13a9 --- /dev/null +++ b/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; +} + diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputs_impl.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_fputs_impl.c new file mode 100644 index 00000000..9743de6b --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcmp.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcmp.c new file mode 100644 index 00000000..6719b878 --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcmp.c @@ -0,0 +1,38 @@ +#include "fslc_string.h" +#include + +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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcpy.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcpy.c new file mode 100644 index 00000000..2383248c --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memcpy.c @@ -0,0 +1,40 @@ +#include "fslc_string.h" +#include + +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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memmove.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memmove.c new file mode 100644 index 00000000..68426ca6 --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memmove.c @@ -0,0 +1,69 @@ +#include "fslc_string.h" +#include + +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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memset.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memset.c new file mode 100644 index 00000000..321211f8 --- /dev/null +++ b/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); +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memset_l.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memset_l.c new file mode 100644 index 00000000..f4d46336 --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_memset_l.c @@ -0,0 +1,31 @@ +#include "stringx.h" +#include + +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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_printf.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_printf.c new file mode 100644 index 00000000..2609e377 --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_putchar.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_putchar.c new file mode 100644 index 00000000..d4c1a54d --- /dev/null +++ b/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); +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_puts.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_puts.c new file mode 100644 index 00000000..c1f5d94c --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_stdout.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_stdout.c new file mode 100644 index 00000000..5418196a --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_stdout.c @@ -0,0 +1,3 @@ +#include "fslc_stdio.h" + +FSLC_FILE *fslc_stdout; diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strchr.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strchr.c new file mode 100644 index 00000000..cf3e239c --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcmp.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcmp.c new file mode 100644 index 00000000..216e147a --- /dev/null +++ b/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); +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcpy.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcpy.c new file mode 100644 index 00000000..e0fe5011 --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcpy_e.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strcpy_e.c new file mode 100644 index 00000000..313243a5 --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strlen.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strlen.c new file mode 100644 index 00000000..15543224 --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncmp.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncmp.c new file mode 100644 index 00000000..3ad69714 --- /dev/null +++ b/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); +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy.c new file mode 100644 index 00000000..13e74f4a --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy_e.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy_e.c new file mode 100644 index 00000000..b0ab743e --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy_impl.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy_impl.c new file mode 100644 index 00000000..76e80a3c --- /dev/null +++ b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strncpy_impl.c @@ -0,0 +1,10 @@ +#include + +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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strpbrk.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strpbrk.c new file mode 100644 index 00000000..743b5256 --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strspn.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strspn.c new file mode 100644 index 00000000..583a0d2d --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strstr.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strstr.c new file mode 100644 index 00000000..99966c97 --- /dev/null +++ b/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 +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strtok_r.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_strtok_r.c new file mode 100644 index 00000000..5859eb5e --- /dev/null +++ b/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; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_vfprintf.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_vfprintf.c new file mode 100644 index 00000000..53424299 --- /dev/null +++ b/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 diff --git a/Source/hoc-clk/bpmpfw/source/libc/src/fslc_vprintf.c b/Source/hoc-clk/bpmpfw/source/libc/src/fslc_vprintf.c new file mode 100644 index 00000000..6f5abd1e --- /dev/null +++ b/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); +} diff --git a/Source/hoc-clk/bpmpfw/source/libc_platform.cpp b/Source/hoc-clk/bpmpfw/source/libc_platform.cpp new file mode 100644 index 00000000..9ba02640 --- /dev/null +++ b/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 . + * + */ + +#include + +#include "regs.hpp" + +namespace { + + int UartStreamPutc(int c, FILE *) { + UartPutc(static_cast(c)); + return c; + } + + void UartStreamPostOutput(FILE *) { + UartFlush(); + } + + FILE g_uartStdout = { nullptr, UartStreamPutc, nullptr, nullptr, UartStreamPostOutput, -1 }; + +} // namespace + +void InitializeLibc() { + stdout = &g_uartStdout; +} diff --git a/Source/hoc-clk/bpmpfw/source/libc_platform.hpp b/Source/hoc-clk/bpmpfw/source/libc_platform.hpp new file mode 100644 index 00000000..f29c8b03 --- /dev/null +++ b/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 . + * + */ + +#pragma once + +void InitializeLibc(); diff --git a/Source/hoc-clk/bpmpfw/source/main.cpp b/Source/hoc-clk/bpmpfw/source/main.cpp new file mode 100644 index 00000000..2da95075 --- /dev/null +++ b/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 . + * + */ + +#include +#include + +#include "libc_platform.hpp" +#include "regs.hpp" +#include "sensors.hpp" + +#include + +namespace { + + HocClkBpmpSharedInfo &SharedInfo() { + return *reinterpret_cast(WorkRamStart); + } + +} // namespace + +void MainLoop(HocClkBpmpSharedInfo &info) { + for (;;) { + const u32 temp1 = MMIO32(SocthermBase + SocthermSensorTemp1); + const u32 temp2 = MMIO32(SocthermBase + SocthermSensorTemp2); + + info.tempCpu = TranslateSocthermTemp(static_cast(temp1 >> 16)); + info.tempGpu = TranslateSocthermTemp(static_cast(temp1)); + info.tempMem = TranslateSocthermTemp(static_cast(temp2 >> 16)); + info.tempPllx = TranslateSocthermTemp(static_cast(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(;;) + ; +} diff --git a/Source/hoc-clk/bpmpfw/source/regs.hpp b/Source/hoc-clk/bpmpfw/source/regs.hpp new file mode 100644 index 00000000..b287c27b --- /dev/null +++ b/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 . + * + */ + +#pragma once +#include "defs.hpp" + +#define MMIO32(addr) (*reinterpret_cast(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(static_cast(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]); + } +} diff --git a/Source/hoc-clk/bpmpfw/source/sensors.hpp b/Source/hoc-clk/bpmpfw/source/sensors.hpp new file mode 100644 index 00000000..64aad219 --- /dev/null +++ b/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 . + * + */ + +#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; +} diff --git a/Source/hoc-clk/bpmpfw/source/start.s b/Source/hoc-clk/bpmpfw/source/start.s new file mode 100644 index 00000000..83f2e136 --- /dev/null +++ b/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 . + * + */ + +.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 diff --git a/Source/hoc-clk/build.sh b/Source/hoc-clk/build.sh index b3de7e5f..74634101 100755 --- a/Source/hoc-clk/build.sh +++ b/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")" diff --git a/Source/hoc-clk/common/include/hocclk/board.h b/Source/hoc-clk/common/include/hocclk/board.h index 45b576d8..389ba513 100644 --- a/Source/hoc-clk/common/include/hocclk/board.h +++ b/Source/hoc-clk/common/include/hocclk/board.h @@ -30,7 +30,10 @@ #include #include #include +#ifdef __SWITCH__ // Wont be defined in BPMP-FW #include +#endif + typedef enum { HocClkSocType_Erista = 0, // T210, found in Icosa and Copper diff --git a/Source/hoc-clk/common/include/hocclk/bpmp.h b/Source/hoc-clk/common/include/hocclk/bpmp.h new file mode 100644 index 00000000..d2b5f862 --- /dev/null +++ b/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 . + * + */ + +#pragma once + +#include +#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); diff --git a/Source/hoc-clk/common/include/hocclk/clock_manager.h b/Source/hoc-clk/common/include/hocclk/clock_manager.h index 21e47646..f2338354 100644 --- a/Source/hoc-clk/common/include/hocclk/clock_manager.h +++ b/Source/hoc-clk/common/include/hocclk/clock_manager.h @@ -29,6 +29,7 @@ #include #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); \ No newline at end of file +static_assert(sizeof(HocClkContext) == 0x520); diff --git a/Source/hoc-clk/overlay/src/ui/gui/about_gui.cpp b/Source/hoc-clk/overlay/src/ui/gui/about_gui.cpp index 2febf53f..a0dd851a 100644 --- a/Source/hoc-clk/overlay/src/ui/gui/about_gui.cpp +++ b/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 { diff --git a/Source/hoc-clk/sysmodule/Makefile b/Source/hoc-clk/sysmodule/Makefile index 03b7eb07..01693f80 100644 --- a/Source/hoc-clk/sysmodule/Makefile +++ b/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 diff --git a/Source/hoc-clk/sysmodule/perms.json b/Source/hoc-clk/sysmodule/perms.json index 01728ddf..57e821af 100644 --- a/Source/hoc-clk/sysmodule/perms.json +++ b/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, diff --git a/Source/hoc-clk/sysmodule/src/board/board.cpp b/Source/hoc-clk/sysmodule/src/board/board.cpp index 8923374a..da191e6d 100644 --- a/Source/hoc-clk/sysmodule/src/board/board.cpp +++ b/Source/hoc-clk/sysmodule/src/board/board.cpp @@ -35,6 +35,7 @@ #include #include +#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 { diff --git a/Source/hoc-clk/sysmodule/src/board/board.hpp b/Source/hoc-clk/sysmodule/src/board/board.hpp index 0a50622b..313400af 100644 --- a/Source/hoc-clk/sysmodule/src/board/board.hpp +++ b/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; diff --git a/Source/hoc-clk/sysmodule/src/bpmp/bpmp.cpp b/Source/hoc-clk/sysmodule/src/bpmp/bpmp.cpp new file mode 100644 index 00000000..bf1824cf --- /dev/null +++ b/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 . + * + */ + +#include +#include +#include + +#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(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(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(FwIramPhysBase + 0x0); + const u32 fw_panic_vector = static_cast(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(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 diff --git a/Source/hoc-clk/sysmodule/src/bpmp/bpmp.hpp b/Source/hoc-clk/sysmodule/src/bpmp/bpmp.hpp new file mode 100644 index 00000000..f89810bb --- /dev/null +++ b/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 . + * + */ + +#pragma once +#include + +#include + +namespace bpmp { + Result StartBpmfwExecution(); + + void StartSleepMonitorThread(); + void StopSleepMonitorThread(); + + const HocClkBpmpSharedInfo *GetSharedInfo(); + + // Always true on stock exosphere + bool IsAwake(); +} diff --git a/Source/hoc-clk/sysmodule/src/file/file_utils.cpp b/Source/hoc-clk/sysmodule/src/file/file_utils.cpp index 8af7e964..107a2b73 100644 --- a/Source/hoc-clk/sysmodule/src/file/file_utils.cpp +++ b/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"); diff --git a/Source/hoc-clk/sysmodule/src/file/file_utils.hpp b/Source/hoc-clk/sysmodule/src/file/file_utils.hpp index 69991f86..f2ab4b2a 100644 --- a/Source/hoc-clk/sysmodule/src/file/file_utils.hpp +++ b/Source/hoc-clk/sysmodule/src/file/file_utils.hpp @@ -35,7 +35,6 @@ #include #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" diff --git a/Source/hoc-clk/sysmodule/src/main.cpp b/Source/hoc-clk/sysmodule/src/main.cpp index f7c5b0c1..f2452f05 100644 --- a/Source/hoc-clk/sysmodule/src/main.cpp +++ b/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); diff --git a/Source/hoc-clk/sysmodule/src/mapping/mem_map.cpp b/Source/hoc-clk/sysmodule/src/mapping/mem_map.cpp index ae34021a..f0d52b4d 100644 --- a/Source/hoc-clk/sysmodule/src/mapping/mem_map.cpp +++ b/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]; } \ No newline at end of file diff --git a/Source/hoc-clk/sysmodule/src/mapping/mem_map.hpp b/Source/hoc-clk/sysmodule/src/mapping/mem_map.hpp index 6cb6e1be..fa5900aa 100644 --- a/Source/hoc-clk/sysmodule/src/mapping/mem_map.hpp +++ b/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); diff --git a/Source/hoc-clk/sysmodule/src/mgr/clock_manager.cpp b/Source/hoc-clk/sysmodule/src/mgr/clock_manager.cpp index 9448fc6b..487d1f11 100644 --- a/Source/hoc-clk/sysmodule/src/mgr/clock_manager.cpp +++ b/Source/hoc-clk/sysmodule/src/mgr/clock_manager.cpp @@ -30,6 +30,7 @@ #include #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 diff --git a/Source/hoc-clk/sysmodule/src/tsensor/soctherm.cpp b/Source/hoc-clk/sysmodule/src/tsensor/soctherm.cpp index 6a053f46..af56a833 100644 --- a/Source/hoc-clk/sysmodule/src/tsensor/soctherm.cpp +++ b/Source/hoc-clk/sysmodule/src/tsensor/soctherm.cpp @@ -24,6 +24,7 @@ #include #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() { diff --git a/build.sh b/build.sh index 31712ab7..edceb3b6 100755 --- a/build.sh +++ b/build.sh @@ -120,6 +120,7 @@ if [ "$NO_EXO" -eq 0 ]; then cp -v "$EXO_SRC/secmon_exception_vectors_access_table_data.inc" "$EXO_DEST/" cp -v "$EXO_SRC/secmon_smc_register_access.cpp" "$EXO_DEST/" cp -v "$EXO_SRC/secmon_smc_handler.cpp" "$EXO_DEST/" + cp -v "$EXO_SRC/secmon_smc_power_management.cpp" "$EXO_DEST/" cp -v "$EXO_SRC/secmon_memory_layout.hpp" "$LIBEXO_DEST/" fi