Update AMD SR5650 and SB700

This updates the code for the AMD SR5650 and SB700 southbridges.
Among other things, it changes the romstage.c files by replacing a
.C file include with a pair of .H file includes.  The .C file is
now added to the romstage in the SB700 or SR5650 Makefile.inc.
file to the romstage and ramstage elements.  This particular change
affects all mainboards that use the SB700, and their changes are
include herein.  These mainboards are:
  Advansus a785e,
  AMD Mahogany, Mahogany-fam10, Tilapia-fam10,
  Asrock 939a785gmh,
  Asus m4a78-em, m4a785-m,
  Gigabyte ma785gm,
  Iei Kino-780am2-fam10
  Jetway pa78vm5
  Supermicro h8scm_fam10
The nuvoton/wpcm450 earlysetup interface is changed because the file
is no longer included in the mainboard romstage.c files.

Change-Id: I502c0b95a7b9e7bb5dd81d03902bbc2143257e33
Signed-off-by: Frank Vibrans <frank.vibrans@amd.com>
Signed-off-by: efdesign98 <efdesign98@gmail.com>
Reviewed-on: http://review.coreboot.org/107
Tested-by: build bot (Jenkins)
Reviewed-by: Kerry She <shekairui@gmail.com>
Reviewed-by: Marc Jones <marcj303@gmail.com>

6 files changed, 199 insertions, 127 deletions

diff --git a/src/southbridge/amd/sr5650/Makefile.inc b/src/southbridge/amd/sr5650/Makefile.inc
index a2d10d7145..0a4ce39da9 100644
--- a/src/southbridge/amd/sr5650/Makefile.inc
+++ b/src/southbridge/amd/sr5650/Makefile.inc
@@ -1,3 +1,5 @@
 driver-y += sr5650.c
 driver-y += pcie.c
 driver-y += ht.c
+
+romstage-y += early_setup.c
diff --git a/src/southbridge/amd/sr5650/cmn.h b/src/southbridge/amd/sr5650/cmn.h
index e94fcddc86..6692b86d1f 100644..100755
--- a/src/southbridge/amd/sr5650/cmn.h
+++ b/src/southbridge/amd/sr5650/cmn.h
@@ -20,13 +20,21 @@
 #ifndef __SR5650_CMN_H__
 #define __SR5650_CMN_H__
 
+#include <arch/io.h>
+
 #define NBMISC_INDEX 	0x60
 #define NBHTIU_INDEX 	0x94 /* Note: It is different with RS690, whose HTIU index is 0xA8 */
 #define NBMC_INDEX 	0xE8
 #define NBPCIE_INDEX  	0xE0
-#define EXT_CONF_BASE_ADDRESS 0xE0000000
+#define EXT_CONF_BASE_ADDRESS	CONFIG_MMCONF_BASE_ADDRESS
 #define	TEMP_MMIO_BASE_ADDRESS	0xC0000000
 
