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>

17 files changed, 434 insertions, 216 deletions

diff --git a/src/southbridge/amd/sb700/Makefile.inc b/src/southbridge/amd/sb700/Makefile.inc
index 8e8e0297db..e174e8b347 100644..100755
--- a/src/southbridge/amd/sb700/Makefile.inc
+++ b/src/southbridge/amd/sb700/Makefile.inc
@@ -1,11 +1,16 @@
 driver-y += sb700.c
 driver-y += usb.c
 driver-y += lpc.c
+driver-y += smbus.c
 driver-y += sm.c
 driver-y += ide.c
 driver-y += sata.c
 driver-y += hda.c
 driver-y += pci.c
 ramstage-$(CONFIG_GENERATE_ACPI_TABLES) += fadt.c
+romstage-y += reset.c
 ramstage-y += reset.c
 romstage-y += enable_usbdebug.c
+
+romstage-y += early_setup.c
+romstage-y += smbus.c
diff --git a/src/southbridge/amd/sb700/early_setup.c b/src/southbridge/amd/sb700/early_setup.c
index a5e76c7a05..1f46da2228 100644..100755
--- a/src/southbridge/amd/sb700/early_setup.c
+++ b/src/southbridge/amd/sb700/early_setup.c
@@ -20,14 +20,19 @@
 #ifndef _SB700_EARLY_SETUP_C_
 #define _SB700_EARLY_SETUP_C_
 
+#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 <reset.h>
 #include <arch/cpu.h>
 #include <cbmem.h>
 #include "sb700.h"
-#include "smbus.c"
+#include "smbus.h"
 
