Update Kontron board

 - use new features of the ich7 update
 - move rambase above 1M to avoid memory trashing through SMM relocation
 - enable superio HWM

Update ICH7 driver

 - minor smi cosmetics (in progress)
 - add real ac97 driver
 - add real azalia driver
 - fix some interrupt issues
 - fix some sata issues 
 - include Patrick's fix for _lpc.c

Signed-off-by: Stefan Reinauer <stepan@coresystems.de>
Acked-by: Patrick Georgi <patrick.georgi@coresystems.de>



git-svn-id: svn://svn.coreboot.org/coreboot/trunk@3886 2b7e53f0-3cfb-0310-b3e9-8179ed1497e1

11 files changed, 340 insertions, 32 deletions

diff --git a/src/mainboard/kontron/986lcd-m/Config.lb b/src/mainboard/kontron/986lcd-m/Config.lb
index 9f965e29af..833478bb79 100644
--- a/src/mainboard/kontron/986lcd-m/Config.lb
+++ b/src/mainboard/kontron/986lcd-m/Config.lb
@@ -82,7 +82,7 @@ if HAVE_ACPI_TABLES
 	object acpi_tables.o
 	makerule dsdt.c
 		depends "$(MAINBOARD)/dsdt.dsl"
-		action  "iasl -p $(PWD)/dsdt -tc $(MAINBOARD)/dsdt.dsl"
+		action  "iasl -p dsdt -tc $(MAINBOARD)/dsdt.dsl"
 		action  "mv $(PWD)/dsdt.hex dsdt.c"
 	end
 	object ./dsdt.o
@@ -187,9 +187,18 @@ chip northbridge/intel/i945
 		device pci 02.1 on end # display controller
 
                 chip southbridge/intel/i82801gx
+			register "pirqa_routing" = "0x05"
+			register "pirqb_routing" = "0x07"
+			register "pirqc_routing" = "0x06"
+			register "pirqd_routing" = "0x07"
+			register "pirqe_routing" = "0x80"
+			register "pirqf_routing" = "0x80"
+			register "pirqg_routing" = "0x80"
+			register "pirqh_routing" = "0x05"
+
                         register "ide_legacy_combined" = "0x1"
                         register "ide_enable_primary" = "0x1"
-                        register "ide_enable_secondary" = "0x1"
+                        register "ide_enable_secondary" = "0x0"
                         register "sata_ahci" = "0x0"
 
                 	device pci 1b.0 on end # High Definition Audio
diff --git a/src/mainboard/kontron/986lcd-m/Options.lb b/src/mainboard/kontron/986lcd-m/Options.lb
index 926e69820b..dbf21d5801 100644
--- a/src/mainboard/kontron/986lcd-m/Options.lb
+++ b/src/mainboard/kontron/986lcd-m/Options.lb
@@ -233,7 +233,7 @@ default FALLBACK_SIZE=131072
 ##
 ## coreboot C code runs at this location in RAM
 ##
-default _RAMBASE=0x00004000
+default _RAMBASE=0x00100000
 
 ##
 ## Load the payload from the ROM
diff --git a/src/mainboard/kontron/986lcd-m/auto.c b/src/mainboard/kontron/986lcd-m/auto.c
index c708f1ae15..5a6d88a850 100644
--- a/src/mainboard/kontron/986lcd-m/auto.c
+++ b/src/mainboard/kontron/986lcd-m/auto.c
@@ -169,6 +169,13 @@ static void early_superio_config_w83627thg(void)
 	pnp_set_logical_device(dev);
 	pnp_set_enable(dev, 0);
 
+	/* Enable HWM */
+	dev=PNP_DEV(0x2e, W83627THG_HWM);
+	pnp_set_logical_device(dev);
+	pnp_set_enable(dev, 0);
+	pnp_set_iobase(dev, PNP_IDX_IO0, 0xa00);
+	pnp_set_enable(dev, 1);
+
 	pnp_exit_ext_func_mode(dev);
 
 	dev=PNP_DEV(0x4e, W83627THG_SP1);