+#define axindxc_reg(reg, mask, val) \
+	alink_ax_indx(0, (reg), (mask), (val))
+
+#define AB_INDX   0xCD8
+#define AB_DATA   (AB_INDX+4)
+
 static inline u32 nb_read_index(device_t dev, u32 index_reg, u32 index)
 {
 	pci_write_config32(dev, index_reg, index);
diff --git a/src/southbridge/amd/sr5650/early_setup.c b/src/southbridge/amd/sr5650/early_setup.c
index 5f7438ce85..50f836e77c 100644..100755
--- a/src/southbridge/amd/sr5650/early_setup.c
+++ b/src/southbridge/amd/sr5650/early_setup.c
@@ -17,20 +17,50 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
  */
 
+#include <stdint.h>
+#include <arch/cpu.h>
+#include <arch/io.h>
+#include <arch/romcc_io.h>
+#include <console/console.h>
+#include <cpu/x86/msr.h>
 #include "sr5650.h"
 #include "cmn.h"
 
+/* space = 0: AX_INDXC, AX_DATAC
+ * space = 1: AX_INDXP, AX_DATAP
+ */
+static void alink_ax_indx(u32 space, u32 axindc, u32 mask, u32 val)
+{
+        u32 tmp;
+
+        /* read axindc to tmp */
+        outl(space << 30 | space << 3 | 0x30, AB_INDX);
+        outl(axindc, AB_DATA);
+        outl(space << 30 | space << 3 | 0x34, AB_INDX);
+        tmp = inl(AB_DATA);
+
+        tmp &= ~mask;
+        tmp |= val;
+
+        /* write tmp */
+        outl(space << 30 | space << 3 | 0x30, AB_INDX);
+        outl(axindc, AB_DATA);
+        outl(space << 30 | space << 3 | 0x34, AB_INDX);
+        outl(tmp, AB_DATA);
+}
+
+
 /* family 10 only, for reg > 0xFF */
-#if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1
+#if (CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1) || (CONFIG_NORTHBRIDGE_AMD_AGESA_FAMILY10 == 1)
 static void set_fam10_ext_cfg_enable_bits(device_t fam10_dev, u32 reg_pos, u32 mask,
 				  u32 val)
 {
 	u32 reg_old, reg;
-	reg = reg_old = Get_NB32(fam10_dev, reg_pos);
+	reg = reg_old = pci_read_config32(fam10_dev, reg_pos);
 	reg &= ~mask;
 	reg |= val;
 	if (reg != reg_old) {
-		Set_NB32(fam10_dev, reg_pos, reg);
+		pci_write_config32(fam10_dev, reg_pos, reg);
 	}
 }
 #else
@@ -113,17 +143,19 @@ static const u8 sr5650_ibias[] = {
 	[0xe] = 0xC6,		/* 2.6Ghz HyperTransport 3 only */
 };
 
-static void sr5650_htinit(void)
+void sr5650_htinit(void)
 {
 	/*
 	 * About HT, it has been done in enumerate_ht_chain().
 	 */
-	device_t cpu_f0, sr5650_f0, clk_f1, cpu1_f0;
+	device_t cpu_f0, sr5650_f0, clk_f1;
 	u32 reg;
-	u8 cpu_ht_freq, ibias;
+	u8 cpu_ht_freq, cpu_htfreq_max, ibias;
+	u8 sbnode;
+	u8 sblink;
+	u16 linkfreq_reg;
+	u16 linkfreqext_reg;
 
-	cpu_f0 = PCI_DEV(0, 0x18, 0);
-	cpu1_f0 = PCI_DEV(0, 0x19, 0);
 	/************************
 	* get cpu's ht freq, in cpu's function 0, offset 0x88
 	* bit11-8, specifics the maximum operation frequency of the link's transmitter clock.
@@ -133,16 +165,36 @@ static void sr5650_htinit(void)
 	* please see the table sr5650_ibias about the value and its corresponding frequency.
 	************************/
 	/* Link0, Link1 are for connection between P0 and P1.
-	 * Link2 should be 0xC8?
 	 * TODO: Check the topology of the MP and NB. Or we just read the nbconfig? */
 	/* NOTE: In most cases, we only have one CPU. In that case, we should read 0x88. */
 
-	reg = pci_read_config32(cpu1_f0, 0x0);
-	reg = pci_read_config32(cpu_f0,
-				reg == 0 || reg == -1 ? 0x88 : 0xC8
-		);
+	/* Find out the node ID and the Link ID that
+	 * connects to the Southbridge (system IO hub).
+	 */
+	sbnode = (pci_read_config32(PCI_DEV(0, 0x18, 0), 0x60) >> 8) & 7;
+	sblink = (pci_read_config32(PCI_DEV(0, 0x18, 0), 0x64) >> 8) & 3; /* bit[10] sublink, bit[9,8] link. */
+	cpu_f0 = PCI_DEV(0, (0x18 + sbnode), 0);
+
+	/*
+	 * link freq reg of Link0, 1, 2, 3 is 0x88, 0xA8, 0xC8, 0xE8 respectively
+	 * link freq ext reg of Link0, 1, 2, 3 is 0x9C, 0xBC, 0xDC, 0xFC respectively
+	 */
+	linkfreq_reg = 0x88 + (sblink << 5);
+	linkfreqext_reg = 0x9C + (sblink << 5);
+	reg = pci_read_config32(cpu_f0, linkfreq_reg);
+
 	cpu_ht_freq = (reg & 0xf00) >> 8;
-	printk(BIOS_INFO, "sr5650_htinit cpu_ht_freq=%x.\n", cpu_ht_freq);
+
+	/* Freq[4] is only valid for revision D and later processors */
+	if (cpuid_eax(1) >= 0x100F80) {
+		cpu_htfreq_max = 0x14;
+		cpu_ht_freq |= ((pci_read_config32(cpu_f0, linkfreqext_reg) & 0x01) << 4);
+	} else {
+		cpu_htfreq_max = 0x0F;
+	}
+
+	printk(BIOS_INFO, "sr5650_htinit: Node %x Link %x, HT freq=%x.\n",
+			sbnode, sblink, cpu_ht_freq);
 	sr5650_f0 = PCI_DEV(0, 0, 0);
 
 	clk_f1 = PCI_DEV(0, 0, 1); /* We need to make sure the F1 is accessible. */
@@ -162,13 +214,13 @@ static void sr5650_htinit(void)
 		set_nbcfg_enable_bits(clk_f1, 0xD8, 0x3FF, ibias);
 		/* Optimizes chipset HT transmitter drive strength */
 		set_htiu_enable_bits(sr5650_f0, 0x2A, 0x3, 0x3);
-	} else if ((cpu_ht_freq > 0x6) && (cpu_ht_freq < 0xf)) {
+	} else if ((cpu_ht_freq > 0x6) && (cpu_ht_freq < cpu_htfreq_max)) {
 		printk(BIOS_INFO, "sr5650_htinit: HT3 mode\n");
 
 		/* Enable Protocol checker */
 		set_htiu_enable_bits(sr5650_f0, 0x1E, 0xFFFFFFFF, 0x7FFFFFFC);
 
-		#if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1		/* save some spaces */
+#if (CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1) || (CONFIG_NORTHBRIDGE_AMD_AGESA_FAMILY10 == 1) /* save some spaces */
 		/* HT3 mode, RPR 5.4.3 */
 		set_nbcfg_enable_bits(sr5650_f0, 0x9c, 0x3 << 16, 0);
 
@@ -189,83 +241,36 @@ static void sr5650_htinit(void)
 		/* Enables strict TM4 detection */
 		set_htiu_enable_bits(sr5650_f0, 0x15, 0x1 << 22, 0x1 << 22);
 
+		/* Optimizes chipset HT transmitter drive strength */
+		set_htiu_enable_bits(sr5650_f0, 0x2A, 0x3 << 0, 0x1 << 0);
+
 		/* HyperTransport 3 Processor register settings to be done in northbridge */
+
 		/* Enables error-retry mode */
-		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x130, 1 << 0, 1 << 0);
-		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x134, 1 << 0, 1 << 0); /* TODO: Check if it is needed to set other node. */
-		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x138, 1 << 0, 1 << 0);
+		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x130 + (sblink << 2), 1 << 0, 1 << 0);
+
 		/* Enables scrambling */
