northbridge/intel/sandybridge: Enable PEG clock-gating on demand

Activate PEG clock-gating only if all PEG devices are disabled.
Fixes system hang when trying to access PEG registers.

Test system:
     * Intel Pentium CPU G2130
     * Gigabyte GA-B75M-D3H

Change-Id: I7d62fbb83c16741965639cea1a0e4978d4e3d6da
Signed-off-by: Patrick Rudolph <siro@das-labor.org>
Signed-off-by: Alexandru Gagniuc <mr.nuke.me@gmail.com>
Reviewed-on: http://review.coreboot.org/11059
Tested-by: build bot (Jenkins)

3 files changed, 48 insertions, 15 deletions

diff --git a/src/northbridge/intel/sandybridge/northbridge.c b/src/northbridge/intel/sandybridge/northbridge.c
index 0e0ba00cf1..76f03f3ba9 100644
--- a/src/northbridge/intel/sandybridge/northbridge.c
+++ b/src/northbridge/intel/sandybridge/northbridge.c
@@ -361,6 +361,51 @@ static void northbridge_dmi_init(struct device *dev)
 	DMIBAR32(0x88) = reg32;
 }
 
+/* Disable unused PEG devices based on devicetree */
+static void disable_peg(void)
+{
+	struct device *dev;
+	u32 reg;
+
+	dev = dev_find_slot(0, PCI_DEVFN(0, 0));
+	reg = pci_read_config32(dev, DEVEN);
+
+	dev = dev_find_slot(0, PCI_DEVFN(1, 2));
+	if (!dev || !dev->enabled) {
+		printk(BIOS_DEBUG, "Disabling PEG12.\n");
+		reg &= ~DEVEN_PEG12;
+	}
+	dev = dev_find_slot(0, PCI_DEVFN(1, 1));
+	if (!dev || !dev->enabled) {
+		printk(BIOS_DEBUG, "Disabling PEG11.\n");
+		reg &= ~DEVEN_PEG11;
+	}
+	dev = dev_find_slot(0, PCI_DEVFN(1, 0));
+	if (!dev || !dev->enabled) {
+		printk(BIOS_DEBUG, "Disabling PEG10.\n");
+		reg &= ~DEVEN_PEG10;
+	}
+	dev = dev_find_slot(0, PCI_DEVFN(2, 0));
+	if (!dev || !dev->enabled) {
+		printk(BIOS_DEBUG, "Disabling IGD.\n");
+		reg &= ~DEVEN_IGD;
+	}
+	dev = dev_find_slot(0, PCI_DEVFN(6, 0));
+	if (!dev || !dev->enabled) {
+		printk(BIOS_DEBUG, "Disabling PEG60.\n");
+		reg &= ~DEVEN_PEG60;
+	}
+
+	dev = dev_find_slot(0, PCI_DEVFN(0, 0));
+	pci_write_config32(dev, DEVEN, reg);
+	if (!(reg & (DEVEN_PEG60 | DEVEN_PEG10 | DEVEN_PEG11 | DEVEN_PEG12))) {
+		/* Set the PEG clock gating bit.
+		 * Disables the IO clock on all PEG devices. */
+		MCHBAR32(0x7010) = MCHBAR32(0x7010) | 0x01;
+		printk(BIOS_DEBUG, "Disabling PEG IO clock.\n");
+	}
+}
+
 static void northbridge_init(struct device *dev)
 {
 	u8 bios_reset_cpl;
@@ -411,6 +456,9 @@ static void northbridge_init(struct device *dev)
 
 	/* Set here before graphics PM init */
 	MCHBAR32(0x5500) = 0x00100001;
+
+	/* Turn off unused devices */
+	disable_peg();
 }
 
 static void northbridge_enable(device_t dev)
diff --git a/src/northbridge/intel/sandybridge/raminit.c b/src/northbridge/intel/sandybridge/raminit.c
index 790f47c46c..9df102afc6 100644
--- a/src/northbridge/intel/sandybridge/raminit.c
+++ b/src/northbridge/intel/sandybridge/raminit.c
@@ -234,12 +234,6 @@ static void report_memory_config(void)
 	}
 }
 
-static void post_system_agent_init(void)
-{
-	/* If PCIe init is skipped, set the PEG clock gating */
-	MCHBAR32(0x7010) = MCHBAR32(0x7010) | 0x01;
-}
-
 void read_spd(spd_raw_data * spd, u8 addr)
 {
 	int j;
@@ -3888,7 +3882,6 @@ void init_dram_ddr3(spd_raw_data * spds, int mobile, int min_tck,
 	intel_early_me_init_done(ME_INIT_STATUS_SUCCESS);
 	intel_early_me_status();
 
-	post_system_agent_init();
 	report_memory_config();
 
 	cbmem_was_inited = !cbmem_recovery(s3resume);
diff --git a/src/northbridge/intel/sandybridge/raminit_mrc.c b/src/northbridge/intel/sandybridge/raminit_mrc.c
index 053a4873cd..1f6b75c95f 100644
--- a/src/northbridge/intel/sandybridge/raminit_mrc.c
+++ b/src/northbridge/intel/sandybridge/raminit_mrc.c
@@ -199,13 +199,6 @@ static void report_memory_config(void)
 	}
 }
 
-static void post_system_agent_init(struct pei_data *pei_data)
-{
-	/* If PCIe init is skipped, set the PEG clock gating */
-	if (!pei_data->pcie_init)
-		MCHBAR32(0x7010) = MCHBAR32(0x7010) | 0x01;
-}
-
 /**
  * Find PEI executable in coreboot filesystem and execute it.
  *
@@ -288,6 +281,5 @@ void sdram_initialize(struct pei_data *pei_data)
 	else
 		intel_early_me_status();
 
-	post_system_agent_init(pei_data);
 	report_memory_config();
 }