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>
diff --git a/src/southbridge/amd/sr5650/Makefile.inc b/src/southbridge/amd/sr5650/Makefile.inc
index a2d10d7..0a4ce39 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
old mode 100644
new mode 100755
index e94fcdd..6692b86
--- 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
old mode 100644
new mode 100755
index 5f7438c..50f836e
--- 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 @@
 	[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 @@
 	* 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 @@
 		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 @@
 		/* 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 @@
 	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 @@
 	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 @@
 }
 
 /* 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 @@
 /*
 * 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 @@
 		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
old mode 100644
new mode 100755
index 3ad5a7d..37743ca
--- a/src/southbridge/amd/sr5650/pcie.c
+++ b/src/southbridge/amd/sr5650/pcie.c
@@ -267,23 +267,25 @@
 }
 
 /*
-*/
+ * 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 @@
 			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 @@
 			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 @@
 	}
 }
 
-#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 @@
 	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 @@
  		//PCIE_CORE_INDEX_GPP2
 		case 11:
 		case 12:
-			reg = 0xE8;
 			value = 1 << 28;
 			break;
 
@@ -444,13 +450,11 @@
 		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 @@
 	/* 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 @@
 	 * 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 @@
 	/* 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 @@
 	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 @@
 	/* 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 @@
 	}
 }
 
+/**
+ * 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
old mode 100644
new mode 100755
index 616ca44..14b919d
--- a/src/southbridge/amd/sr5650/sr5650.c
+++ b/src/southbridge/amd/sr5650/sr5650.c
@@ -266,15 +266,21 @@
 }
 
 /*
-* 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;
 
-	/* set TOM */
-	pci_write_config32(nb_dev, 0x90, uma_memory_base);
+	/* 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);
+
+	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 @@
 	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 @@
 	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
old mode 100644
new mode 100755
index 5da3544..1b5112b
--- a/src/southbridge/amd/sr5650/sr5650.h
+++ b/src/southbridge/amd/sr5650/sr5650.h
@@ -103,6 +103,10 @@
 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 @@
 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 */