-		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x170, 1 << 3, 1 << 3);
-		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x174, 1 << 3, 1 << 3); /* TODO: Check if it is needed to set other node. */
-		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x178, 1 << 3, 1 << 3);
+		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x170 + (sblink << 2), 1 << 3, 1 << 3);
+
 		/* Enables transmitter de-emphasis
-		 * This depends on the PCB design and the trace */
+		 * This depends on the PCB design and the trace
+		 */
 		/* Disables command throttling */
 		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x168, 1 << 10, 1 << 10);
+
 		/* Sets Training 0 Time. See T0Time table for encodings */
-		set_fam10_ext_cfg_enable_bits(cpu_f0, 0x16C, 0x3F, 0x20);
+		/* AGESA have set it to recommanded value already
+		 * The recommended values are 14h(2us) if F0x[18C:170][LS2En]=0
+		 * and 26h(12us) if F0x[18C:170][LS2En]=1
+		 */
+		//set_fam10_ext_cfg_enable_bits(cpu_f0, 0x16C, 0x3F, 0x26);
+
 		/* HT Buffer Allocation for Ganged Links!!! */
-		#endif	/* #if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1 */
+#endif	/* #if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1 */
 	}
 }
 
-#if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 != 1		/* save some spaces */
-/*******************************************************
-* Optimize k8 with UMA.
-* See BKDG_NPT_0F guide for details.
-* The processor node is addressed by its Node ID on the HT link and can be
-* accessed with a device number in the PCI configuration space on Bus0.
-* The Node ID 0 is mapped to Device 24 (0x18), the Node ID 1 is mapped
-* to Device 25, and so on.
-* The processor implements configuration registers in PCI configuration
-* space using the following four headers
-*	Function0: HT technology configuration
-*	Function1: Address map configuration
-*	Function2: DRAM and HT technology Trace mode configuration
-*	Function3: Miscellaneous configuration
-*******************************************************/
-static void k8_optimization(void)
-{
-	device_t k8_f0, k8_f2, k8_f3;
-	msr_t msr;
-
-	printk(BIOS_INFO, "k8_optimization()\n");
-	k8_f0 = PCI_DEV(0, 0x18, 0);
-	k8_f2 = PCI_DEV(0, 0x18, 2);
-	k8_f3 = PCI_DEV(0, 0x18, 3);
-
-	pci_write_config32(k8_f0, 0x90, 0x01700169);	/* CIM NPT_Optimization */
-	set_nbcfg_enable_bits(k8_f0, 0x68, 1 << 28, 0 << 28);
-	set_nbcfg_enable_bits(k8_f0, 0x68, 1 << 26 | 1 << 27,
-			      1 << 26 | 1 << 27);
-	set_nbcfg_enable_bits(k8_f0, 0x68, 1 << 11, 1 << 11);
-	/* set_nbcfg_enable_bits(k8_f0, 0x84, 1 << 11 | 1 << 13 | 1 << 15, 1 << 11 | 1 << 13 | 1 << 15); */	/* TODO */
-
-	pci_write_config32(k8_f3, 0x70, 0x51220111);	/* CIM NPT_Optimization */
-	pci_write_config32(k8_f3, 0x74, 0x50404021);
-	pci_write_config32(k8_f3, 0x78, 0x08002A00);
-	if (pci_read_config32(k8_f3, 0xE8) & 0x3<<12)
-		pci_write_config32(k8_f3, 0x7C, 0x0000211A); /* dual core */
-	else
-		pci_write_config32(k8_f3, 0x7C, 0x0000212B); /* single core */
-	set_nbcfg_enable_bits_8(k8_f3, 0xDC, 0xFF, 0x25);
-
-	set_nbcfg_enable_bits(k8_f2, 0xA0, 1 << 5, 1 << 5);
-	set_nbcfg_enable_bits(k8_f2, 0x94, 0xF << 24, 7 << 24);
-	set_nbcfg_enable_bits(k8_f2, 0x90, 1 << 10, 0 << 10);
-	set_nbcfg_enable_bits(k8_f2, 0xA0, 3 << 2, 3 << 2);
-	set_nbcfg_enable_bits(k8_f2, 0xA0, 1 << 5, 1 << 5);
-
-	msr = rdmsr(0xC001001F);
-	msr.lo &= ~(1 << 9);
-	msr.hi &= ~(1 << 4);
-	wrmsr(0xC001001F, msr);
-}
-#else
-#define k8_optimization() do{}while(0)
-#endif	/* #if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 != 1 */
-
-#if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1		/* save some spaces */
+#if (CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1) || (CONFIG_NORTHBRIDGE_AMD_AGESA_FAMILY10 == 1) /* save some spaces */
 void fam10_optimization(void)
 {
 	device_t cpu_f0, cpu_f2, cpu_f3;
@@ -340,13 +345,12 @@ static void sr5650_por_misc_index_init(device_t nb_dev)
 	set_nbmisc_enable_bits(nb_dev, 0x2B, 1 << 15 | 1 << 27, 1 << 15 | 1 << 27);
 	set_nbmisc_enable_bits(nb_dev, 0x2C, 1 << 0 | 1 << 1 | 1 << 5 | 1 << 4 | 1 << 10, 1 << 0 | 1 << 1 | 1 << 5);
 	set_nbmisc_enable_bits(nb_dev, 0x32, 0x3F << 20, 0x2A << 20);
-	set_nbmisc_enable_bits(nb_dev, 0x34, 1 << 7 | 1 << 15 | 1 << 23 | 1 << 31, 0); /* bit31 BTS fail */
+	set_nbmisc_enable_bits(nb_dev, 0x34, 1 << 7 | 1 << 15 | 1 << 23, 0);
 	set_nbmisc_enable_bits(nb_dev, 0x35, 0x3F << 26, 0x2A << 26);
 	set_nbmisc_enable_bits(nb_dev, 0x37, 0xfff << 20, 0xddd << 20);
 	set_nbmisc_enable_bits(nb_dev, 0x37, 7 << 11, 0);
 	/* PCIE CDR setting */
 	set_nbmisc_enable_bits(nb_dev, 0x38, 0xFFFFFFFF, 0xC0C0C0);
-	set_nbmisc_enable_bits(nb_dev, 0x39, 1 << 31, 0); /* bit31 BTS fail */
 	set_nbmisc_enable_bits(nb_dev, 0x22, 0xFFFFFFFF, (1 << 27) | (0x8 << 12) | (0x8 << 16) | (0x8 << 20));
 	set_nbmisc_enable_bits(nb_dev, 0x22, 1 << 1 | 1 << 2 | 1 << 6 | 1 << 7, 1 << 1 | 1 << 2 | 1 << 6 | 1 << 7);
 
@@ -381,7 +385,7 @@ static void sr5650_por_misc_index_init(device_t nb_dev)
 	set_nbmisc_enable_bits(nb_dev, 0x47, 0xFFFFFFFF, 0x0000000B);
 
 	set_nbmisc_enable_bits(nb_dev, 0x12, 0xFFFFFFFF, 0x00FB5555);
-	set_nbmisc_enable_bits(nb_dev, 0x0C, 0xFFFFFFFF, 0x001f37EC);
+	set_nbmisc_enable_bits(nb_dev, 0x0C, 0xFFFFFFFF, 0x001F37FC);
 	set_nbmisc_enable_bits(nb_dev, 0x15, 0xFFFFFFFF, 0x0);
 
 	/* NB_PROG_DEVICE_REMAP */
@@ -478,7 +482,7 @@ static void sr5650_por_init(device_t nb_dev)
 }
 
 /* enable CFG access to Dev8, which is the SB P2P Bridge */
