4 files changed, 42 insertions, 2 deletions
diff --git a/src/soc/intel/apollolake/chip.c b/src/soc/intel/apollolake/chip.c
index 181d4d68bc..d722992c58 100644
--- a/src/soc/intel/apollolake/chip.c
+++ b/src/soc/intel/apollolake/chip.c
@@ -461,6 +461,7 @@ void platform_fsp_silicon_init_params_cb(FSPS_UPD *silupd)
 		silconfig->EmmcRxCmdDataCntl2 = cfg->emmc_rx_cmd_data_cntl2;
 
 	silconfig->LPSS_S0ixEnable = cfg->lpss_s0ix_enable;
+	silconfig->SkipMpInit = 1;
 
 	/* Disable monitor mwait since it is broken due to a hardware bug without a fix */
 	silconfig->MonitorMwaitEnable = 0;
diff --git a/src/soc/intel/apollolake/cpu.c b/src/soc/intel/apollolake/cpu.c
index e67842c369..bd5cff50a9 100644
--- a/src/soc/intel/apollolake/cpu.c
+++ b/src/soc/intel/apollolake/cpu.c
@@ -25,11 +25,32 @@
 #include <cpu/x86/mtrr.h>
 #include <device/device.h>
 #include <device/pci.h>
+#include <reg_script.h>
 #include <soc/cpu.h>
+#include <soc/iomap.h>
 #include <soc/smm.h>
 
+static const struct reg_script core_msr_script[] = {
+	/* Enable C-state and IO/MWAIT redirect */
+	REG_MSR_WRITE(MSR_PMG_CST_CONFIG_CONTROL,
+		(PKG_C_STATE_LIMIT_C2_MASK | CORE_C_STATE_LIMIT_C10_MASK
+		| IO_MWAIT_REDIRECT_MASK | CST_CFG_LOCK_MASK)),
+	/* Power Management I/O base address for I/O trapping to C-states */
+	REG_MSR_WRITE(MSR_PMG_IO_CAPTURE_BASE,
+		(ACPI_PMIO_CST_REG | (PMG_IO_BASE_CST_RNG_BLK_SIZE << 16))),
+	/* Disable C1E */
+	REG_MSR_RMW(MSR_POWER_CTL, ~0x2, 0),
+	REG_SCRIPT_END
+};
+
+static void soc_core_init(device_t cpu)
+{
+	/* Set core MSRs */
+	reg_script_run(core_msr_script);
+}
+
 static struct device_operations cpu_dev_ops = {
-	.init = DEVICE_NOOP,
+	.init = soc_core_init,
 };
 
 static struct cpu_device_id cpu_table[] = {
diff --git a/src/soc/intel/apollolake/include/soc/cpu.h b/src/soc/intel/apollolake/include/soc/cpu.h
index bffe4bcd8d..e94972d90b 100644
--- a/src/soc/intel/apollolake/include/soc/cpu.h
+++ b/src/soc/intel/apollolake/include/soc/cpu.h
@@ -54,6 +54,19 @@ void apollolake_init_cpus(struct device *dev);
  */
 #define MB_POWER_LIMIT1_TIME_DEFAULT	0x6e
 
+/* Set MSR_PMG_CST_CONFIG_CONTROL[3:0] for Package C-State limit */
+#define PKG_C_STATE_LIMIT_C2_MASK	0x2
+/* Set MSR_PMG_CST_CONFIG_CONTROL[7:4] for Core C-State limit*/
+#define CORE_C_STATE_LIMIT_C10_MASK	0x70
+/* Set MSR_PMG_CST_CONFIG_CONTROL[10] to IO redirect to MWAIT */
+#define IO_MWAIT_REDIRECT_MASK	0x400
+/* Set MSR_PMG_CST_CONFIG_CONTROL[15] to lock CST_CFG [0-15] bits */
+#define CST_CFG_LOCK_MASK	0x8000
+
+#define MSR_PMG_CST_CONFIG_CONTROL	0xe2
+#define MSR_PMG_IO_CAPTURE_BASE	0xe4
+#define MSR_POWER_CTL	0x1fc
+
 #define MSR_L2_QOS_MASK(reg)		(0xd10 + reg)
 #define MSR_IA32_PQR_ASSOC		0xc8f
 /* MSR bits 33:32 encode slot number 0-3 */
diff --git a/src/soc/intel/apollolake/include/soc/iomap.h b/src/soc/intel/apollolake/include/soc/iomap.h
index 621b0a6808..d5d8f878ef 100644
--- a/src/soc/intel/apollolake/include/soc/iomap.h
+++ b/src/soc/intel/apollolake/include/soc/iomap.h
@@ -27,7 +27,12 @@
 
 #define ACPI_PMIO_BASE			0x400
 #define ACPI_PMIO_SIZE			0x100
-#define  R_ACPI_PM1_TMR			0x8
+#define R_ACPI_PM1_TMR			0x8
+
+/* CST Range (R/W) IO port block size */
+#define PMG_IO_BASE_CST_RNG_BLK_SIZE	0x5
+/* ACPI PMIO Offset to C-state register*/
+#define ACPI_PMIO_CST_REG	(ACPI_PMIO_BASE + 0x14)
 
 /* Accesses to these BARs are hardcoded in FSP */
 #define PMC_BAR0			0xfe042000