1 files changed, 93 insertions, 1 deletions

diff --git a/src/cpu/intel/model_6ex/model_6ex_init.c b/src/cpu/intel/model_6ex/model_6ex_init.c
index 169b908901..6ecaca6d15 100644
--- a/src/cpu/intel/model_6ex/model_6ex_init.c
+++ b/src/cpu/intel/model_6ex/model_6ex_init.c
@@ -52,6 +52,76 @@ static void fill_processor_name(char *processor_name)
 	strcpy(processor_name, processor_name_start);
 }
 
+#define IA32_FEATURE_CONTROL 0x003a
+
+#define CPUID_VMX (1 << 5)
+#define CPUID_SMX (1 << 6)
+static void enable_vmx(void)
+{
+	struct cpuid_result regs;
+	msr_t msr;
+
+	msr = rdmsr(IA32_FEATURE_CONTROL);
+
+	if (msr.lo & (1 << 0)) {
+		/* VMX locked. If we set it again we get an illegal
+		 * instruction
+		 */
+		return;
+	}
+
+	regs = cpuid(1);
+	if (regs.ecx & CPUID_VMX) {
+		msr.lo |= (1 << 2);
+		if (regs.ecx & CPUID_SMX)
+			msr.lo |= (1 << 1);
+	}
+
+	wrmsr(IA32_FEATURE_CONTROL, msr);
+
+	msr.lo |= (1 << 0); /* Set lock bit */
+
+	wrmsr(IA32_FEATURE_CONTROL, msr);
+}
+
+#define PMG_CST_CONFIG_CONTROL	0xe2
+static void configure_c_states(void)
+{
+	msr_t msr;
+
+	msr = rdmsr(PMG_CST_CONFIG_CONTROL);
+	msr.lo &= ~(1 << 9); // Issue a  single stop grant cycle upon stpclk
+
+	// TODO Do we want Deep C4 and  Dynamic L2 shrinking?
+	wrmsr(PMG_CST_CONFIG_CONTROL, msr);
+}
+
+#define IA32_MISC_ENABLE	0x1a0
+static void configure_misc(void)
+{
+	msr_t msr;
+
+	msr = rdmsr(IA32_MISC_ENABLE);
+	msr.lo |= (1 << 3); 	/* TM1 enable */
+	msr.lo |= (1 << 13);	/* TM2 enable */
+	msr.lo |= (1 << 17);	/* Bidirectional PROCHOT# */
+
+	msr.lo |= (1 << 10);	/* FERR# multiplexing */
+
+	// TODO: Only if  IA32_PLATFORM_ID[17] = 0 and IA32_PLATFORM_ID[50] = 1
+	msr.lo |= (1 << 16);	/* Enhanced SpeedStep Enable */
+
+	// TODO Do we want Deep C4 and  Dynamic L2 shrinking?
+	wrmsr(IA32_MISC_ENABLE, msr);
+
+	msr.lo |= (1 << 20);	/* Lock Enhanced SpeedStep Enable */
+	wrmsr(IA32_MISC_ENABLE, msr);
+}
+
+#if CONFIG_USBDEBUG_DIRECT
+static unsigned ehci_debug_addr;
+#endif
+
 static void model_6ex_init(device_t cpu)
 {
 	char processor_name[49];
@@ -66,13 +136,35 @@ static void model_6ex_init(device_t cpu)
 	fill_processor_name(processor_name);
 	printk_info("CPU: %s.\n", processor_name);
 
+#if CONFIG_USBDEBUG_DIRECT
+	// Is this caution really needed?
+	if(!ehci_debug_addr) 
+		ehci_debug_addr = get_ehci_debug();
+	set_ehci_debug(0);
+#endif
+
 	/* Setup MTRRs */
 	x86_setup_mtrrs(36);
 	x86_mtrr_check();
-	
+
+#if CONFIG_USBDEBUG_DIRECT
+	set_ehci_debug(ehci_debug_addr);
+#endif
+
 	/* Enable the local cpu apics */
 	setup_lapic();
 
+	/* Enable virtualization */
+	enable_vmx();
+
+	/* Configure C States */
+	configure_c_states();
+
+	/* Configure Enhanced SpeedStep and Thermal Sensors */
+	configure_misc();
+
+	/* TODO: PIC thermal sensor control */
+
 	/* Start up my cpu siblings */
 	intel_sibling_init(cpu);
 }