-static void enable_sr5650_dev8(void)
+void enable_sr5650_dev8(void)
 {
 	set_nbmisc_enable_bits(PCI_DEV(0, 0, 0), 0x00, 1 << 6, 1 << 6);
 }
@@ -486,14 +490,14 @@ static void enable_sr5650_dev8(void)
 /*
 * Compliant with CIM_33's AtiNBInitEarlyPost (AtiInitNBBeforePCIInit).
 */
-static void sr5650_before_pci_init(void)
+void sr5650_before_pci_init(void)
 {
 }
 
 /*
 * The calling sequence is same as CIM.
 */
-static void sr5650_early_setup(void)
+void sr5650_early_setup(void)
 {
 	device_t nb_dev = PCI_DEV(0, 0, 0);
 	printk(BIOS_INFO, "sr5650_early_setup()\n");
@@ -513,28 +517,24 @@ static void sr5650_early_setup(void)
 		break;
 	}
 
-#if CONFIG_NORTHBRIDGE_AMD_AMDFAM10 == 1
-
 	fam10_optimization();
-#else
-	k8_optimization();
-#endif
-
 	sr5650_por_init(nb_dev);
 }
 
 /**
- * @brief disable GPP1 Port0,1, GPP3a Port0,1,2,3,4,5
+ * @brief disable GPP1 Port0,1, GPP2, GPP3a Port0,1,2,3,4,5, GPP3b
  *
  */
