soc/amd/common/cpu/tsc: add get_pstate_core_freq for family 15h and 16h

This function will be used in follow-up patches for both the TSC rate
calculation and the still to be implemented P state ACPI table
generation in coreboot. The was checked against BKDG 52740 Rev 3.05,
BKDG #55072 Rev 3.04, and BKDG #50742 Rev 3.08.

Signed-off-by: Felix Held <felix-coreboot@felixheld.de>
Change-Id: I9afaa044da994d330c3e546b774eb1f82e4f30e4
Reviewed-on: https://review.coreboot.org/c/coreboot/+/74011
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Martin Roth <martin.roth@amd.corp-partner.google.com>
diff --git a/src/soc/amd/common/block/cpu/Kconfig b/src/soc/amd/common/block/cpu/Kconfig
index 391e1e5..056e133 100644
--- a/src/soc/amd/common/block/cpu/Kconfig
+++ b/src/soc/amd/common/block/cpu/Kconfig
@@ -39,6 +39,12 @@
 
 endif # SOC_AMD_COMMON_BLOCK_NONCAR
 
+config SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM15H_16H
+	bool
+	help
+	  Select this option to include code to calculate the CPU frequency
+	  from the P state MSR values on AMD CPU families 15h and 16h.
+
 config SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM17H_19H
 	bool
 	help
diff --git a/src/soc/amd/common/block/cpu/tsc/Makefile.inc b/src/soc/amd/common/block/cpu/tsc/Makefile.inc
index 6176023..77bdf77 100644
--- a/src/soc/amd/common/block/cpu/tsc/Makefile.inc
+++ b/src/soc/amd/common/block/cpu/tsc/Makefile.inc
@@ -1,17 +1,22 @@
 ## SPDX-License-Identifier: GPL-2.0-only
 
+bootblock-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM15H_16H) += cpufreq_15_16.c
 bootblock-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM17H_19H) += cpufreq_17_19.c
 bootblock-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM1AH) += cpufreq_1a.c
 
+verstage_x86-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM15H_16H) += cpufreq_15_16.c
 verstage_x86-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM17H_19H) += cpufreq_17_19.c
 verstage_x86-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM1AH) += cpufreq_1a.c
 
+romstage-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM15H_16H) += cpufreq_15_16.c
 romstage-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM17H_19H) += cpufreq_17_19.c
 romstage-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM1AH) += cpufreq_1a.c
 
+ramstage-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM15H_16H) += cpufreq_15_16.c
 ramstage-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM17H_19H) += cpufreq_17_19.c
 ramstage-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM1AH) += cpufreq_1a.c
 
+smm-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM15H_16H) += cpufreq_15_16.c
 smm-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM17H_19H) += cpufreq_17_19.c
 smm-$(CONFIG_SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM1AH) += cpufreq_1a.c
 
diff --git a/src/soc/amd/common/block/cpu/tsc/cpufreq_15_16.c b/src/soc/amd/common/block/cpu/tsc/cpufreq_15_16.c
new file mode 100644
index 0000000..579c2f8
--- /dev/null
+++ b/src/soc/amd/common/block/cpu/tsc/cpufreq_15_16.c
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+#include <amdblocks/cpu.h>
+#include <console/console.h>
+#include <soc/msr.h>
+#include <types.h>
+
+#define PSTATE_DEF_FREQ_DIV_MAX		4
+#define PSTATE_DEF_CORE_FREQ_BASE	100
+#define PSTATE_DEF_CORE_FREQ_ID_OFFSET	0x10
+
+uint32_t get_pstate_core_freq(union pstate_msr pstate_reg)
+{
+	uint32_t core_freq, core_freq_mul, core_freq_div;
+
+	/* Core frequency multiplier */
+	core_freq_mul = pstate_reg.cpu_fid_0_5;
+
+	/* Core frequency divisor ID */
+	core_freq_div = pstate_reg.cpu_dfs_id;
+
+	if (core_freq_div > PSTATE_DEF_FREQ_DIV_MAX) {
+		printk(BIOS_WARNING, "Undefined core_freq_div %x used. Force to lowest "
+				     "divider.\n", core_freq_div);
+		core_freq_div = 0;
+	}
+
+	core_freq = (PSTATE_DEF_CORE_FREQ_BASE *
+		(core_freq_mul + PSTATE_DEF_CORE_FREQ_ID_OFFSET)) / (1 << core_freq_div);
+
+	return core_freq;
+}
diff --git a/src/soc/amd/stoneyridge/Kconfig b/src/soc/amd/stoneyridge/Kconfig
index 8cc67ff..0d790a3 100644
--- a/src/soc/amd/stoneyridge/Kconfig
+++ b/src/soc/amd/stoneyridge/Kconfig
@@ -26,6 +26,7 @@
 	select SOC_AMD_COMMON_BLOCK_AOAC
 	select SOC_AMD_COMMON_BLOCK_BANKED_GPIOS
 	select SOC_AMD_COMMON_BLOCK_CAR
+	select SOC_AMD_COMMON_BLOCK_CPUFREQ_FAM15H_16H
 	select SOC_AMD_COMMON_BLOCK_HDA
 	select SOC_AMD_COMMON_BLOCK_I2C
 	select SOC_AMD_COMMON_BLOCK_IOMMU