diff --git a/fusee_cpp/program/source/mtc/fusee_mtc_erista.cpp b/fusee_cpp/program/source/mtc/fusee_mtc_erista.cpp index 0b6e5a857..f2ec81035 100644 --- a/fusee_cpp/program/source/mtc/fusee_mtc_erista.cpp +++ b/fusee_cpp/program/source/mtc/fusee_mtc_erista.cpp @@ -22,6 +22,10 @@ namespace ams::nxboot { namespace { + constexpr inline const uintptr_t CLKRST = secmon::MemoryRegionPhysicalDeviceClkRst.GetAddress(); + + static constinit bool g_next_pll = false; + #include "fusee_mtc_tables_erista.inc" using EmcDvfsTimingTable = erista::EmcDvfsTimingTable; @@ -41,6 +45,171 @@ namespace ams::nxboot { } } + bool IsSamePll(u32 next_2x, u32 prev_2x) { + if (next_2x == prev_2x) { + return true; + } else if ((next_2x == PLLM_OUT0 || next_2x == PLLM_UD) && (prev_2x == PLLM_OUT0 || prev_2x == PLLM_UD)) { + return true; + } else { + return false; + } + } + + bool PllReprogram(u32 next_rate_khz, u32 next_clk_src, u32 prev_rate_khz, u32 prev_clk_src) { + /* Get current pll/divp value. */ + u32 pll_base, pll_p; + switch (reg::GetValue(CLKRST + CLK_RST_CONTROLLER_CLK_SOURCE_EMC, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_SRC))) { + case PLLM_UD: + case PLLM_OUT0: + pll_base = reg::Read(CLKRST + CLK_RST_CONTROLLER_PLLM_BASE); + pll_p = reg::GetField(pll_base, CLK_RST_REG_BITS_MASK(PLLM_BASE_PLLM_DIVP)); + break; + case PLLMB_UD: + case PLLMB_OUT0: + pll_base = reg::Read(CLKRST + CLK_RST_CONTROLLER_PLLMB_BASE); + pll_p = reg::GetField(pll_base, CLK_RST_REG_BITS_MASK(PLLMB_BASE_PLLMB_DIVP)); + break; + default: + pll_base = 0; + pll_p = 0; + } + + /* Check pll divp. */ + if (pll_p > 5) { + ShowFatalError("Invalid PLL divp: %" PRIu32 "\n", pll_p); + } + + /* Get clk src/divisor. */ + const u32 next_2x = reg::GetField(next_clk_src, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_SRC)); + const u32 prev_2x = reg::GetField(prev_clk_src, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_SRC)); + u32 next_div = reg::GetField(next_clk_src, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR)); + u32 prev_div = reg::GetField(prev_clk_src, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR)); + + /* Update divisor, if necessary. */ + if (next_2x == PLLM_UD || next_2x == PLLMB_UD) { + next_div = 0; + } + if (prev_2x == PLLM_UD || prev_2x == PLLMB_UD) { + prev_div = 0; + } + + /* If the pll is different, reprogramming is necessary. */ + if (!IsSamePll(next_2x, prev_2x)) { + return true; + } + + /* Return whether the ratios are different. */ + const float next_freq = next_rate_khz * (1 + (next_div >> 1) + (0.5 * (next_div & 1))) * (pll_p + 1); + const float prev_freq = prev_rate_khz * (1 + (prev_div >> 1) + (0.5 * (prev_div & 1))) * (pll_p + 1); + + const float ratio = prev_freq / next_freq; + + return ratio > 1.01 || ratio < 0.99; + } + + u32 ProgramPllm(u32 next_rate_khz, u32 next_clk_src, bool is_pllmb) { + /* Hardcode values for 1600MHz. */ + if (next_rate_khz != 1600000) { + ShowFatalError("Unexpected ProgramPllm next rate %" PRIu32 "\n", next_rate_khz); + } + + const u32 divn = 0x7D; + const u32 divm = 0x03; + const u32 divp = 0x00; + + const auto next_2x = reg::GetField(next_clk_src, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_SRC)); + if (is_pllmb) { + /* Set divisors. */ + reg::Write(CLKRST + CLK_RST_CONTROLLER_PLLMB_BASE, CLK_RST_REG_BITS_VALUE(PLLMB_BASE_PLLMB_DIVM, divm), + CLK_RST_REG_BITS_VALUE(PLLMB_BASE_PLLMB_DIVN, divn), + CLK_RST_REG_BITS_VALUE(PLLMB_BASE_PLLMB_DIVP, divp)); + reg::Read(CLKRST + CLK_RST_CONTROLLER_PLLMB_BASE); + + /* Set enable. */ + reg::ReadWrite(CLKRST + CLK_RST_CONTROLLER_PLLMB_BASE, CLK_RST_REG_BITS_ENUM(PLLMB_BASE_PLLMB_ENABLE, ENABLE)); + + /* Adjust next clock source. */ + if (next_2x == PLLM_UD) { + reg::SetField(next_clk_src, CLK_RST_REG_BITS_VALUE(CLK_SOURCE_EMC_EMC_2X_CLK_SRC, PLLMB_UD)); + } else if (next_2x == PLLM_OUT0) { + reg::SetField(next_clk_src, CLK_RST_REG_BITS_VALUE(CLK_SOURCE_EMC_EMC_2X_CLK_SRC, PLLMB_OUT0)); + } + + /* Wait for pll to lock. */ + while (!reg::HasValue(CLKRST + CLK_RST_CONTROLLER_PLLMB_BASE, CLK_RST_REG_BITS_ENUM(PLLMB_BASE_PLLMB_LOCK, LOCK))) { + /* ... */ + } + } else { + /* Set divisors. */ + reg::Write(CLKRST + CLK_RST_CONTROLLER_PLLM_BASE, CLK_RST_REG_BITS_VALUE(PLLM_BASE_PLLM_DIVM, divm), + CLK_RST_REG_BITS_VALUE(PLLM_BASE_PLLM_DIVN, divn), + CLK_RST_REG_BITS_VALUE(PLLM_BASE_PLLM_DIVP, divp)); + reg::Read(CLKRST + CLK_RST_CONTROLLER_PLLM_BASE); + + /* Set LKCDET. */ + reg::ReadWrite(CLKRST + CLK_RST_CONTROLLER_PLLM_MISC2, CLK_RST_REG_BITS_ENUM(PLLM_MISC2_PLLM_EN_LCKDET, ENABLE)); + + /* Set enable. */ + reg::ReadWrite(CLKRST + CLK_RST_CONTROLLER_PLLM_BASE, CLK_RST_REG_BITS_ENUM(PLLM_BASE_PLLM_ENABLE, ENABLE)); + + /* Adjust next clock source. */ + if (next_2x == PLLM_UD) { + reg::SetField(next_clk_src, CLK_RST_REG_BITS_VALUE(CLK_SOURCE_EMC_EMC_2X_CLK_SRC, PLLM_UD)); + } else if (next_2x == PLLM_OUT0) { + reg::SetField(next_clk_src, CLK_RST_REG_BITS_VALUE(CLK_SOURCE_EMC_EMC_2X_CLK_SRC, PLLM_OUT0)); + } + + /* Wait for pll to lock. */ + while (!reg::HasValue(CLKRST + CLK_RST_CONTROLLER_PLLM_BASE, CLK_RST_REG_BITS_ENUM(PLLM_BASE_PLLM_LOCK, LOCK))) { + /* ... */ + } + } + + return next_clk_src; + } + + void Dvfs(EmcDvfsTimingTable *dst_timing_tables, EmcDvfsTimingTable *src_timing_tables, bool train) { + /* Get the old 2x clock source. */ + const u32 prev_2x_clk_src = reg::GetValue(CLKRST + CLK_RST_CONTROLLER_CLK_SOURCE_EMC, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_SRC)); + + /* Set g_next_pll. */ + g_next_pll = prev_2x_clk_src == PLLMB_UD || prev_2x_clk_src == PLLMB_OUT0; + + /* Reprogram pll. */ + u32 next_clk_src; + if (PllReprogram(dst_timing_tables->rate_khz, dst_timing_tables->clk_src_emc, src_timing_tables->rate_khz, src_timing_tables->clk_src_emc)) { + if (prev_2x_clk_src == PLLMB_UD || prev_2x_clk_src == PLLMB_OUT0) { + g_next_pll = 0; + } else if (prev_2x_clk_src == PLLM_UD || prev_2x_clk_src == PLLM_OUT0) { + g_next_pll = !g_next_pll; + } + + next_clk_src = ProgramPllm(dst_timing_tables->rate_khz, dst_timing_tables->clk_src_emc, g_next_pll); + } else { + next_clk_src = dst_timing_tables->clk_src_emc; + + const u32 next_2x_clk_src = reg::GetField(next_clk_src, CLK_RST_REG_BITS_MASK(CLK_SOURCE_EMC_EMC_2X_CLK_SRC)); + if (next_2x_clk_src == PLLM_UD || next_2x_clk_src == PLLMB_UD) { + if (g_next_pll) { + reg::SetField(next_clk_src, CLK_RST_REG_BITS_VALUE(CLK_SOURCE_EMC_EMC_2X_CLK_SRC, PLLMB_UD)); + } + } else if (next_2x_clk_src == PLLM_OUT0 || next_2x_clk_src == PLLMB_OUT0) { + if (g_next_pll) { + reg::SetField(next_clk_src, CLK_RST_REG_BITS_VALUE(CLK_SOURCE_EMC_EMC_2X_CLK_SRC, PLLMB_OUT0)); + } + } + } + + if (train) { + TrainFreq(src_timing_tables, dst_timing_tables, next_clk_src); + if (PllReprogram(dst_timing_tables->rate_khz, dst_timing_tables->clk_src_emc, src_timing_tables->rate_khz, src_timing_tables->clk_src_emc)) { + g_next_pll = !g_next_pll; + } + } else { + FreqChange(src_timing_tables, dst_timing_tables, next_clk_src); + } + } + } void DoMemoryTrainingErista() { @@ -54,7 +223,20 @@ namespace ams::nxboot { ShowFatalError("EmcDvfsTimingTables seem corrupted %" PRIu32 " %" PRIu32 "?\n", src_timing_tables->rate_khz, dst_timing_tables->rate_khz); } - /* TODO */ + /* Check that we should do training. */ + if (src_timing_tables->clk_src_emc != reg::Read(CLKRST + CLK_RST_CONTROLLER_CLK_SOURCE_EMC)) { + /* Our clock source isn't what's expected, so presumably training has already been done? */ + /* Either way, the safe bet is to skip it. */ + return; + } + + /* Train 1600MHz. */ + Dvfs(dst_timing_tables, src_timing_tables, true); + + /* Switch to 1600MHz. */ + Dvfs(dst_timing_tables, src_timing_tables, false); + + /* TODO: Periodic compensation */ } } diff --git a/fusee_cpp/program/source/mtc/fusee_mtc_timing_table_common.hpp b/fusee_cpp/program/source/mtc/fusee_mtc_timing_table_common.hpp index adf24c507..ae10255e2 100644 --- a/fusee_cpp/program/source/mtc/fusee_mtc_timing_table_common.hpp +++ b/fusee_cpp/program/source/mtc/fusee_mtc_timing_table_common.hpp @@ -23,6 +23,17 @@ namespace ams::nxboot { #define EMC0_BASE (0x7001E000) #define EMC1_BASE (0x7001F000) + enum { + PLLM_OUT0 = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_PLLM_OUT0, + PLLC_OUT0 = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_PLLC_OUT0, + PLLP_OUT0 = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_PLLP_OUT0, + CLK_M = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_CLK_M, + PLLM_UD = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_PLLM_UD, + PLLMB_UD = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_PLLMB_UD, + PLLMB_OUT0 = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_PLLMB_OUT0, + PLLP_UD = CLK_RST_CONTROLLER_CLK_SOURCE_EMC_EMC_2X_CLK_SRC_PLLP_UD + }; + #define FOREACH_PER_CHANNEL_BURST_REG(HANDLER) \ HANDLER(EMC0, EMC_MRW10, emc0_mrw10) \ HANDLER(EMC1, EMC_MRW10, emc1_mrw10) \ diff --git a/libraries/libvapours/include/vapours/reg.hpp b/libraries/libvapours/include/vapours/reg.hpp index 7cf770312..d8e6be43a 100644 --- a/libraries/libvapours/include/vapours/reg.hpp +++ b/libraries/libvapours/include/vapours/reg.hpp @@ -56,6 +56,12 @@ namespace ams::reg { return (EncodeMask(masks) | ...); } + template requires UnsignedNonConstIntegral + constexpr ALWAYS_INLINE IntType GetField(const IntType &value, const BitsMask mask) { return (value & EncodeMask(mask)) >> GetOffset(mask); } + + template requires UnsignedNonConstIntegral + constexpr ALWAYS_INLINE void SetField(IntType &value, const BitsValue v) { value = (value & ~EncodeMask(v)) | EncodeValue(v); } + template requires UnsignedNonConstIntegral ALWAYS_INLINE void Write(volatile IntType *reg, std::type_identity_t val) { *reg = val; } diff --git a/libraries/libvapours/include/vapours/tegra/tegra_clkrst.hpp b/libraries/libvapours/include/vapours/tegra/tegra_clkrst.hpp index b1f8a859e..bd3a660b4 100644 --- a/libraries/libvapours/include/vapours/tegra/tegra_clkrst.hpp +++ b/libraries/libvapours/include/vapours/tegra/tegra_clkrst.hpp @@ -118,6 +118,8 @@ DEFINE_CLK_RST_REG_BIT_ENUM(PLLM_BASE_PLLM_REF_DIS, 29, REF_ENABLE, REF_DISABLE) DEFINE_CLK_RST_REG_BIT_ENUM(PLLM_BASE_PLLM_ENABLE, 30, DISABLE, ENABLE); DEFINE_CLK_RST_REG_BIT_ENUM(PLLM_BASE_PLLM_BYPASSPLL, 31, DISABLE, ENABLE); +DEFINE_CLK_RST_REG_BIT_ENUM(PLLM_MISC2_PLLM_EN_LCKDET, 4, DISABLE, ENABLE); + DEFINE_CLK_RST_REG_BIT_ENUM(PLLD_BASE_CSI_CLK_SRC, 23, BRICK, PLL_D); DEFINE_CLK_RST_REG_BIT_ENUM(PLLD_BASE_PLLD_REF_DIS, 29, REF_ENABLE, REF_DISABLE); DEFINE_CLK_RST_REG_BIT_ENUM(PLLD_BASE_PLLD_ENABLE, 30, DISABLE, ENABLE); @@ -149,6 +151,10 @@ DEFINE_CLK_RST_REG_BIT_ENUM(PLLC4_BASE_PLLC4_IDDQ, 18, OFF, ON); DEFINE_CLK_RST_REG_BIT_ENUM(PLLC4_BASE_PLLC4_LOCK, 27, NOT_LOCK, LOCK_FEQ_AND_PHASE); DEFINE_CLK_RST_REG_BIT_ENUM(PLLC4_BASE_PLLC4_ENABLE, 30, DISABLE, ENABLE); +DEFINE_CLK_RST_REG(PLLMB_BASE_PLLMB_DIVM, 0, 8); +DEFINE_CLK_RST_REG(PLLMB_BASE_PLLMB_DIVN, 8, 8); +DEFINE_CLK_RST_REG(PLLMB_BASE_PLLMB_DIVP, 20, 5); +DEFINE_CLK_RST_REG_BIT_ENUM(PLLMB_BASE_PLLMB_LOCK, 27, NOT_LOCK, LOCK); DEFINE_CLK_RST_REG_BIT_ENUM(PLLMB_BASE_PLLMB_ENABLE, 30, DISABLE, ENABLE); /* RST_DEVICES */ @@ -332,6 +338,7 @@ DEFINE_CLK_RST_REG_THREE_BIT_ENUM(CLK_SOURCE_VI_VI_CLK_SRC, 29, RESERVED0, PLLC2 DEFINE_CLK_RST_REG_THREE_BIT_ENUM(CLK_SOURCE_HOST1X_HOST1X_CLK_SRC, 29, PLLC4_OUT1, PLLC2_OUT0, PLLC_OUT0, PLLC4_OUT2, PLLP_OUT0, CLK_M, PLLA_OUT0, PLLC4_OUT0); +DEFINE_CLK_RST_REG(CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR, 0, 8); DEFINE_CLK_RST_REG_THREE_BIT_ENUM(CLK_SOURCE_EMC_EMC_2X_CLK_SRC, 29, PLLM_OUT0, PLLC_OUT0, PLLP_OUT0, CLK_M, PLLM_UD, PLLMB_UD, PLLMB_OUT0, PLLP_UD); DEFINE_CLK_RST_REG_THREE_BIT_ENUM(CLK_SOURCE_NVENC_NVENC_CLK_SRC, 29, RESERVED0, PLLC2_OUT0, PLLC_OUT0, PLLC3_OUT0, PLLP_OUT0, RESERVED5, PLLA1_OUT0, CLK_M);