-void disable_pcie_bridge(void)
+void sr5650_disable_pcie_bridge(void)
 {
 	u32 mask;
 	u32 reg;
 	device_t nb_dev = PCI_DEV(0, 0, 0);
 
-	mask = (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7) |
-			(1 << 16) | (1 << 17);
+	mask = (1 << 2) | (1 << 3); /*GPP1*/
+	mask |= (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7) | (1 << 16) | (1 << 17); /*GPP3a*/
+	mask |= (1 << 18) | (1 << 19); /*GPP2*/
+	mask |= (1 << 20); /*GPP3b*/
 	reg = mask;
 	set_nbmisc_enable_bits(nb_dev, 0x0c, mask, reg);
 }
diff --git a/src/southbridge/amd/sr5650/pcie.c b/src/southbridge/amd/sr5650/pcie.c
index 3ad5a7d3c3..37743cacae 100644..100755
--- a/src/southbridge/amd/sr5650/pcie.c
+++ b/src/southbridge/amd/sr5650/pcie.c
@@ -267,23 +267,25 @@ void disable_pcie_bar3(device_t nb_dev)
 }
 
 /*
-*/
+ * GEN2 Software Compliance
+ */
 void init_gen2(device_t nb_dev, device_t dev, u8 port)
 {
 	u32 reg, val;
+
 	/* for A11 (0x89 == 0) */
 	reg = 0x34;
-	if (port <= 3){
+	if (port <= 3) {
 		val = 1<<5;
-	}else{
+	} else {
 		val = 1<<31;
-		if (port >= 9 )
+		if (port >= 9)
 			reg = 0x39;
 	}
 
-	/* todo: check for rev > a11
+	/* TODO: check for rev > a11 */
 	switch (port) {
-		case 2;
+		case 2:
 			reg = 0x34;
 			val = 1<<5;
 			break;
@@ -300,7 +302,9 @@ void init_gen2(device_t nb_dev, device_t dev, u8 port)
 			reg = 0x39;
 			val = 1<<31;
 			break;
-		case 7..9:
+		case 7:
+		case 8:
+		case 9:
 			reg = 0x37;
 			val = 1<<port;
 			break;
@@ -312,9 +316,11 @@ void init_gen2(device_t nb_dev, device_t dev, u8 port)
 			reg = 0;
 			break;
 	}
-	*/
+
+	/* Enables GEN2 capability of the device */
 	set_pcie_enable_bits(dev, 0xA4, 0x1, 0x1);
-	pci_ext_write_config32(nb_dev, dev, 0x88, 0xF0, 1<<2); /* LINK_CRTL2*/
+	/* Advertise the link speed to be Gen2 */
+	pci_ext_write_config32(nb_dev, dev, 0x88, 0xF0, 1<<2); /* LINK_CRTL2 */
 	set_nbmisc_enable_bits(nb_dev, reg, val, val);
 }
 
@@ -409,7 +415,7 @@ static void gpp12_cpl_buf_alloc(device_t nb_dev, device_t dev)
 	}
 }
 
