Update Kontron board
authorStefan Reinauer <stepan@coresystems.de>
Tue, 20 Jan 2009 22:53:10 +0000 (22:53 +0000)
committerStefan Reinauer <stepan@openbios.org>
Tue, 20 Jan 2009 22:53:10 +0000 (22:53 +0000)
 - 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

src/mainboard/kontron/986lcd-m/Config.lb
src/mainboard/kontron/986lcd-m/Options.lb
src/mainboard/kontron/986lcd-m/auto.c
src/southbridge/intel/i82801gx/Config.lb
src/southbridge/intel/i82801gx/chip.h
src/southbridge/intel/i82801gx/i82801gx_ac97.c
src/southbridge/intel/i82801gx/i82801gx_lpc.c
src/southbridge/intel/i82801gx/i82801gx_pci.c
src/southbridge/intel/i82801gx/i82801gx_pcie.c
src/southbridge/intel/i82801gx/i82801gx_sata.c
src/southbridge/intel/i82801gx/i82801gx_smihandler.c

index 9f965e29af87a595ec3cb163ef177ea7c51eb102..833478bb79e8307878b85fdd87bf87fde168f6fa 100644 (file)
@@ -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
index 926e69820b52d728ae2ebe8b47a6ae5b2e669577..dbf21d5801784145686ffd5e7b972b8441338599 100644 (file)
@@ -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
index c708f1ae15b8df358d17ab614700a1de0cd4b3e4..5a6d88a8502e4c5c3a2d13af122ee75394cd5b53 100644 (file)
@@ -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);
index 0ef93e89f14dba9c6c3573936775527f7e63bc3d..6e5cba2338cd622f7d5348e8f9d9d41df28c06e7 100644 (file)
@@ -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
index 755c3175312352183850fea4f0bbdf0d162a4d8c..45c011a1c0c8049f470893280207e1dd6f988367 100644 (file)
@@ -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
 #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;
index 3e07b5c1b7e915e2e51e99358d1c9a44f13cceff..4253284452ad5a83739a2f42181b9f48d0f3e0fb 100644 (file)
@@ -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
 #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 = {
index cb1f42c51d9248cbf3aa8b3b858f770c14315246..78c4efe3b040d731daaa3c7ba556e647347b5bae 100644 (file)
@@ -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,
index 98b017c4e67c87c304c2d456e9a28382b78a5b2c..b2e4ce55611104ff3ee0bc321c49169f6c11e8d7 100644 (file)
@@ -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)
index d984a6a42f55e0b014ea97fc0f19f08d3d87e9fa..882c3e4a78165a6cc292fdb9f4cf87fc07fead92 100644 (file)
@@ -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)
index eced4b6d9a856176ff86b9101b00432bb13ada58..7f17b73b0ac4070f1fd77c7c88611f17af2206b6 100644 (file)
@@ -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 = {
index e15dcad531bf0175219d8996fe02946023f02023..11096fecd15f079023eda1939d87090355e9152f 100644 (file)
@@ -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);
        }
 
 }