diff --git a/src/southbridge/intel/i82801gx/Config.lb b/src/southbridge/intel/i82801gx/Config.lb
index 0ef93e89f1..6e5cba2338 100644
--- a/src/southbridge/intel/i82801gx/Config.lb
+++ b/src/southbridge/intel/i82801gx/Config.lb
@@ -1,7 +1,7 @@
 ##
 ## This file is part of the coreboot project.
 ##
-## Copyright (C) 2008 coresystems GmbH
+## Copyright (C) 2008-2009 coresystems GmbH
 ##
 ## 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
@@ -23,6 +23,7 @@ uses HAVE_SMI_HANDLER
 config chip.h
 driver i82801gx.o
 driver i82801gx_ac97.o
+driver i82801gx_azalia.o
 driver i82801gx_ide.o
 driver i82801gx_lpc.o
 driver i82801gx_nic.o
diff --git a/src/southbridge/intel/i82801gx/chip.h b/src/southbridge/intel/i82801gx/chip.h
index 755c317531..45c011a1c0 100644
--- a/src/southbridge/intel/i82801gx/chip.h
+++ b/src/southbridge/intel/i82801gx/chip.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2009 coresystems GmbH
  *
  * 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
@@ -22,10 +22,24 @@
 #define SOUTHBRIDGE_INTEL_I82801GX_CHIP_H
 
 struct southbridge_intel_i82801gx_config {
+	/* LPC configuration */
+	uint8_t pirqa_routing;
+	uint8_t pirqb_routing;
+	uint8_t pirqc_routing;
+	uint8_t pirqd_routing;
+	uint8_t pirqe_routing;
+	uint8_t pirqf_routing;
+	uint8_t pirqg_routing;
+	uint8_t pirqh_routing;
+
+	/* IDE configuration */
 	uint32_t ide_legacy_combined;
 	uint32_t ide_enable_primary;
 	uint32_t ide_enable_secondary;
 	uint32_t sata_ahci;
+
+	/* Azalia Configuration */
+	unsigned long hda_viddid;
 };
 
 extern struct chip_operations southbridge_intel_i82801gx_ops;
diff --git a/src/southbridge/intel/i82801gx/i82801gx_ac97.c b/src/southbridge/intel/i82801gx/i82801gx_ac97.c
index 3e07b5c1b7..4253284452 100644
--- a/src/southbridge/intel/i82801gx/i82801gx_ac97.c
+++ b/src/southbridge/intel/i82801gx/i82801gx_ac97.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2009 coresystems GmbH
  *
  * 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
@@ -22,16 +22,234 @@
 #include <device/device.h>
 #include <device/pci.h>
 #include <device/pci_ids.h>
+#include <arch/io.h>
+#include <delay.h>
 #include "i82801gx.h"
 