-#if 0				/* BTS report error without this function. But some board
+#if 1				/* BTS report error without this function. But some board
 				 * fail to boot. Leave it here for future debug. */
 
 /*
@@ -424,6 +430,7 @@ static void EnableLclkGating(device_t dev)
 	device_t nb_dev = dev_find_slot(0, 0);
 	device_t clk_f1= dev_find_slot(0, 1);
 
+	reg = 0xE8;
 	port = dev->path.pci.devfn >> 3;
 	switch (port) {
  		//PCIE_CORE_INDEX_GPP1
@@ -436,7 +443,6 @@ static void EnableLclkGating(device_t dev)
  		//PCIE_CORE_INDEX_GPP2
 		case 11:
 		case 12:
-			reg = 0xE8;
 			value = 1 << 28;
 			break;
 
@@ -444,13 +450,11 @@ static void EnableLclkGating(device_t dev)
 		case 4 ... 7:
 		case 9:
 		case 10:
-			reg = 0xE8;
 			value = 1 << 31;
 			break;
 
 		//PCIE_CORE_INDEX_GPP3b;
 		case 13:
-			reg = 0xE8;
 			value = 1 << 25;
 			break;
 
@@ -541,6 +545,14 @@ void sr5650_gpp_sb_init(device_t nb_dev, device_t dev, u32 port)
 	/* 4.4.2.step13.6. Set REGS_LC_ALLOW_TX_L1_CONTROL to allow TX to
 	   prevent LC from going to L1 when there are outstanding completions.*/
 	set_pcie_enable_bits(dev, 0x02, 1 << 15, 1 << 15);
+
+	/* Enables the PLL power down when all lanes are inactive.
+	 * It should be on in GPP.
+	 */
+	if (gpp_sb_sel == PCIE_CORE_INDEX_GPP3a || gpp_sb_sel == PCIE_CORE_INDEX_GPP3b || gpp_sb_sel == PCIE_CORE_INDEX_SB) {
+		set_pcie_enable_bits(nb_dev, 0x02 | gpp_sb_sel, 1 << 3, 1 << 3);
+	}
+
 	/* 4.4.2.step13.7. Set REGS_LC_DONT_GO_TO_L0S_IF_L1_ARMED to prevent
 	   lc to go to from L0 to Rcv_L0s if L1 is armed. */
 	set_pcie_enable_bits(dev, 0xA1, 1 << 11, 1 << 11);
@@ -594,8 +606,11 @@ void sr5650_gpp_sb_init(device_t nb_dev, device_t dev, u32 port)
 	 * RPR typo- it says enable but the bit setting says disable.
 	 * Disable it here and we enable it later. */
 	set_pcie_enable_bits(dev, 0xA4, 1 << 0, 1 << 0);
-	/* 4.4.2.step13.21. */
-	/* 4.4.2.step13.22 */
+
+	/* 4.4.2.step13.21. Legacy Hot Plug  -CMOS Option */
+	/* NOTE: This feature can be enabled only for Hot-Plug slots implemented on SR5690 platform. */
+
+	/* 4.4.2.step13.22. Native PCIe Mode -CMOS Option */
 	/* Enable native PME. */
 	set_pcie_enable_bits(dev, 0x10, 1 << 3, 1 < 3);
 	/* This bit when set indicates that the PCIe Link associated with this port
@@ -607,7 +622,7 @@ void sr5650_gpp_sb_init(device_t nb_dev, device_t dev, u32 port)
 	/* Enables flushing of TLPs when Data Link is down. */
 	set_pcie_enable_bits(dev, 0x20, 1 << 19, 0 << 19);
 