-#define SMBUS_IO_BASE 0x6000	/* Is it a temporary SMBus I/O base address? */
-	 /*SIZE 0x40 */
 
 static void pmio_write(u8 reg, u8 value)
 {
@@ -129,7 +134,7 @@ static u8 set_sb700_revision(void)
 *	Console output through any port besides 0x3f8 is unsupported.
 *	If you use FWH ROMs, you have to setup IDSEL.
 ***************************************/
-static void sb7xx_51xx_lpc_init(void)
+void sb7xx_51xx_lpc_init(void)
 {
 	u8 reg8;
 	u32 reg32;
@@ -216,7 +221,7 @@ void sb7xx_51xx_disable_wideio(u8 wio_index)
 }
 
 /* what is its usage? */
-static u32 get_sbdn(u32 bus)
+u32 __attribute__ ((weak)) get_sbdn(u32 bus)
 {
 	device_t dev;
 
@@ -233,7 +238,7 @@ static u8 dual_core(void)
 /*
  * RPR 2.4 C-state and VID/FID change for the K8 platform.
  */
-static void enable_fid_change_on_sb(u32 sbbusn, u32 sbdn)
+void __attribute__((weak)) enable_fid_change_on_sb(u32 sbbusn, u32 sbdn)
 {
 	u8 byte;
 	byte = pmio_read(0x9a);
@@ -284,22 +289,6 @@ static void enable_fid_change_on_sb(u32 sbbusn, u32 sbdn)
 	pmio_write(0x67, 0x6);
 }
 
-void hard_reset(void)
-{
-	set_bios_reset();
-
-	/* full reset */
-	outb(0x0a, 0x0cf9);
-	outb(0x0e, 0x0cf9);
-}
-
-void soft_reset(void)
-{
-	set_bios_reset();
-	/* link reset */
-	outb(0x06, 0x0cf9);
-}
-
 void sb7xx_51xx_pci_port80(void)
 {
 	u8 byte;
@@ -681,7 +670,7 @@ static void sb700_por_init(void)
 /*
 * It should be called during early POST after memory detection and BIOS shadowing but before PCI bus enumeration.
 */
-static void sb7xx_51xx_before_pci_init(void)
+void sb7xx_51xx_before_pci_init(void)
 {
 	sb700_pci_cfg();
 }
@@ -689,18 +678,13 @@ static void sb7xx_51xx_before_pci_init(void)
 /*
 * This function should be called after enable_sb700_smbus().
 */
-static void sb7xx_51xx_early_setup(void)
+void sb7xx_51xx_early_setup(void)
 {
 	printk(BIOS_INFO, "sb700_early_setup()\n");
 	sb700_por_init();
 	sb700_acpi_init();
 }
 
-static int smbus_read_byte(u32 device, u32 address)
-{
-	return do_smbus_read_byte(SMBUS_IO_BASE, device, address);
-}
-
 int s3_save_nvram_early(u32 dword, int size, int  nvram_pos)
 {
 	int i;
@@ -732,7 +716,7 @@ int s3_load_nvram_early(int size, u32 *old_dword, int nvram_pos)
 }
 
 #if CONFIG_HAVE_ACPI_RESUME == 1
-static int acpi_is_wakeup_early(void)
+int acpi_is_wakeup_early(void)
 {
 	u16 tmp;
 	tmp = inw(ACPI_PM1_CNT_BLK);
diff --git a/src/southbridge/amd/sb700/pmio.c b/src/southbridge/amd/sb700/pmio.c
new file mode 100755
index 0000000000..baded54ba6
--- /dev/null
+++ b/src/southbridge/amd/sb700/pmio.c
@@ -0,0 +1,55 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2011 Advanced Micro Devices, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
+ */
+
+
+#include <arch/io.h>	/*inb, outb*/
+#include "pmio.h"
+
+static void pmio_write_index(u16 port_base, u8 reg, u8 value)
+{
+	outb(reg, port_base);
+	outb(value, port_base + 1);
+}
+
+static u8 pmio_read_index(u16 port_base, u8 reg)
+{
+	outb(reg, port_base);
+	return inb(port_base + 1);
+}
+
+void pm_iowrite(u8 reg, u8 value)
+{
+	pmio_write_index(PM_INDEX, reg, value);
+}
+
+u8 pm_ioread(u8 reg)
+{
+	return pmio_read_index(PM_INDEX, reg);
+}
+
+void pm2_iowrite(u8 reg, u8 value)
+{
+	pmio_write_index(PM2_INDEX, reg, value);
+}
+
+u8 pm2_ioread(u8 reg)
+{
+	return pmio_read_index(PM2_INDEX, reg);
+}
+
diff --git a/src/southbridge/amd/sb700/pmio.h b/src/southbridge/amd/sb700/pmio.h
new file mode 100755
index 0000000000..207fdc24ab
--- /dev/null
+++ b/src/southbridge/amd/sb700/pmio.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2011 Advanced Micro Devices, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
+ */
+
+
+#ifndef _PMIO_H_
+#define _PMIO_H_
+
+#define PM_INDEX	0xCD6
+#define PM_DATA		0xCD7
+#define PM2_INDEX	0xCD0
+#define PM2_DATA	0xCD1
+
+void pm_iowrite(u8 reg, u8 value);
+u8 pm_ioread(u8 reg);
+void pm2_iowrite(u8 reg, u8 value);
+u8 pm2_ioread(u8 reg);
+
+#endif
diff --git a/src/southbridge/amd/sb700/reset.c b/src/southbridge/amd/sb700/reset.c
index 32ee66b4b5..27ca32eb5e 100644..100755
--- a/src/southbridge/amd/sb700/reset.c
+++ b/src/southbridge/amd/sb700/reset.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2010 Advanced Micro Devices, Inc.
+ * Copyright (C) 2010 - 2011 Advanced Micro Devices, Inc.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -17,17 +17,51 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
  */
 
-#include <reset.h>
-#include <arch/io.h>
-#include <arch/romcc_io.h>
+#include <reset.h>		/* hard_reset, soft_rest*/
+#include <arch/io.h>		/* inb, outb */
+#include <arch/romcc_io.h>	/* pci_read_config32, device_t, PCI_DEV */
 
-#include "../../../northbridge/amd/amdk8/reset_test.c"
+#define HT_INIT_CONTROL		0x6C
+#define HTIC_BIOSR_Detect	(1<<5)
+
+#if CONFIG_MAX_PHYSICAL_CPUS > 32
+#define NODE_PCI(x, fn)        ((x<32)?(PCI_DEV(CONFIG_CBB,(CONFIG_CDB+x),fn)):(PCI_DEV((CONFIG_CBB-1),(CONFIG_CDB+x-32),fn)))
+#else
+#define NODE_PCI(x, fn)        PCI_DEV(CONFIG_CBB,(CONFIG_CDB+x),fn)
+#endif
+
+static void set_bios_reset(void)
+{
+	u32 nodes;
+	u32 htic;
+	device_t dev;
+	int i;
+
+	nodes = ((pci_read_config32(PCI_DEV(CONFIG_CBB, CONFIG_CDB, 0), 0x60) >> 4) & 7) + 1;
+	for(i = 0; i < nodes; i++) {
+		dev = NODE_PCI(i, 0);
+		htic = pci_read_config32(dev, HT_INIT_CONTROL);
+		htic &= ~HTIC_BIOSR_Detect;
+		pci_write_config32(dev, HT_INIT_CONTROL, htic);
+	}
+}
 
 void hard_reset(void)
 {
 	set_bios_reset();
+
 	/* Try rebooting through port 0xcf9 */
-	/* Actually it is not a real hard_reset --- it only reset coherent link table, but not reset link freq and width */
+	/* Actually it is not a real hard_reset
+	 * --- it only reset coherent link table, but not reset link freq and width
+	 */
 	outb((0 << 3) | (0 << 2) | (1 << 1), 0xcf9);
 	outb((0 << 3) | (1 << 2) | (1 << 1), 0xcf9);
 }
+
+void soft_reset(void)
+{
+	set_bios_reset();
+	/* link reset */
+	outb(0x06, 0x0cf9);
+}
+
diff --git a/src/southbridge/amd/sb700/sata.c b/src/southbridge/amd/sb700/sata.c
index 89eba4b168..411baf571f 100644..100755
--- a/src/southbridge/amd/sb700/sata.c
+++ b/src/southbridge/amd/sb700/sata.c
@@ -280,7 +280,7 @@ static void sata_init(struct device *dev)
 }
 
 static struct pci_operations lops_pci = {
-	/* .set_subsystem = pci_dev_set_subsystem, */
+	.set_subsystem = pci_dev_set_subsystem,
 };
 
 static struct device_operations sata_ops = {
diff --git a/src/southbridge/amd/sb700/sb700.h b/src/southbridge/amd/sb700/sb700.h
index 60eea47ee7..794dd96a81 100644..100755
--- a/src/southbridge/amd/sb700/sb700.h
+++ b/src/southbridge/amd/sb700/sb700.h
@@ -63,8 +63,11 @@ void sb7xx_51xx_enable(device_t dev);
 #ifdef __PRE_RAM__
 void sb7xx_51xx_lpc_port80(void);
 void sb7xx_51xx_pci_port80(void);
+void sb7xx_51xx_lpc_init(void);
 void sb7xx_51xx_enable_wideio(u8 wio_index, u16 base);
 void sb7xx_51xx_disable_wideio(u8 wio_index);
+void sb7xx_51xx_early_setup(void);
+void sb7xx_51xx_before_pci_init(void);
 #else
 #include <device/pci.h>
 /* allow override in mainboard.c */
@@ -72,8 +75,15 @@ void sb7xx_51xx_setup_sata_phys(struct device *dev);
 
 #endif
 
+#if CONFIG_HAVE_ACPI_RESUME == 1
+int acpi_is_wakeup_early(void);
+#endif
+
 int s3_save_nvram_early(u32 dword, int size, int  nvram_pos);
 int s3_load_nvram_early(int size, u32 *old_dword, int nvram_pos);
 
 void enable_usbdebug(unsigned int port);
+
+u32 __attribute__ ((weak)) get_sbdn(u32 bus);
+void __attribute__((weak)) enable_fid_change_on_sb(u32 sbbusn, u32 sbdn);
 #endif /* SB700_H */
diff --git a/src/southbridge/amd/sb700/sm.c b/src/southbridge/amd/sb700/sm.c
index 7d341df327..05065b6dbe 100644..100755
--- a/src/southbridge/amd/sb700/sm.c
+++ b/src/southbridge/amd/sb700/sm.c
@@ -30,7 +30,7 @@
 #include <arch/ioapic.h>
 #include <stdlib.h>
 #include "sb700.h"
-#include "smbus.c"
+#include "smbus.h"
 
 #define NMI_OFF 0
 
@@ -95,6 +95,20 @@ static void sm_init(device_t dev)
 	byte |= 4 << 2;		/* set NumSerIrqBits=4 */
 	pci_write_config8(dev, 0x69, byte);
 
+	/* Sx State Settings
+	 * Note: These 2 registers need to be set correctly for the S-state
+	 * to work properly. Otherwise the system may hang during resume
+	 * from the S-state.
+	 */
+	/*Use 8us clock for delays in the S-state resume timing sequence.*/
+	byte = pm_ioread(0x65);
+	byte &= ~(1 << 7);
+	pm_iowrite(0x65, byte);
+	/* Delay the APIC interrupt to the CPU until the system has fully resumed from the S-state. */
+	byte = pm_ioread(0x68);
+	byte |= 1 << 2;
+	pm_iowrite(0x68, byte);
+
 	/* IRQ0From8254 */
 	byte = pci_read_config8(dev, 0x41);
 	byte &= ~(1 << 7);
@@ -219,10 +233,10 @@ static void sm_init(device_t dev)
 	 *  Transactions for the K8 Platform (for All Revisions) */
 	abcfg_reg(0x10090, 1 << 8, 1 << 8);
 
-	/* ACPI_SOFT_CLOCK_THROTTLE_PERIOD */
+	/* Set ACPI Software clock Throttling Period to 244 us*/
 	byte = pm_ioread(0x68);
 	byte &= ~(3 << 6);
-	byte |= (2 << 6);	/* 224us */
+	byte |= (2 << 6);	/* 244us */
 	pm_iowrite(0x68, byte);
 
 	if (REV_SB700_A15 == rev) {
@@ -367,16 +381,16 @@ static void sb700_sm_read_resources(device_t dev)
 	res->limit = 0xFFFFFFFFUL;	/* res->base + res->size -1; */
 	res->align = 8;
 	res->gran = 8;
-	res->flags = IORESOURCE_MEM | IORESOURCE_FIXED;
+	res->flags = IORESOURCE_MEM | IORESOURCE_FIXED | IORESOURCE_RESERVE | IORESOURCE_ASSIGNED;
 
-	/* Linux ACPI crashes when it is 1. For late debugging. */
+	/* HPET */
 	res = new_resource(dev, 0xB4);	/* TODO: test hpet */
 	res->base  = 0xfed00000;	/* reset hpet to widely accepted address */
 	res->size = 0x400;
 	res->limit = 0xFFFFFFFFUL;	/* res->base + res->size -1; */
 	res->align = 8;
 	res->gran = 8;
-	res->flags = IORESOURCE_MEM | IORESOURCE_FIXED;
+	res->flags = IORESOURCE_MEM | IORESOURCE_FIXED | IORESOURCE_RESERVE | IORESOURCE_ASSIGNED;
 
 	/* dev->command |= PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER; */
 
@@ -387,7 +401,7 @@ static void sb700_sm_read_resources(device_t dev)
 	res->limit = 0xFFFFUL;	/* res->base + res->size -1; */
 	res->align = 8;
 	res->gran = 8;
-	res->flags = IORESOURCE_IO | IORESOURCE_FIXED;
+	res->flags = IORESOURCE_IO | IORESOURCE_FIXED | IORESOURCE_RESERVE | IORESOURCE_ASSIGNED;
 
 	compact_resources(dev);
 }
diff --git a/src/southbridge/amd/sb700/smbus.c b/src/southbridge/amd/sb700/smbus.c
index e47bceed47..6edc3de80c 100644..100755
--- a/src/southbridge/amd/sb700/smbus.c
+++ b/src/southbridge/amd/sb700/smbus.c
@@ -22,6 +22,51 @@
 
 #include "smbus.h"
 
+void alink_ab_indx(u32 reg_space, u32 reg_addr, u32 mask, u32 val)
+{
+	u32 tmp;
+
+	outl((reg_space & 0x3) << 30 | reg_addr, AB_INDX);
+	tmp = inl(AB_DATA);
+	/* rpr 4.2
+	 * For certain revisions of the chip, the ABCFG registers,
+	 * with an address of 0x100NN (where 'N' is any hexadecimal
+	 * number), require an extra programming step.*/
+	reg_addr & 0x10000 ? outl(0, AB_INDX) : NULL;
+
+	tmp &= ~mask;
+	tmp |= val;
+
+	/* printk(BIOS_DEBUG, "about write %x, index=%x", tmp, (reg_space&0x3)<<30 | reg_addr); */
+	outl((reg_space & 0x3) << 30 | reg_addr, AB_INDX);	/* probably we dont have to do it again. */
+	outl(tmp, AB_DATA);
+	reg_addr & 0x10000 ? outl(0, AB_INDX) : NULL;
+}
+
+/* space = 0: AX_INDXC, AX_DATAC
+ * space = 1: AX_INDXP, AX_DATAP
+ */
+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);
+}
+
+
 static inline void smbus_delay(void)
 {
 	outb(inb(0x80), 0x80);
@@ -177,48 +222,4 @@ int do_smbus_write_byte(u32 smbus_io_base, u32 device, u32 address, u8 val)
 	return 0;
 }
 
-static void alink_ab_indx(u32 reg_space, u32 reg_addr, u32 mask, u32 val)
-{
-	u32 tmp;
-
-	outl((reg_space & 0x3) << 30 | reg_addr, AB_INDX);
-	tmp = inl(AB_DATA);
-	/* rpr 4.2
-	 * For certain revisions of the chip, the ABCFG registers,
-	 * with an address of 0x100NN (where 'N' is any hexadecimal
-	 * number), require an extra programming step.*/
-	reg_addr & 0x10000 ? outl(0, AB_INDX) : NULL;
-
-	tmp &= ~mask;
-	tmp |= val;
-
-	/* printk(BIOS_DEBUG, "about write %x, index=%x", tmp, (reg_space&0x3)<<30 | reg_addr); */
-	outl((reg_space & 0x3) << 30 | reg_addr, AB_INDX);	/* probably we dont have to do it again. */
-	outl(tmp, AB_DATA);
-	reg_addr & 0x10000 ? outl(0, AB_INDX) : NULL;
-}
-
-/* space = 0: AX_INDXC, AX_DATAC
- * space = 1: AX_INDXP, AX_DATAP
- */
-static inline void alink_ax_indx(u32 space /*c or p? */ , 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);
-}
 #endif
diff --git a/src/southbridge/amd/sb700/smbus.h b/src/southbridge/amd/sb700/smbus.h
index c21a1dc0a2..9ddfc35303 100644..100755
--- a/src/southbridge/amd/sb700/smbus.h
+++ b/src/southbridge/amd/sb700/smbus.h
@@ -20,6 +20,13 @@
 #ifndef SB700_SMBUS_H
 #define SB700_SMBUS_H
 
+#include <stdint.h>
+#include "stddef.h"
+#include <arch/io.h>
+
+#define SMBUS_IO_BASE 0x6000	/* Is it a temporary SMBus I/O base address? */
+				/*SIZE 0x40 */
+
 #define SMBHSTSTAT 0x0
 #define SMBSLVSTAT 0x1
 #define SMBHSTCTRL 0x2
@@ -56,10 +63,11 @@
 #define axindxp_reg(reg, mask, val)	\
 	alink_ax_indx(1, (reg), (mask), (val))
 
+void alink_ab_indx(u32 reg_space, u32 reg_addr, u32 mask, u32 val);
+void alink_ax_indx(u32 space, u32 axindc, u32 mask, u32 val);
 int do_smbus_recv_byte(u32 smbus_io_base, u32 device);
 int do_smbus_send_byte(u32 smbus_io_base, u32 device, u8 val);
 int do_smbus_read_byte(u32 smbus_io_base, u32 device, u32 address);
 int do_smbus_write_byte(u32 smbus_io_base, u32 device, u32 address, u8 val);
 
-
 #endif
diff --git a/src/southbridge/amd/sb700/usb.c b/src/southbridge/amd/sb700/usb.c
index 2957dc5aca..793240187f 100644..100755
--- a/src/southbridge/amd/sb700/usb.c
+++ b/src/southbridge/amd/sb700/usb.c
@@ -178,6 +178,7 @@ static void usb_init2(struct device *dev)
 		dword |= 1 << 8;
 		dword &= ~(1 << 27); /* 6.23 */
 	}
+	pci_write_config32(dev, 0x50, dword);
 	printk(BIOS_DEBUG, "rpr 6.23, final dword=%x\n", dword);
 }
 
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 */