+#define NAMBAR		0x10
+#define   MASTER_VOL	0x02
+#define   PAGING	0x24
+#define   EXT_AUDIO	0x28
+#define   FUNC_SEL	0x66
+#define   INFO_IO	0x68
+#define   CONNECTOR	0x6a
+#define   VENDOR_ID1	0x7c
+#define   VENDOR_ID2	0x7e
+#define   SEC_VENDOR_ID1 0xfc
+#define   SEC_VENDOR_ID2 0xfe
+
+#define NABMBAR		0x14
+#define   GLOB_CNT	0x2c
+#define   GLOB_STA	0x30
+#define   CAS		0x34
+
+#define MMBAR		0x10
+#define   EXT_MODEM_ID1	0x3c
+#define   EXT_MODEM_ID2	0xbc
+
+#define MBAR		0x14
+#define   SEC_CODEC	0x40
+
+
+/* FIXME. This table is probably mainboard specific */
+static u16 ac97_function[16*2][4] = {
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) },
+	{ (1 << 5), (2 << 11), (1 << 10), (3 << 13) }
+};
+
+static u16 nabmbar;
+static u16 nambar;
+
+static int ac97_semaphore(void)
+{
+	int timeout;
+	u8 reg8;
+
+	timeout = 0xffff;
+	do {
+		reg8 = inb(nabmbar + CAS);
+		timeout--;
+	} while ((reg8 & 1) && timeout);
+	if (! timeout) {
+		printk_debug("Timeout!\n");
+	}
+
+	return (!timeout);
+}
+
+static void init_cnr(void)
+{
+	// TODO
+}
+
+static void program_sigid(struct device *dev, u32 id)
+{
+	pci_write_config32(dev, 0x2c, id);
+}
+
 static void ac97_audio_init(struct device *dev)
 {
-	// XXX init AC97 codecs.
+	u8 reg8;
+	u16 reg16;
+	u32 reg32;
+	int i;
+
+	printk_debug("Initializing AC'97 Audio.\n");
+
+	/* top 16 bits are zero, so don't read them */
+	nabmbar = pci_read_config16(dev, NABMBAR) & 0xfffe;
+	nambar = pci_read_config16(dev, NAMBAR) & 0xfffe;
+
+	reg16 = inw(nabmbar + GLOB_CNT);
+	reg16 |= (1 << 1); /* Remove AC_RESET# */
+	outw(reg16, nabmbar + GLOB_CNT);
+
+	/* Wait 600ms. Ouch. */
+	udelay(600 * 1000);
+
+	init_cnr();
+
+	/* Detect Primary AC'97 Codec */
+	reg32 = inl(nabmbar + GLOB_STA);
+	if ((reg32 & ((1 << 28) | (1 << 9) | (1 << 8))) == 0) {
+		/* Primary Codec not found */
+		printk_debug("No primary codec. Disabling AC'97 Audio.\n");
+		return;
+	}
+	
+	ac97_semaphore();
+
+	/* Detect if codec is programmable */
+	outw(0x8000, nambar + MASTER_VOL);
+	ac97_semaphore();
+	if (inw(nambar + MASTER_VOL) != 0x8000) {
+		printk_debug("Codec not programmable. Disabling AC'97 Audio.\n");
+		return;
+	}
+
+	/* Program Vendor IDs */
+	reg32 = inw(nambar + VENDOR_ID1);
+	reg32 <<= 16;
+	reg32 |= (u16)inw(nambar + VENDOR_ID2);
+
+	program_sigid(dev, reg32);
+
+	/* Is Codec AC'97 2.3 compliant? */
+	reg16 = inw(nambar + EXT_AUDIO);
+	/* [11:10] = 10b -> AC'97 2.3 */
+	if ((reg16 & 0x0c00) != 0x0800) {
+		/* No 2.3 Codec. We're done */
+		return;
+	}
+
+	/* Select Page 1 */
+	reg16 = inw(nambar + PAGING);
+	reg16 &= 0xfff0;
+	reg16 |= 0x0001;
+	outw(reg16, nambar + PAGING);
+
+	for (i = 0x0a * 2; i > 0; i--) {
+		outw(i, nambar + FUNC_SEL);
+
+		/* Function could not be selected. Next one */
+		if (inw(nambar + FUNC_SEL) != i)
+			continue;
+
+		reg16 = inw(nambar + INFO_IO);
+
+		/* Function Information present? */
+		if (!(reg16 & (1 << 0)))
+			continue;
+
+		/* Function Information valid? */
+		if (!(reg16 & (1 << 4)))
+			continue;
+
+		/* Program Buffer Delay [9:5] */
+		reg16 &= 0x03e0;
+		reg16 |= ac97_function[i][0];
+
+		/* Program Gain [15:11] */
+		reg16 |= ac97_function[i][1];
+
+		/* Program Inversion [10] */
+		reg16 |= ac97_function[i][2];
+
+		outw(reg16, nambar + INFO_IO);
+
+		/* Program Connector / Jack Location */
+		reg16 = inw(nambar + CONNECTOR);
+		reg16 &= 0x1fff;
+		reg16 |= ac97_function[i][3];
+		outw(reg16, nambar + CONNECTOR);
+	}
 }
 
 static void ac97_modem_init(struct device *dev)
 {
-	// XXX init modem?
+	u16 reg16;
+	u32 reg32;
+	u16 mmbar, mbar;
+
+	mmbar = pci_read_config16(dev, MMBAR) & 0xfffe;
+	mbar = pci_read_config16(dev, MBAR) & 0xfffe;
+
+	reg16 = inw(mmbar + EXT_MODEM_ID1);
+	if ((reg16 & 0xc000) != 0xc000 ) {
+		if (reg16 & (1 << 0)) {
+			reg32 = inw(mmbar + VENDOR_ID2);
+			reg32 <<= 16;
+			reg32 |= (u16)inw(mmbar + VENDOR_ID1);
+			program_sigid(dev, reg32);
+			return;
+		}
+	}
+
+	/* Secondary codec? */
+	reg16 = inw(mbar + SEC_CODEC);
+	if ((reg16 & (1 << 9)) == 0)
+		return;
+
+	reg16 = inw(mmbar + EXT_MODEM_ID2);
+	if ((reg16 & 0xc000) == 0x4000) {
+		if (reg16 & (1 << 0)) {
+			reg32 = inw(mmbar + SEC_VENDOR_ID2);
+			reg32 <<= 16;
+			reg32 |= (u16)inw(mmbar + SEC_VENDOR_ID1);
+			program_sigid(dev, reg32);
+			return;
+		}
+	}
 }
 
 static struct device_operations ac97_audio_ops = {
diff --git a/src/southbridge/intel/i82801gx/i82801gx_lpc.c b/src/southbridge/intel/i82801gx/i82801gx_lpc.c
index cb1f42c51d..78c4efe3b0 100644
--- a/src/southbridge/intel/i82801gx/i82801gx_lpc.c
+++ b/src/southbridge/intel/i82801gx/i82801gx_lpc.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2009 coresystems GmbH
  *
  * 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
@@ -24,6 +24,7 @@
 #include <device/pci_ids.h>
 #include <pc80/mc146818rtc.h>
 #include <pc80/isa-dma.h>
+#include <pc80/i8259.h>
 #include <arch/io.h>
 #include "i82801gx.h"
 
@@ -38,6 +39,8 @@
 
 #define NMI_OFF 0
 
+typedef struct southbridge_intel_i82801gx_config config_t;
+
 /* PIRQ[n]_ROUT[3:0] - PIRQ Routing Control
  * 0x00 - 0000 = Reserved
  * 0x01 - 0001 = Reserved
@@ -108,15 +111,44 @@ static void i82801gx_enable_serial_irqs(struct device *dev)
 
 static void i82801gx_pirq_init(device_t dev)
 {
-	pci_write_config8(dev, PIRQA_ROUT, 0x85);
-	pci_write_config8(dev, PIRQB_ROUT, 0x87);
-	pci_write_config8(dev, PIRQC_ROUT, 0x86);
-	pci_write_config8(dev, PIRQD_ROUT, 0x87);
-
-	pci_write_config8(dev, PIRQE_ROUT, 0x80);
-	pci_write_config8(dev, PIRQF_ROUT, 0x80);
-	pci_write_config8(dev, PIRQG_ROUT, 0x80);
-	pci_write_config8(dev, PIRQH_ROUT, 0x85);
+	device_t irq_dev;
+	/* Get the chip configuration */
+	config_t *config = dev->chip_info;
+
+	pci_write_config8(dev, PIRQA_ROUT, config->pirqa_routing);
+	pci_write_config8(dev, PIRQB_ROUT, config->pirqb_routing);
+	pci_write_config8(dev, PIRQC_ROUT, config->pirqc_routing);
+	pci_write_config8(dev, PIRQD_ROUT, config->pirqd_routing);
+
+	pci_write_config8(dev, PIRQE_ROUT, config->pirqe_routing);
+	pci_write_config8(dev, PIRQF_ROUT, config->pirqf_routing);
+	pci_write_config8(dev, PIRQG_ROUT, config->pirqg_routing);
+	pci_write_config8(dev, PIRQH_ROUT, config->pirqh_routing);
+
+	/* Eric Biederman once said we should let the OS do this.
+	 * I am not so sure anymore he was right.
+	 */
+
+	for(irq_dev = all_devices; irq_dev; irq_dev = irq_dev->next) {
+		u8 int_pin=0, int_line=0;
+
+		if (!irq_dev->enabled || irq_dev->path.type != DEVICE_PATH_PCI)
+			continue;
+
+		int_pin = pci_read_config8(irq_dev, PCI_INTERRUPT_PIN);
+
+		switch (int_pin) {
+		case 1: /* INTA# */ int_line = config->pirqa_routing; break;
+		case 2: /* INTB# */ int_line = config->pirqb_routing; break;
+		case 3: /* INTC# */ int_line = config->pirqc_routing; break;
+		case 4: /* INTD# */ int_line = config->pirqd_routing; break;
+		}
+
+		if (!int_line)
+			continue;
+
+		pci_write_config8(irq_dev, PCI_INTERRUPT_LINE, int_line);
+	}
 }
 
 static void i82801gx_power_options(device_t dev)
