3 files changed, 70 insertions, 3 deletions

diff --git a/src/cpu/intel/haswell/romstage.c b/src/cpu/intel/haswell/romstage.c
index a25f8836be..688f3579db 100644
--- a/src/cpu/intel/haswell/romstage.c
+++ b/src/cpu/intel/haswell/romstage.c
@@ -143,6 +143,8 @@ void romstage_common(const struct romstage_params *params)
 	#endif
 	}
 
+	haswell_unhide_peg();
+
 	setup_sdram_meminfo(params->pei_data);
 
 	romstage_handoff_init(wake_from_s3);
diff --git a/src/northbridge/intel/haswell/early_init.c b/src/northbridge/intel/haswell/early_init.c
index e31bb0e72c..75fc7a240d 100644
--- a/src/northbridge/intel/haswell/early_init.c
+++ b/src/northbridge/intel/haswell/early_init.c
@@ -19,9 +19,12 @@
 #include <console/console.h>
 #include <arch/io.h>
 #include <device/pci_def.h>
+#include <device/pci_ops.h>
 #include <elog.h>
 #include "haswell.h"
 
+static bool peg_hidden[3];
+
 static void haswell_setup_bars(void)
 {
 	printk(BIOS_DEBUG, "Setting up static northbridge registers...");
@@ -45,13 +48,13 @@ static void haswell_setup_bars(void)
 	printk(BIOS_DEBUG, " done.\n");
 }
 
-static void haswell_setup_graphics(void)
+static void haswell_setup_igd(void)
 {
 	bool igd_enabled;
 	u16 ggc;
 	u8 reg8;
 
-	printk(BIOS_DEBUG, "Initializing Graphics...\n");
+	printk(BIOS_DEBUG, "Initializing IGD...\n");
 
 	igd_enabled = !!(pci_read_config32(PCI_DEV(0, 0, 0), DEVEN)
 			 & DEVEN_D2EN);
@@ -79,6 +82,66 @@ static void haswell_setup_graphics(void)
 	pci_write_config8(PCI_DEV(0, 2, 0), MSAC, reg8);
 }
 
+static void start_peg2_link_training(const pci_devfn_t dev)
+{
+	u32 mask;
+
+	switch (dev) {
+	case PCI_DEV(0, 1, 2):
+		mask = DEVEN_D1F2EN;
+		break;
+	case PCI_DEV(0, 1, 1):
+		mask = DEVEN_D1F1EN;
+		break;
+	case PCI_DEV(0, 1, 0):
+		mask = DEVEN_D1F0EN;
+		break;
+	default:
+		printk(BIOS_ERR, "Link training tried on a non-PEG device!\n");
+		return;
+	}
+
+	pci_update_config32(dev, 0xc24, ~(1 << 16), 1 << 5);
+	printk(BIOS_DEBUG, "Started PEG1%d link training.\n", PCI_FUNC(dev));
+
+	/*
+	 * The PEG device is hidden while the MRC runs. This is because the
+	 * MRC makes configurations that are not ideal if it sees a VGA
+	 * device in a PEG slot, and it locks registers preventing changes
+	 * to these configurations.
+	 */
+	pci_update_config32(PCI_DEV(0, 0, 0), DEVEN, ~mask, 0);
+	peg_hidden[PCI_FUNC(dev)] = true;
+	printk(BIOS_DEBUG, "Temporarily hiding PEG1%d.\n", PCI_FUNC(dev));
+}
+
+void haswell_unhide_peg(void)
+{
+	u32 deven = pci_read_config32(PCI_DEV(0, 0, 0), DEVEN);
+
+	for (u8 fn = 0; fn <= 2; fn++) {
+		if (peg_hidden[fn]) {
+			deven |= DEVEN_D1F0EN >> fn;
+			peg_hidden[fn] = false;
+			printk(BIOS_DEBUG, "Unhiding PEG1%d.\n", fn);
+		}
+	}
+
+	pci_write_config32(PCI_DEV(0, 0, 0), DEVEN, deven);
+}
+
+static void haswell_setup_peg(void)
+{
+	u32 deven = pci_read_config32(PCI_DEV(0, 0, 0), DEVEN);
+
+	if (deven & DEVEN_D1F2EN)
+		start_peg2_link_training(PCI_DEV(0, 1, 2));
+	if (deven & DEVEN_D1F1EN)
+		start_peg2_link_training(PCI_DEV(0, 1, 1));
+	if (deven & DEVEN_D1F0EN)
+		start_peg2_link_training(PCI_DEV(0, 1, 0));
+}
+
 static void haswell_setup_misc(void)
 {
 	u32 reg32;
@@ -140,7 +203,8 @@ void haswell_early_initialization(int chipset_type)
 	/* Setup IOMMU BARs */
 	haswell_setup_iommu();
 
-	haswell_setup_graphics();
+	haswell_setup_peg();
+	haswell_setup_igd();
 
 	haswell_setup_misc();
 }
diff --git a/src/northbridge/intel/haswell/haswell.h b/src/northbridge/intel/haswell/haswell.h
index bc2fc60e40..f67995d4e9 100644
--- a/src/northbridge/intel/haswell/haswell.h
+++ b/src/northbridge/intel/haswell/haswell.h
@@ -221,6 +221,7 @@ void intel_northbridge_haswell_finalize_smm(void);
 void haswell_early_initialization(int chipset_type);
 void haswell_late_initialization(void);
 void set_translation_table(int start, int end, u64 base, int inc);
+void haswell_unhide_peg(void);
 
 /* debugging functions */
 void print_pci_devices(void);