-	/* 4.4.2.step14. Server Class Hot Plug Feature */
+	/* 4.4.2.step14. Server Class Hot Plug Feature. NOTE: This feature is not supported on SR5670 and SR5650 */
 	/* 4.4.2 step14.1: Advertising Hot Plug Capabilities */
 	/* 4.4.2.step14.2: Firmware Upload */
 	/* 4.4.2.Step14.3: SBIOS Acknowledgment to Firmware of Successful Firmware Upload */
@@ -630,11 +645,13 @@ void sr5650_gpp_sb_init(device_t nb_dev, device_t dev, u32 port)
 	if ( port == 8 )
 		set_pcie_enable_bits(dev, 0xA0, 0, 1 << 23);
 
+#if 0 //SR56x0 pcie Gen2 code is not tested yet, we should enable it again when test finished.
 	/* set automatic Gen2 support, needs mainboard config option as Gen2 can cause issues on some platforms. */
 	init_gen2(nb_dev, dev, port);
 	set_pcie_enable_bits(dev, 0xA4, 1 << 29, 1 << 29);
 	set_pcie_enable_bits(dev, 0xC0, 1 << 15, 0);
 	set_pcie_enable_bits(dev, 0xA2, 1 << 13, 0);
+#endif
 
 	/* Hotplug Support - bit5 + bit6  capable and surprise */
 	pci_ext_write_config32(nb_dev, dev, 0x6c, 0x60, 0x60);
@@ -715,12 +732,13 @@ void sr5650_gpp_sb_init(device_t nb_dev, device_t dev, u32 port)
 	/* 4.4..7.1 TXCLK Gating in L1, Enables powering down TXCLK clock pads on the receive side. */
 	set_pcie_enable_bits(nb_dev, 0x40 | gpp_sb_sel, 1 << 6, 1 << 6);
 
-	/* Step 21: Register Locking PCIE Misc. Late Core sttting - Must move somewhere do PciInitLate FIXME */
-	/* Lock HWInit Register */
-	//set_pcie_enable_bits(nb_dev, 0x10 | gpp_sb_sel, 1 << 0, 1 << 0);
+	/* Step 20: Disables immediate RCB timeout on link down */
+	if (!((pci_read_config32(dev, 0x6C ) >> 6) & 0x01)) {
+		set_pcie_enable_bits(dev, 0x70, 1 << 19, 0 << 19);
+	}
 
 	/* Step 27: LCLK Gating	*/
-	//EnableLclkGating(dev);
+	EnableLclkGating(dev);
 
 	/* Set Common Clock */
 	/* If dev present, set PcieCapPtr+0x10, BIT6);
@@ -734,6 +752,34 @@ void sr5650_gpp_sb_init(device_t nb_dev, device_t dev, u32 port)
 	}
 }
 
+/**
+ * Step 21: Register Locking
+ * Lock HWInit Register of each pcie core
+ */
+static void lock_hwinitreg(device_t nb_dev)
+{
+	/* Step 21: Register Locking, Lock HWInit Register */
+	set_pcie_enable_bits(nb_dev, 0x10 | PCIE_CORE_INDEX_GPP1, 1 << 0, 1 << 0);
+	set_pcie_enable_bits(nb_dev, 0x10 | PCIE_CORE_INDEX_SB, 1 << 0, 1 << 0);
+	set_pcie_enable_bits(nb_dev, 0x10 | PCIE_CORE_INDEX_GPP2, 1 << 0, 1 << 0);
+	set_pcie_enable_bits(nb_dev, 0x10 | PCIE_CORE_INDEX_GPP3a, 1 << 0, 1 << 0);
+	set_pcie_enable_bits(nb_dev, 0x10 | PCIE_CORE_INDEX_GPP3b, 1 << 0, 1 << 0);
+}
+
+/**
+ * Lock HWInit Register
+ */
+void sr56x0_lock_hwinitreg(void)
+{
+	device_t nb_dev = dev_find_slot(0, PCI_DEVFN(0, 0));
+
+	/* Lock HWInit Register */
+	lock_hwinitreg(nb_dev);
+
+	/* Lock HWInit Register NBMISCIND:0x0 NBCNTL[7] HWINIT_WR_LOCK */
+	set_nbmisc_enable_bits(nb_dev, 0x00, 1 << 7, 1 << 7);
+}
+
 /*****************************************
 * Compliant with CIM_33's PCIEConfigureGPPCore
 *****************************************/