@@ -328,8 +360,8 @@ static struct device_operations device_ops = {
 	.ops_pci		= &pci_ops,
 };
 
-/* 82801GB/GR/GDH (ICH7/ICH7R/ICH7DH) */
-static const struct pci_driver ich7_ich7r_ich7dh_lpc __pci_driver = {
+/* 82801GB/GR (ICH7/ICH7R) */
+static const struct pci_driver ich7_ich7r_lpc __pci_driver = {
 	.ops	= &device_ops,
 	.vendor	= PCI_VENDOR_ID_INTEL,
 	.device	= 0x27b8,
diff --git a/src/southbridge/intel/i82801gx/i82801gx_pci.c b/src/southbridge/intel/i82801gx/i82801gx_pci.c
index 98b017c4e6..b2e4ce5561 100644
--- a/src/southbridge/intel/i82801gx/i82801gx_pci.c
+++ b/src/southbridge/intel/i82801gx/i82801gx_pci.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2009 coresystems GmbH
  *
  * 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
@@ -46,6 +46,9 @@ static void pci_init(struct device *dev)
 	reg16 = pci_read_config16(dev, 0x1e);
 	reg16 |= 0xf900;
 	pci_write_config16(dev, 0x1e, reg16);
+
+	/* Will this improve throughput of bus masters? */
+	pci_write_config8(dev, PCI_MIN_GNT, 0x06);
 }
 
 static void ich_pci_dev_enable_resources(struct device *dev)
diff --git a/src/southbridge/intel/i82801gx/i82801gx_pcie.c b/src/southbridge/intel/i82801gx/i82801gx_pcie.c
index d984a6a42f..882c3e4a78 100644
--- a/src/southbridge/intel/i82801gx/i82801gx_pcie.c
+++ b/src/southbridge/intel/i82801gx/i82801gx_pcie.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2009 coresystems GmbH
  *
  * 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
@@ -53,13 +53,13 @@ static void pci_init(struct device *dev)
 	pci_write_config16(dev, 0x1e, reg16);
 
 	reg32 = pci_read_config32(dev, 0x20);
-	printk_debug("    MBL    = 0x%08x\n", reg32);
+	printk_spew("    MBL    = 0x%08x\n", reg32);
 	reg32 = pci_read_config32(dev, 0x24);
-	printk_debug("    PMBL   = 0x%08x\n", reg32);
+	printk_spew("    PMBL   = 0x%08x\n", reg32);
 	reg32 = pci_read_config32(dev, 0x28);
-	printk_debug("    PMBU32 = 0x%08x\n", reg32);
+	printk_spew("    PMBU32 = 0x%08x\n", reg32);
 	reg32 = pci_read_config32(dev, 0x2c);
-	printk_debug("    PMLU32 = 0x%08x\n", reg32);
+	printk_spew("    PMLU32 = 0x%08x\n", reg32);
 }
 
 static void set_subsystem(device_t dev, unsigned vendor, unsigned device)
