2 files changed, 50 insertions, 0 deletions
diff --git a/src/soc/amd/stoneyridge/Kconfig b/src/soc/amd/stoneyridge/Kconfig
index 0d790a3a58..6f5c6e0775 100644
--- a/src/soc/amd/stoneyridge/Kconfig
+++ b/src/soc/amd/stoneyridge/Kconfig
@@ -39,6 +39,7 @@ config SOC_AMD_STONEYRIDGE
 	select SOC_AMD_COMMON_BLOCK_SMBUS
 	select SOC_AMD_COMMON_BLOCK_SMI
 	select SOC_AMD_COMMON_BLOCK_SMM
+	select SOC_AMD_COMMON_BLOCK_SMN
 	select SOC_AMD_COMMON_BLOCK_SPI
 	select SOC_AMD_COMMON_BLOCK_SVI2
 	select SOC_AMD_COMMON_BLOCK_UART
diff --git a/src/soc/amd/stoneyridge/cpu.c b/src/soc/amd/stoneyridge/cpu.c
index 2f4c0d09cb..c5c2f68200 100644
--- a/src/soc/amd/stoneyridge/cpu.c
+++ b/src/soc/amd/stoneyridge/cpu.c
@@ -4,6 +4,7 @@
 #include <amdblocks/iomap.h>
 #include <amdblocks/mca.h>
 #include <amdblocks/reset.h>
+#include <amdblocks/smn.h>
 #include <cpu/amd/msr.h>
 #include <cpu/cpu.h>
 #include <cpu/x86/mp.h>
@@ -14,6 +15,7 @@
 #include <soc/pci_devs.h>
 #include <soc/cpu.h>
 #include <soc/iomap.h>
+#include <soc/msr.h>
 #include <console/console.h>
 #include <types.h>
 
@@ -75,3 +77,50 @@ uint32_t get_pstate_0_reg(void)
 {
 	return (pci_read_config32(SOC_PM_DEV, CORE_PERF_BOOST_CTRL) >> 2) & 0x7;
 }
+
+static bool all_pstates_have_same_frequency_id(void)
+{
+	union pstate_msr pstate_reg;
+	size_t i;
+	bool first = true;
+	uint32_t frequency_id;
+
+	for (i = 0; i < 7; i++) {
+		pstate_reg.raw = rdmsr(PSTATE_MSR(i)).raw;
+
+		if (!pstate_reg.pstate_en)
+			continue;
+
+		if (first) {
+			frequency_id = pstate_reg.cpu_fid_0_5;
+			first = false;
+		} else if (frequency_id != pstate_reg.cpu_fid_0_5) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+#define CLK_PLL_LOCK_TIMER		0xD82220B8
+#define CLK_GATER_SEQUENCE_REGISTER	0xD8222114
+
+uint32_t get_pstate_latency(void)
+{
+	uint32_t latency = 0;
+	uint32_t smn_data;
+	uint32_t gaters_on_time, gaters_off_time;
+
+	smn_data = smn_read32(CLK_GATER_SEQUENCE_REGISTER);
+	gaters_on_time = (smn_data & 0xff) * 10;
+	gaters_off_time = (smn_data >> 8 & 0xff) * 10;
+	latency += DIV_ROUND_UP(15 * gaters_on_time, 1000);
+	latency += DIV_ROUND_UP(15 * gaters_off_time, 1000);
+
+	if (!all_pstates_have_same_frequency_id()) {
+		smn_data = smn_read32(CLK_PLL_LOCK_TIMER);
+		latency += DIV_ROUND_UP(smn_data & 0x1fff, 100);
+	}
+
+	return latency;
+}