diff --git a/src/southbridge/amd/sr5650/sr5650.c b/src/southbridge/amd/sr5650/sr5650.c
index 616ca44ee6..14b919de72 100644..100755
--- a/src/southbridge/amd/sr5650/sr5650.c
+++ b/src/southbridge/amd/sr5650/sr5650.c
@@ -266,15 +266,21 @@ u8 PcieTrainPort(device_t nb_dev, device_t dev, u32 port)
 }
 
 /*
-* Compliant with CIM_33's ATINB_SetToms.
-* Set Top Of Memory below and above 4G.
-*/
+ * Set Top Of Memory below and above 4G.
+ */
 void sr5650_set_tom(device_t nb_dev)
 {
-	extern u64 uma_memory_base;
+	msr_t sysmem;
+
+	/* The system top memory in SR56X0. */
+	sysmem = rdmsr(0xc001001A);
+	printk(BIOS_DEBUG, "Sysmem TOM = %x_%x\n", sysmem.hi, sysmem.lo);
+	pci_write_config32(nb_dev, 0x90, sysmem.lo);
 
-	/* set TOM */
-	pci_write_config32(nb_dev, 0x90, uma_memory_base);
+	sysmem = rdmsr(0xc001001D);
+	printk(BIOS_DEBUG, "Sysmem TOM2 = %x_%x\n", sysmem.hi, sysmem.lo);
+	htiu_write_index(nb_dev, 0x31, sysmem.hi);
+	htiu_write_index(nb_dev, 0x30, sysmem.lo | 1);
 }
 
 u32 get_vid_did(device_t dev)
@@ -308,7 +314,7 @@ void sr5650_nb_pci_table(device_t nb_dev)
 	temp8 &= ~(1<<1);
 	pci_write_config8(nb_dev, 0x8d, temp8);
 
-	/* set temporary NB TOM to 0x40000000. */
+	/* The system top memory in SR56X0. */
 	sr5650_set_tom(nb_dev);
 
 	/* Program NB HTIU table. */
@@ -424,6 +430,11 @@ void sr5650_enable(device_t dev)
 	default:
 		printk(BIOS_DEBUG, "unknown dev: %s\n", dev_path(dev));
 	}
+
+	/* Lock HWInit Register after the last device was done */
+	if (dev_ind == 13) {
+		sr56x0_lock_hwinitreg();
+	}
 }
 
 struct chip_operations southbridge_amd_sr5650_ops = {
diff --git a/src/southbridge/amd/sr5650/sr5650.h b/src/southbridge/amd/sr5650/sr5650.h
index 5da354493b..1b5112b7c2 100644..100755
--- a/src/southbridge/amd/sr5650/sr5650.h
+++ b/src/southbridge/amd/sr5650/sr5650.h
@@ -103,6 +103,10 @@ void ProgK8TempMmioBase(u8 in_out, u32 pcie_base_add, u32 mmio_base_add);
 void enable_pcie_bar3(device_t nb_dev);
 void disable_pcie_bar3(device_t nb_dev);
 
+void enable_sr5650_dev8(void);
+void sr5650_htinit(void);
+void sr5650_early_setup(void);
+void sr5650_before_pci_init(void);
 void sr5650_enable(device_t dev);
 void sr5650_gpp_sb_init(device_t nb_dev, device_t dev, u32 port);
 void sr5650_gfx_init(device_t nb_dev, device_t dev, u32 port);
@@ -112,8 +116,9 @@ void PcieReleasePortTraining(device_t nb_dev, device_t dev, u32 port);
 u8 PcieTrainPort(device_t nb_dev, device_t dev, u32 port);
 void pcie_config_misc_clk(device_t nb_dev);
 void fam10_optimization(void);
-void disable_pcie_bridge(void);
+void sr5650_disable_pcie_bridge(void);
 u32 get_vid_did(device_t dev);
 void sr5650_nb_pci_table(device_t nb_dev);
 void init_gen2(device_t nb_dev, device_t dev, u8 port);
+void sr56x0_lock_hwinitreg(void);
 #endif /* SR5650_H */