diff --git a/src/southbridge/intel/i82801gx/i82801gx_sata.c b/src/southbridge/intel/i82801gx/i82801gx_sata.c
index eced4b6d9a..7f17b73b0a 100644
--- a/src/southbridge/intel/i82801gx/i82801gx_sata.c
+++ b/src/southbridge/intel/i82801gx/i82801gx_sata.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2009 coresystems GmbH
  *
  * 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
@@ -133,6 +133,8 @@ static void sata_init(struct device *dev)
 	reg32 |= 0x121200aa;
 	pci_write_config32(dev, 0xa4, reg32);
 	pci_write_config8(dev, 0xa0, 0x00);
+
+	pci_write_config8(dev, PCI_INTERRUPT_LINE, 0);
 }
 
 static struct device_operations sata_ops = {
diff --git a/src/southbridge/intel/i82801gx/i82801gx_smihandler.c b/src/southbridge/intel/i82801gx/i82801gx_smihandler.c
index e15dcad531..11096fecd1 100644
--- a/src/southbridge/intel/i82801gx/i82801gx_smihandler.c
+++ b/src/southbridge/intel/i82801gx/i82801gx_smihandler.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the coreboot project.
  *
- * Copyright (C) 2008 coresystems GmbH
+ * Copyright (C) 2008-2009 coresystems GmbH
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -248,16 +248,26 @@ void southbridge_io_trap_handler(int smif)
 
 	printk_debug("SMI function trap 0x%x: ", smif);
 
-
 	switch (smif) {
 	case 0x32:
 		printk_debug("OS Init\n");
+		//gnvs->smif = 0;
+		break;
+	case 0xd5:
+		printk_debug("Set Brightness\n");
+		reg8 = gnvs->brtl;
+		printk_debug("brtl: %x\n", reg8);
+		outb(0x17, 0x66);
+		outb(reg8, 0x62);
+		//gnvs->smif = 0;
 		break;
 	case 0xd6:
 		printk_debug("Get Brightness\n");
 		outb(0x17, 0x66);
 		reg8 = inb(0x62);
+		printk_debug("brtl: %x\n", reg8);
 		gnvs->brtl = reg8;
+		//gnvs->smif = 0;
 		break;
 	default:
 		printk_debug("Unknown function\n");
@@ -401,12 +411,24 @@ void southbridge_smi_handler(unsigned int node, smm_state_save_area_t *state_sav
 
 	if (smi_sts & (1 << 4)) { // SLP_SMI
 		u32 reg32;
+
+		/* First, disable further SMIs */
+		reg8 = inb(pmbase + SMI_EN);
+		reg8 &= ~SLP_SMI_EN;
+		outb(reg8, pmbase + SMI_EN);
+
+		/* Next, do the deed, we should change
+		 * power on after power loss bits here
+		 * if we're going to S5
+		 */
+
+		/* Write back to the SLP register to cause the
+		 * originally intended event again.
+		 */
 		reg32 = inl(pmbase + 0x04);
 		printk_debug("SMI#: SLP = 0x%08x\n");
 		printk_debug("SMI#: Powering off.\n");
-		outl((6 << 10), pmbase + 0x04);
-		outl((1 << 13) | (6 << 10), pmbase + 0x04);
-		printk_debug("....\n");
+		outl(reg32, pmbase + 0x04);
 	}
 
 }