i82801gx: Support power-on-after-power-fail better
[coreboot.git] / src / southbridge / intel / i82801gx / smihandler.c
index 8e5569ad2fc4ec43bfba0a3ba4490755c1af49ea..401c9adcf83493b1f2f4201bf3bdb338d6cf777e 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
  * MA 02110-1301 USA
  */
 
+#include <types.h>
 #include <arch/io.h>
 #include <arch/romcc_io.h>
 #include <console/console.h>
 #include <cpu/x86/cache.h>
 #include <cpu/x86/smm.h>
-#include "chip.h"
-
-// Future TODO: Move to i82801gx directory
-#include "../../../northbridge/intel/i945/ich7.h"
-
-#define DEBUG_SMI
-
-#define ACPI_DISABLE   0x1e
-#define ACPI_ENABLE    0xe1
+#include <device/pci_def.h>
+#include <pc80/mc146818rtc.h>
+#include "i82801gx.h"
 
 /* I945 */
 #define SMRAM          0x9d
 #define   G_SMRANE     (1 << 3)
 #define   C_BASE_SEG   ((0 << 2) | (1 << 1) | (0 << 0))
 
-/* ICH7 */
-#define PM1_STS                0x00
-#define PM1_EN         0x02
-#define PM1_CNT                0x04
-#define PM1_TMR                0x08
-#define PROC_CNT       0x10
-#define LV2            0x14
-#define LV3            0x15
-#define LV4            0x16
-#define PM2_CNT                0x20 // mobile only
-#define GPE0_STS       0x28
-#define GPE0_EN                0x2c
-#define SMI_EN         0x30
-#define   EL_SMI_EN     (1 << 25) // Intel Quick Resume Technology
-#define   INTEL_USB2_EN         (1 << 18) // Intel-Specific USB2 SMI logic
-#define   LEGACY_USB2_EN (1 << 17) // Legacy USB2 SMI logic
-#define   PERIODIC_EN   (1 << 14) // SMI on PERIODIC_STS in SMI_STS
-#define   TCO_EN        (1 << 13) // Enable TCO Logic (BIOSWE et al)
-#define   MCSMI_EN      (1 << 11) // Trap microcontroller range access
-#define   BIOS_RLS      (1 <<  7) // asserts SCI on bit set
-#define   SWSMI_TMR_EN  (1 <<  6) // start software smi timer on bit set
-#define   APMC_EN       (1 <<  5) // Writes to APM_CNT cause SMI#
-#define   SLP_SMI_EN    (1 <<  4) // Write to SLP_EN in PM1_CNT asserts SMI#
-#define   LEGACY_USB_EN  (1 <<  3) // Legacy USB circuit SMI logic
-#define   BIOS_EN       (1 <<  2) // Assert SMI# on setting GBL_RLS bit
-#define   EOS           (1 <<  1) // End of SMI (deassert SMI#)
-#define   GBL_SMI_EN    (1 <<  0) // SMI# generation at all?
-#define SMI_STS                0x34
-#define ALT_GP_SMI_EN  0x38
-#define ALT_GP_SMI_STS 0x3a
-#define GPE_CNTL       0x42
-#define DEVACT_STS     0x44
-#define SS_CNT         0x50
-#define C3_RES         0x54
-
-#include "i82801gx_nvs.h"
+#include "nvs.h"
 
 /* While we read PMBASE dynamically in case it changed, let's
  * initialize it with a sane value
  */
-static u16 pmbase = DEFAULT_PMBASE;
-
-typedef enum { SMI_LOCKED, SMI_UNLOCKED } smi_semaphore;
-
-/* SMI multiprocessing semaphore */
-static volatile smi_semaphore smi_handler_status = SMI_UNLOCKED;
-
-static int smi_obtain_lock(void)
-{
-       u8 ret = SMI_LOCKED;
-
-       asm volatile (
-               "movb %2, %%al\n"
-               "xchgb %%al, %1\n"
-               "movb %%al, %0\n"
-               : "=g" (ret), "=m" (smi_handler_status)
-               : "g" (SMI_LOCKED)
-               : "eax"
-       );
-
-       return (ret == SMI_UNLOCKED);
-}
-
-static void smi_release_lock(void)
-{
-       asm volatile (
-               "movb %1, %%al\n"
-               "xchgb %%al, %0\n"
-               : "=m" (smi_handler_status)
-               : "g" (SMI_UNLOCKED)
-               : "eax"
-       );
-}
-
-#define LAPIC_ID 0xfee00020
-static inline __attribute__((always_inline)) unsigned long nodeid(void)
-{
-       return (*((volatile unsigned long *)(LAPIC_ID)) >> 24);
-}
+u16 pmbase = DEFAULT_PMBASE;
+u8 smm_initialized = 0;
 
+/* GNVS needs to be updated by an 0xEA PM Trap (B2) after it has been located
+ * by coreboot.
+ */
+global_nvs_t *gnvs = (global_nvs_t *)0x0;
+void *tcg = (void *)0x0;
+void *smi1 = (void *)0x0;
 
 /**
- * @brief read and clear PM1_STS 
+ * @brief read and clear PM1_STS
  * @return PM1_STS register
  */
 static u16 reset_pm1_status(void)
 {
        u16 reg16;
-       
+
        reg16 = inw(pmbase + PM1_STS);
        /* set status bits are cleared by writing 1 to them */
        outw(reg16, pmbase + PM1_STS);
-       
+
        return reg16;
 }
 
 static void dump_pm1_status(u16 pm1_sts)
 {
-       printk_debug("PM1_STS: ");
-       if (pm1_sts & (1 << 15)) printk_debug("WAK ");
-       if (pm1_sts & (1 << 14)) printk_debug("PCIEXPWAK ");
-       if (pm1_sts & (1 << 11)) printk_debug("PRBTNOR ");
-       if (pm1_sts & (1 << 10)) printk_debug("RTC ");
-       if (pm1_sts & (1 <<  8)) printk_debug("PWRBTN ");
-       if (pm1_sts & (1 <<  5)) printk_debug("GBL ");
-       if (pm1_sts & (1 <<  4)) printk_debug("BM ");
-       if (pm1_sts & (1 <<  0)) printk_debug("TMROF ");
-       printk_debug("\n");
+       printk(BIOS_SPEW, "PM1_STS: ");
+       if (pm1_sts & (1 << 15)) printk(BIOS_SPEW, "WAK ");
+       if (pm1_sts & (1 << 14)) printk(BIOS_SPEW, "PCIEXPWAK ");
+       if (pm1_sts & (1 << 11)) printk(BIOS_SPEW, "PRBTNOR ");
+       if (pm1_sts & (1 << 10)) printk(BIOS_SPEW, "RTC ");
+       if (pm1_sts & (1 <<  8)) printk(BIOS_SPEW, "PWRBTN ");
+       if (pm1_sts & (1 <<  5)) printk(BIOS_SPEW, "GBL ");
+       if (pm1_sts & (1 <<  4)) printk(BIOS_SPEW, "BM ");
+       if (pm1_sts & (1 <<  0)) printk(BIOS_SPEW, "TMROF ");
+       printk(BIOS_SPEW, "\n");
+       int reg16 = inw(pmbase + PM1_EN);
+       printk(BIOS_SPEW, "PM1_EN: %x\n", reg16);
 }
 
 /**
- * @brief read and clear SMI_STS 
+ * @brief read and clear SMI_STS
  * @return SMI_STS register
  */
 static u32 reset_smi_status(void)
 {
        u32 reg32;
-       
+
        reg32 = inl(pmbase + SMI_STS);
        /* set status bits are cleared by writing 1 to them */
        outl(reg32, pmbase + SMI_STS);
-       
+
        return reg32;
 }
 
 static void dump_smi_status(u32 smi_sts)
 {
-       printk_debug("SMI_STS: ");
-       if (smi_sts & (1 << 26)) printk_debug("SPI ");
-       if (smi_sts & (1 << 25)) printk_debug("EL_SMI ");
-       if (smi_sts & (1 << 21)) printk_debug("MONITOR ");
-       if (smi_sts & (1 << 20)) printk_debug("PCI_EXP_SMI ");
-       if (smi_sts & (1 << 18)) printk_debug("INTEL_USB2 ");
-       if (smi_sts & (1 << 17)) printk_debug("LEGACY_USB2 ");
-       if (smi_sts & (1 << 16)) printk_debug("SMBUS_SMI ");
-       if (smi_sts & (1 << 15)) printk_debug("SERIRQ_SMI ");
-       if (smi_sts & (1 << 14)) printk_debug("PERIODIC ");
-       if (smi_sts & (1 << 13)) printk_debug("TCO ");
-       if (smi_sts & (1 << 12)) printk_debug("DEVMON ");
-       if (smi_sts & (1 << 11)) printk_debug("MCSMI ");
-       if (smi_sts & (1 << 10)) printk_debug("GPI ");
-       if (smi_sts & (1 <<  9)) printk_debug("GPE0 ");
-       if (smi_sts & (1 <<  8)) printk_debug("PM1 ");
-       if (smi_sts & (1 <<  6)) printk_debug("SWSMI_TMR ");
-       if (smi_sts & (1 <<  5)) printk_debug("APM ");
-       if (smi_sts & (1 <<  4)) printk_debug("SLP_SMI ");
-       if (smi_sts & (1 <<  3)) printk_debug("LEGACY_USB ");
-       if (smi_sts & (1 <<  2)) printk_debug("BIOS ");
-       printk_debug("\n");
+       printk(BIOS_DEBUG, "SMI_STS: ");
+       if (smi_sts & (1 << 26)) printk(BIOS_DEBUG, "SPI ");
+       if (smi_sts & (1 << 25)) printk(BIOS_DEBUG, "EL_SMI ");
+       if (smi_sts & (1 << 21)) printk(BIOS_DEBUG, "MONITOR ");
+       if (smi_sts & (1 << 20)) printk(BIOS_DEBUG, "PCI_EXP_SMI ");
+       if (smi_sts & (1 << 18)) printk(BIOS_DEBUG, "INTEL_USB2 ");
+       if (smi_sts & (1 << 17)) printk(BIOS_DEBUG, "LEGACY_USB2 ");
+       if (smi_sts & (1 << 16)) printk(BIOS_DEBUG, "SMBUS_SMI ");
+       if (smi_sts & (1 << 15)) printk(BIOS_DEBUG, "SERIRQ_SMI ");
+       if (smi_sts & (1 << 14)) printk(BIOS_DEBUG, "PERIODIC ");
+       if (smi_sts & (1 << 13)) printk(BIOS_DEBUG, "TCO ");
+       if (smi_sts & (1 << 12)) printk(BIOS_DEBUG, "DEVMON ");
+       if (smi_sts & (1 << 11)) printk(BIOS_DEBUG, "MCSMI ");
+       if (smi_sts & (1 << 10)) printk(BIOS_DEBUG, "GPI ");
+       if (smi_sts & (1 <<  9)) printk(BIOS_DEBUG, "GPE0 ");
+       if (smi_sts & (1 <<  8)) printk(BIOS_DEBUG, "PM1 ");
+       if (smi_sts & (1 <<  6)) printk(BIOS_DEBUG, "SWSMI_TMR ");
+       if (smi_sts & (1 <<  5)) printk(BIOS_DEBUG, "APM ");
+       if (smi_sts & (1 <<  4)) printk(BIOS_DEBUG, "SLP_SMI ");
+       if (smi_sts & (1 <<  3)) printk(BIOS_DEBUG, "LEGACY_USB ");
+       if (smi_sts & (1 <<  2)) printk(BIOS_DEBUG, "BIOS ");
+       printk(BIOS_DEBUG, "\n");
 }
 
 
@@ -201,351 +132,530 @@ static void dump_smi_status(u32 smi_sts)
 static u32 reset_gpe0_status(void)
 {
        u32 reg32;
-       
+
        reg32 = inl(pmbase + GPE0_STS);
        /* set status bits are cleared by writing 1 to them */
        outl(reg32, pmbase + GPE0_STS);
-       
+
        return reg32;
 }
 
 static void dump_gpe0_status(u32 gpe0_sts)
 {
        int i;
-       printk_debug("GPE0_STS: ");
+       printk(BIOS_DEBUG, "GPE0_STS: ");
        for (i=31; i<= 16; i--) {
-               if (gpe0_sts & (1 << i)) printk_debug("GPIO%d ", (i-16));
+               if (gpe0_sts & (1 << i)) printk(BIOS_DEBUG, "GPIO%d ", (i-16));
        }
-       if (gpe0_sts & (1 << 14)) printk_debug("USB4 ");
-       if (gpe0_sts & (1 << 13)) printk_debug("PME_B0 ");
-       if (gpe0_sts & (1 << 12)) printk_debug("USB3 ");
-       if (gpe0_sts & (1 << 11)) printk_debug("PME ");
-       if (gpe0_sts & (1 << 10)) printk_debug("EL_SCI/BATLOW ");
-       if (gpe0_sts & (1 <<  9)) printk_debug("PCI_EXP ");
-       if (gpe0_sts & (1 <<  8)) printk_debug("RI ");
-       if (gpe0_sts & (1 <<  7)) printk_debug("SMB_WAK ");
-       if (gpe0_sts & (1 <<  6)) printk_debug("TCO_SCI ");
-       if (gpe0_sts & (1 <<  5)) printk_debug("AC97 ");
-       if (gpe0_sts & (1 <<  4)) printk_debug("USB2 ");
-       if (gpe0_sts & (1 <<  3)) printk_debug("USB1 ");
-       if (gpe0_sts & (1 <<  2)) printk_debug("HOT_PLUG ");
-       if (gpe0_sts & (1 <<  0)) printk_debug("THRM ");
-       printk_debug("\n");
+       if (gpe0_sts & (1 << 14)) printk(BIOS_DEBUG, "USB4 ");
+       if (gpe0_sts & (1 << 13)) printk(BIOS_DEBUG, "PME_B0 ");
+       if (gpe0_sts & (1 << 12)) printk(BIOS_DEBUG, "USB3 ");
+       if (gpe0_sts & (1 << 11)) printk(BIOS_DEBUG, "PME ");
+       if (gpe0_sts & (1 << 10)) printk(BIOS_DEBUG, "EL_SCI/BATLOW ");
+       if (gpe0_sts & (1 <<  9)) printk(BIOS_DEBUG, "PCI_EXP ");
+       if (gpe0_sts & (1 <<  8)) printk(BIOS_DEBUG, "RI ");
+       if (gpe0_sts & (1 <<  7)) printk(BIOS_DEBUG, "SMB_WAK ");
+       if (gpe0_sts & (1 <<  6)) printk(BIOS_DEBUG, "TCO_SCI ");
+       if (gpe0_sts & (1 <<  5)) printk(BIOS_DEBUG, "AC97 ");
+       if (gpe0_sts & (1 <<  4)) printk(BIOS_DEBUG, "USB2 ");
+       if (gpe0_sts & (1 <<  3)) printk(BIOS_DEBUG, "USB1 ");
+       if (gpe0_sts & (1 <<  2)) printk(BIOS_DEBUG, "HOT_PLUG ");
+       if (gpe0_sts & (1 <<  0)) printk(BIOS_DEBUG, "THRM ");
+       printk(BIOS_DEBUG, "\n");
 }
 
 
 /**
- * @brief read and clear TCOx_STS 
+ * @brief read and clear TCOx_STS
  * @return TCOx_STS registers
  */
 static u32 reset_tco_status(void)
 {
        u32 tcobase = pmbase + 0x60;
        u32 reg32;
-       
+
        reg32 = inl(tcobase + 0x04);
        /* set status bits are cleared by writing 1 to them */
        outl(reg32 & ~(1<<18), tcobase + 0x04); //  Don't clear BOOT_STS before SECOND_TO_STS
        if (reg32 & (1 << 18))
                outl(reg32 & (1<<18), tcobase + 0x04); // clear BOOT_STS
-       
+
        return reg32;
 }
 
 
 static void dump_tco_status(u32 tco_sts)
 {
-       printk_debug("TCO_STS: ");
-       if (tco_sts & (1 << 20)) printk_debug("SMLINK_SLV ");
-       if (tco_sts & (1 << 18)) printk_debug("BOOT ");
-       if (tco_sts & (1 << 17)) printk_debug("SECOND_TO ");
-       if (tco_sts & (1 << 16)) printk_debug("INTRD_DET ");
-       if (tco_sts & (1 << 12)) printk_debug("DMISERR ");
-       if (tco_sts & (1 << 10)) printk_debug("DMISMI ");
-       if (tco_sts & (1 <<  9)) printk_debug("DMISCI ");
-       if (tco_sts & (1 <<  8)) printk_debug("BIOSWR ");
-       if (tco_sts & (1 <<  7)) printk_debug("NEWCENTURY ");
-       if (tco_sts & (1 <<  3)) printk_debug("TIMEOUT ");
-       if (tco_sts & (1 <<  2)) printk_debug("TCO_INT ");
-       if (tco_sts & (1 <<  1)) printk_debug("SW_TCO ");
-       if (tco_sts & (1 <<  0)) printk_debug("NMI2SMI ");
-       printk_debug("\n");
+       printk(BIOS_DEBUG, "TCO_STS: ");
+       if (tco_sts & (1 << 20)) printk(BIOS_DEBUG, "SMLINK_SLV ");
+       if (tco_sts & (1 << 18)) printk(BIOS_DEBUG, "BOOT ");
+       if (tco_sts & (1 << 17)) printk(BIOS_DEBUG, "SECOND_TO ");
+       if (tco_sts & (1 << 16)) printk(BIOS_DEBUG, "INTRD_DET ");
+       if (tco_sts & (1 << 12)) printk(BIOS_DEBUG, "DMISERR ");
+       if (tco_sts & (1 << 10)) printk(BIOS_DEBUG, "DMISMI ");
+       if (tco_sts & (1 <<  9)) printk(BIOS_DEBUG, "DMISCI ");
+       if (tco_sts & (1 <<  8)) printk(BIOS_DEBUG, "BIOSWR ");
+       if (tco_sts & (1 <<  7)) printk(BIOS_DEBUG, "NEWCENTURY ");
+       if (tco_sts & (1 <<  3)) printk(BIOS_DEBUG, "TIMEOUT ");
+       if (tco_sts & (1 <<  2)) printk(BIOS_DEBUG, "TCO_INT ");
+       if (tco_sts & (1 <<  1)) printk(BIOS_DEBUG, "SW_TCO ");
+       if (tco_sts & (1 <<  0)) printk(BIOS_DEBUG, "NMI2SMI ");
+       printk(BIOS_DEBUG, "\n");
 }
 
+/* We are using PCIe accesses for now
+ *  1. the chipset can do it
+ *  2. we don't need to worry about how we leave 0xcf8/0xcfc behind
+ */
+#include "../../../northbridge/intel/i945/pcie_config.c"
 
-/* ********************* smi_util ************************* */
-
-/* Data */
-#define UART_RBR 0x00
-#define UART_TBR 0x00
-
-/* Control */
-#define UART_IER 0x01
-#define UART_IIR 0x02
-#define UART_FCR 0x02
-#define UART_LCR 0x03
-#define UART_MCR 0x04
-#define UART_DLL 0x00
-#define UART_DLM 0x01
-
-/* Status */
-#define UART_LSR 0x05
-#define UART_MSR 0x06
-#define UART_SCR 0x07
-
-static int uart_can_tx_byte(void)
+int southbridge_io_trap_handler(int smif)
 {
-       return inb(TTYS0_BASE + UART_LSR) & 0x20;
-}
+       switch (smif) {
+       case 0x32:
+               printk(BIOS_DEBUG, "OS Init\n");
+               /* gnvs->smif:
+                *  On success, the IO Trap Handler returns 0
+                *  On failure, the IO Trap Handler returns a value != 0
+                */
+               gnvs->smif = 0;
+               return 1; /* IO trap handled */
+       }
 
-static void uart_wait_to_tx_byte(void)
-{
-       while(!uart_can_tx_byte()) 
-       ;
+       /* Not handled */
+       return 0;
 }
 
-static void uart_wait_until_sent(void)
+/**
+ * @brief Set the EOS bit
+ */
+void southbridge_smi_set_eos(void)
 {
-       while(!(inb(TTYS0_BASE + UART_LSR) & 0x40))
-       ; 
-}
+       u8 reg8;
 
-static void uart_tx_byte(unsigned char data)
-{
-       uart_wait_to_tx_byte();
-       outb(data, TTYS0_BASE + UART_TBR);
-       /* Make certain the data clears the fifos */
-       uart_wait_until_sent();
+       reg8 = inb(pmbase + SMI_EN);
+       reg8 |= EOS;
+       outb(reg8, pmbase + SMI_EN);
 }
 
-void console_tx_flush(void)
+static void busmaster_disable_on_bus(int bus)
 {
-       uart_wait_to_tx_byte();
+        int slot, func;
+        unsigned int val;
+        unsigned char hdr;
+
+        for (slot = 0; slot < 0x20; slot++) {
+                for (func = 0; func < 8; func++) {
+                        u32 reg32;
+                        device_t dev = PCI_DEV(bus, slot, func);
+
+                        val = pci_read_config32(dev, PCI_VENDOR_ID);
+
+                        if (val == 0xffffffff || val == 0x00000000 ||
+                            val == 0x0000ffff || val == 0xffff0000)
+                                continue;
+
+                        /* Disable Bus Mastering for this one device */
+                        reg32 = pci_read_config32(dev, PCI_COMMAND);
+                        reg32 &= ~PCI_COMMAND_MASTER;
+                        pci_write_config32(dev, PCI_COMMAND, reg32);
+
+                        /* If this is a bridge, then follow it. */
+                        hdr = pci_read_config8(dev, PCI_HEADER_TYPE);
+                        hdr &= 0x7f;
+                        if (hdr == PCI_HEADER_TYPE_BRIDGE ||
+                            hdr == PCI_HEADER_TYPE_CARDBUS) {
+                                unsigned int buses;
+                                buses = pci_read_config32(dev, PCI_PRIMARY_BUS);
+                                busmaster_disable_on_bus((buses >> 8) & 0xff);
+                        }
+                }
+        }
 }
 
-void console_tx_byte(unsigned char byte)
+
+static void southbridge_smi_sleep(unsigned int node, smm_state_save_area_t *state_save)
 {
-       if (byte == '\n')
-               uart_tx_byte('\r');
-       uart_tx_byte(byte);
-}
+       u8 reg8;
+       u32 reg32;
+       u8 slp_typ;
+       u8 s5pwr = CONFIG_MAINBOARD_POWER_ON_AFTER_POWER_FAIL;
+
+       // save and recover RTC port values
+       u8 tmp70, tmp72;
+       tmp70 = inb(0x70);
+       tmp72 = inb(0x72);
+       get_option(&s5pwr, "power_on_after_fail");
+       outb(tmp70, 0x70);
+       outb(tmp72, 0x72);
+
+       /* First, disable further SMIs */
+       reg8 = inb(pmbase + SMI_EN);
+       reg8 &= ~SLP_SMI_EN;
+       outb(reg8, pmbase + SMI_EN);
 
-/* We are using PCIe accesses for now
- *  1. the chipset can do it
- *  2. we don't need to worry about how we leave 0xcf8/0xcfc behind
- */
-#include "../../../northbridge/intel/i945/pcie_config.c"
+       /* Figure out SLP_TYP */
+       reg32 = inl(pmbase + PM1_CNT);
+       printk(BIOS_SPEW, "SMI#: SLP = 0x%08x\n", reg32);
+       slp_typ = (reg32 >> 10) & 7;
+
+       /* Next, do the deed.
+        */
+
+       switch (slp_typ) {
+       case 0: printk(BIOS_DEBUG, "SMI#: Entering S0 (On)\n"); break;
+       case 1: printk(BIOS_DEBUG, "SMI#: Entering S1 (Assert STPCLK#)\n"); break;
+       case 5:
+               printk(BIOS_DEBUG, "SMI#: Entering S3 (Suspend-To-RAM)\n");
+               /* Invalidate the cache before going to S3 */
+               wbinvd();
+               break;
+       case 6: printk(BIOS_DEBUG, "SMI#: Entering S4 (Suspend-To-Disk)\n"); break;
+       case 7:
+               printk(BIOS_DEBUG, "SMI#: Entering S5 (Soft Power off)\n");
+
+               outl(0, pmbase + GPE0_EN);
+
+               /* Always set the flag in case CMOS was changed on runtime. For
+                * "KEEP", switch to "OFF" - KEEP is software emulated
+                */
+               reg8 = pcie_read_config8(PCI_DEV(0, 0x1f, 0), GEN_PMCON_3);
+               if (s5pwr == MAINBOARD_POWER_ON) {
+                       reg8 &= ~1;
+               } else {
+                       reg8 |= 1;
+               }
+               pcie_write_config8(PCI_DEV(0, 0x1f, 0), GEN_PMCON_3, reg8);
+
+               /* also iterates over all bridges on bus 0 */
+               busmaster_disable_on_bus(0);
+               break;
+       default: printk(BIOS_DEBUG, "SMI#: ERROR: SLP_TYP reserved\n"); break;
+       }
 
-/* ********************* smi_util ************************* */
+       /* Unlock the SMI semaphore. We're currently in SMM, and the semaphore
+        * will never be unlocked because the next outl will switch off the CPU.
+        * This might open a small race between the smi_release_lock() and the outl()
+        * for other SMI handlers. Not sure if this could cause trouble. */
+        if (slp_typ == 5)
+               smi_release_lock();
 
+       /* Write back to the SLP register to cause the originally intended
+        * event again. We need to set BIT13 (SLP_EN) though to make the
+        * sleep happen.
+        */
+       outl(reg32 | SLP_EN, pmbase + PM1_CNT);
 
-void io_trap_handler(int smif)
+       /* In most sleep states, the code flow of this function ends at
+        * the line above. However, if we entered sleep state S1 and wake
+        * up again, we will continue to execute code in this function.
+        */
+       reg32 = inl(pmbase + PM1_CNT);
+       if (reg32 & SCI_EN) {
+               /* The OS is not an ACPI OS, so we set the state to S0 */
+               reg32 &= ~(SLP_EN | SLP_TYP);
+               outl(reg32, pmbase + PM1_CNT);
+       }
+}
+
+static void southbridge_smi_apmc(unsigned int node, smm_state_save_area_t *state_save)
 {
+       u32 pmctrl;
        u8 reg8;
-       global_nvs_t *gnvs = (global_nvs_t *)0xc00;
 
-       printk_debug("SMI function trap 0x%x: ", smif);
+       /* Emulate B2 register as the FADT / Linux expects it */
 
+       reg8 = inb(APM_CNT);
+       if (mainboard_apm_cnt && mainboard_apm_cnt(reg8))
+               return;
 
-       switch (smif) {
-       case 0x32:
-               printk_debug("OS Init\n");
+       switch (reg8) {
+       case APM_CNT_CST_CONTROL:
+               /* Calling this function seems to cause
+                * some kind of race condition in Linux
+                * and causes a kernel oops
+                */
+               printk(BIOS_DEBUG, "C-state control\n");
                break;
-       case 0xd6:
-               printk_debug("Get Brightness\n");
-               outb(0x17, 0x66);
-               reg8 = inb(0x62);
-               gnvs->brtl = reg8;
+       case APM_CNT_PST_CONTROL:
+               /* Calling this function seems to cause
+                * some kind of race condition in Linux
+                * and causes a kernel oops
+                */
+               printk(BIOS_DEBUG, "P-state control\n");
                break;
-       default:
-               printk_debug("Unknown function\n");
+       case APM_CNT_ACPI_DISABLE:
+               pmctrl = inl(pmbase + PM1_CNT);
+               pmctrl &= ~SCI_EN;
+               outl(pmctrl, pmbase + PM1_CNT);
+               printk(BIOS_DEBUG, "SMI#: ACPI disabled.\n");
                break;
+       case APM_CNT_ACPI_ENABLE:
+               pmctrl = inl(pmbase + PM1_CNT);
+               pmctrl |= SCI_EN;
+               outl(pmctrl, pmbase + PM1_CNT);
+               printk(BIOS_DEBUG, "SMI#: ACPI enabled.\n");
+               break;
+       case APM_CNT_GNVS_UPDATE:
+               if (smm_initialized) {
+                       printk(BIOS_DEBUG, "SMI#: SMM structures already initialized!\n");
+                       return;
+               }
+               gnvs = *(global_nvs_t **)0x500;
+               tcg  = *(void **)0x504;
+               smi1 = *(void **)0x508;
+               smm_initialized = 1;
+               printk(BIOS_DEBUG, "SMI#: Setting up structures to %p, %p, %p\n", gnvs, tcg, smi1);
+               break;
+       default:
+               printk(BIOS_DEBUG, "SMI#: Unknown function APM_CNT=%02x\n", reg8);
        }
+}
 
-       /* On success, the IO Trap Handler returns 0
-        * On failure, the IO Trap Handler returns a value != 0
-        *
-        * For now, we force the return value to 0 and log all traps to
-        * see what's going on.
+static void southbridge_smi_pm1(unsigned int node, smm_state_save_area_t *state_save)
+{
+       u16 pm1_sts;
+       volatile u8 cmos_status;
+
+       pm1_sts = reset_pm1_status();
+       dump_pm1_status(pm1_sts);
+
+       /* While OSPM is not active, poweroff immediately
+        * on a power button event.
         */
-       //gnvs->smif = 0;
+       if (pm1_sts & PWRBTN_STS) {
+               // power button pressed
+               u32 reg32;
+               reg32 = (7 << 10) | (1 << 13);
+               outl(reg32, pmbase + PM1_CNT);
+       }
+
+       if (pm1_sts & RTC_STS) {
+               /* read RTC status register to disable the interrupt */
+               cmos_status = cmos_read(RTC_REG_C);
+               printk(BIOS_DEBUG, "RTC IRQ status: %02X\n", cmos_status);
+       }
 }
 
-/**
- * @brief Set the EOS bit
- */
-static void smi_set_eos(void)
+static void southbridge_smi_gpe0(unsigned int node, smm_state_save_area_t *state_save)
 {
-       u8 reg8;
-       
-       reg8 = inb(pmbase + SMI_EN);
-       reg8 |= EOS;
-       outb(reg8, pmbase + SMI_EN);
+       u32 gpe0_sts;
+
+       gpe0_sts = reset_gpe0_status();
+       dump_gpe0_status(gpe0_sts);
 }
 
-/**
- * @brief Interrupt handler for SMI#
- *
- * @param smm_revision revision of the smm state save map
- */
+static void southbridge_smi_gpi(unsigned int node, smm_state_save_area_t *state_save)
+{
+       u16 reg16;
+       reg16 = inw(pmbase + ALT_GP_SMI_STS);
+       outl(reg16, pmbase + ALT_GP_SMI_STS);
+
+       reg16 &= inw(pmbase + ALT_GP_SMI_EN);
+
+       if (mainboard_smi_gpi) {
+               mainboard_smi_gpi(reg16);
+       } else {
+               if (reg16)
+                       printk(BIOS_DEBUG, "GPI (mask %04x)\n",reg16);
+       }
+}
 
-void smi_handler(u32 smm_revision)
+static void southbridge_smi_mc(unsigned int node, smm_state_save_area_t *state_save)
 {
-       u8 reg8;
-       u16 pmctrl;
-       u16 pm1_sts;
-       u32 smi_sts, gpe0_sts, tco_sts;
-       unsigned int node;
-       smm_state_save_area_t state_save;
+       u32 reg32;
+
+       reg32 = inl(pmbase + SMI_EN);
 
-       /* Are we ok to execute the handler? */
-       if (!smi_obtain_lock())
+       /* Are periodic SMIs enabled? */
+       if ((reg32 & MCSMI_EN) == 0)
                return;
 
-       node=nodeid();
+       printk(BIOS_DEBUG, "Microcontroller SMI.\n");
+}
 
-#ifdef DEBUG_SMI
-       console_loglevel = DEFAULT_CONSOLE_LOGLEVEL;
-#else
-       console_loglevel = 1;
-#endif
 
-       printk_debug("\nSMI# #%d\n", node);
 
-       switch (smm_revision) {
-       case 0x00030007:
-               state_save.type = LEGACY;
-               state_save.legacy_state_save = (legacy_smm_state_save_area_t *)
-                       (0xa8000 + 0x7e00 - (node * 0x400));
-               break;
-       case 0x00030100:
-               state_save.type = EM64T;
-               state_save.em64t_state_save = (em64t_smm_state_save_area_t *)
-                       (0xa8000 + 0x7d00 - (node * 0x400));
-               break;
-       default:
-               printk_debug("smm_revision: 0x%08x\n", smm_revision);
-               printk_debug("SMI# not supported on your CPU\n");
-               /* Don't release lock, so no further SMI will happen,
-                * if we don't handle it anyways.
-                */
+static void southbridge_smi_tco(unsigned int node, smm_state_save_area_t *state_save)
+{
+       u32 tco_sts;
+
+       tco_sts = reset_tco_status();
+
+       /* Any TCO event? */
+       if (!tco_sts)
                return;
+
+       if (tco_sts & (1 << 8)) { // BIOSWR
+               u8 bios_cntl;
+
+               bios_cntl = pcie_read_config16(PCI_DEV(0, 0x1f, 0), 0xdc);
+
+               if (bios_cntl & 1) {
+                       /* BWE is RW, so the SMI was caused by a
+                        * write to BWE, not by a write to the BIOS
+                        */
+
+                       /* This is the place where we notice someone
+                        * is trying to tinker with the BIOS. We are
+                        * trying to be nice and just ignore it. A more
+                        * resolute answer would be to power down the
+                        * box.
+                        */
+                       printk(BIOS_DEBUG, "Switching back to RO\n");
+                       pcie_write_config32(PCI_DEV(0, 0x1f, 0), 0xdc, (bios_cntl & ~1));
+               } /* No else for now? */
+       } else if (tco_sts & (1 << 3)) { /* TIMEOUT */
+               /* Handle TCO timeout */
+               printk(BIOS_DEBUG, "TCO Timeout.\n");
+       } else if (!tco_sts) {
+               dump_tco_status(tco_sts);
        }
+}
 
-       pmbase = pcie_read_config16(PCI_DEV(0, 0x1f, 0), 0x40) & 0xfffc;
-       printk_spew("SMI#: pmbase = 0x%04x\n", pmbase);
-       
-       /* We need to clear the SMI status registers, or we won't see what's
-        * happening in the following calls.
-        */
-       smi_sts = reset_smi_status();
-       dump_smi_status(smi_sts);
+static void southbridge_smi_periodic(unsigned int node, smm_state_save_area_t *state_save)
+{
+       u32 reg32;
 
-       if (smi_sts & (1 << 21)) { // MONITOR
-               global_nvs_t *gnvs = (global_nvs_t *)0xc00;
-               int i;
-               u32 reg32;
+       reg32 = inl(pmbase + SMI_EN);
 
-               reg32 = RCBA32(0x1e00); // TRSR - Trap Status Register
-#if 0
-               /* Comment in for some useful debug */
-               for (i=0; i<4; i++) {
-                       if (reg32 & (1 << i)) {
-                               printk_debug("  io trap #%d\n", i);
-                       }
-               }
-#endif
-               RCBA32(0x1e00) = reg32; // TRSR
+       /* Are periodic SMIs enabled? */
+       if ((reg32 & PERIODIC_EN) == 0)
+               return;
 
-               reg32 = RCBA32(0x1e10);
+       printk(BIOS_DEBUG, "Periodic SMI.\n");
+}
 
-               if ((reg32 & 0xfffc) != 0x808) {
-                       printk_debug("  trapped io address = 0x%x\n", reg32 & 0xfffc);
-                       printk_debug("  AHBE = %x\n", (reg32 >> 16) & 0xf);
-                       printk_debug("  read/write: %s\n", (reg32 & (1 << 24)) ? "read" :
-                               "write");
-               }
+static void southbridge_smi_monitor(unsigned int node, smm_state_save_area_t *state_save)
+{
+#define IOTRAP(x) (trap_sts & (1 << x))
+       u32 trap_sts, trap_cycle;
+       u32 data, mask = 0;
+       int i;
 
-               if (!(reg32 & (1 << 24))) {
-                       /* Write Cycle */
-                       reg32 = RCBA32(0x1e18);
-                       printk_debug("  iotrap written data = 0x%08x\n", reg32);
+       trap_sts = RCBA32(0x1e00); // TRSR - Trap Status Register
+       RCBA32(0x1e00) = trap_sts; // Clear trap(s) in TRSR
 
-               }
+       trap_cycle = RCBA32(0x1e10);
+       for (i=16; i<20; i++) {
+               if (trap_cycle & (1 << i))
+                       mask |= (0xff << ((i - 16) << 2));
+       }
 
-               if (gnvs->smif)
+
+       /* IOTRAP(3) SMI function call */
+       if (IOTRAP(3)) {
+               if (gnvs && gnvs->smif)
                        io_trap_handler(gnvs->smif); // call function smif
+               return;
        }
 
-       if (smi_sts & (1 << 13)) { // TCO
-               tco_sts = reset_tco_status();
-               dump_tco_status(tco_sts);
-
-               if (tco_sts & (1 << 8)) { // BIOSWR
-                       u8 bios_cntl;
-
-                       bios_cntl = pcie_read_config16(PCI_DEV(0, 0x1f, 0), 0xdc);
-
-                       if (bios_cntl & 1) {
-                               /* BWE is RW, so the SMI was caused by a
-                                * write to BWE, not by a write to the BIOS
-                                */
-
-                               /* This is the place where we notice someone
-                                * is trying to tinker with the BIOS. We are
-                                * trying to be nice and just ignore it. A more
-                                * resolute answer would be to power down the
-                                * box.
-                                */
-                               printk_debug("Switching back to RO\n");
-                               pcie_write_config32(PCI_DEV(0, 0x1f, 0), 0xdc, (bios_cntl & ~1));
-                       } /* No else for now? */
+       /* IOTRAP(2) currently unused
+        * IOTRAP(1) currently unused */
+
+       /* IOTRAP(0) SMIC */
+       if (IOTRAP(0)) {
+               if (!(trap_cycle & (1 << 24))) { // It's a write
+                       printk(BIOS_DEBUG, "SMI1 command\n");
+                       data = RCBA32(0x1e18);
+                       data &= mask;
+                       // if (smi1)
+                       //      southbridge_smi_command(data);
+                       // return;
                }
+               // Fall through to debug
        }
 
-       if (smi_sts & (1 << 8)) { // PM1
-               pm1_sts = reset_pm1_status();
-               dump_pm1_status(pm1_sts);
-       }
+       printk(BIOS_DEBUG, "  trapped io address = 0x%x\n", trap_cycle & 0xfffc);
+       for (i=0; i < 4; i++) if(IOTRAP(i)) printk(BIOS_DEBUG, "  TRAPĀ = %d\n", i);
+       printk(BIOS_DEBUG, "  AHBE = %x\n", (trap_cycle >> 16) & 0xf);
+       printk(BIOS_DEBUG, "  MASK = 0x%08x\n", mask);
+       printk(BIOS_DEBUG, "  read/write: %s\n", (trap_cycle & (1 << 24)) ? "read" : "write");
 
-       if (smi_sts & (1 << 9)) { // GPE0
-               gpe0_sts = reset_gpe0_status();
-               dump_gpe0_status(gpe0_sts);
+       if (!(trap_cycle & (1 << 24))) {
+               /* Write Cycle */
+               data = RCBA32(0x1e18);
+               printk(BIOS_DEBUG, "  iotrap written data = 0x%08x\n", data);
        }
+#undef IOTRAP
+}
+
+typedef void (*smi_handler_t)(unsigned int node,
+               smm_state_save_area_t *state_save);
+
+smi_handler_t southbridge_smi[32] = {
+       NULL,                     //  [0] reserved
+       NULL,                     //  [1] reserved
+       NULL,                     //  [2] BIOS_STS
+       NULL,                     //  [3] LEGACY_USB_STS
+       southbridge_smi_sleep,    //  [4] SLP_SMI_STS
+       southbridge_smi_apmc,     //  [5] APM_STS
+       NULL,                     //  [6] SWSMI_TMR_STS
+       NULL,                     //  [7] reserved
+       southbridge_smi_pm1,      //  [8] PM1_STS
+       southbridge_smi_gpe0,     //  [9] GPE0_STS
+       southbridge_smi_gpi,      // [10] GPI_STS
+       southbridge_smi_mc,       // [11] MCSMI_STS
+       NULL,                     // [12] DEVMON_STS
+       southbridge_smi_tco,      // [13] TCO_STS
+       southbridge_smi_periodic, // [14] PERIODIC_STS
+       NULL,                     // [15] SERIRQ_SMI_STS
+       NULL,                     // [16] SMBUS_SMI_STS
+       NULL,                     // [17] LEGACY_USB2_STS
+       NULL,                     // [18] INTEL_USB2_STS
+       NULL,                     // [19] reserved
+       NULL,                     // [20] PCI_EXP_SMI_STS
+       southbridge_smi_monitor,  // [21] MONITOR_STS
+       NULL,                     // [22] reserved
+       NULL,                     // [23] reserved
+       NULL,                     // [24] reserved
+       NULL,                     // [25] EL_SMI_STS
+       NULL,                     // [26] SPI_STS
+       NULL,                     // [27] reserved
+       NULL,                     // [28] reserved
+       NULL,                     // [29] reserved
+       NULL,                     // [30] reserved
+       NULL                      // [31] reserved
+};
 
-       if (smi_sts & (1 << 5)) { // APM
-               /* Emulate B2 register as the FADT / Linux expects it */
-
-               reg8 = inb(0xb2);
-               switch (reg8) {
-               case ACPI_DISABLE:
-                       pmctrl = inw(pmbase + 0x04);
-                       pmctrl |= (1 << 0);
-                       outw(pmctrl, pmbase + 0x04);
-                       printk_debug("SMI#: ACPI disabled.\n");
-                       break;
-               case ACPI_ENABLE:
-                       pmctrl = inw(pmbase + 0x04);
-                       pmctrl &= ~(1 << 0);
-                       outw(pmctrl, pmbase + 0x04);
-                       printk_debug("SMI#: ACPI enabled.\n");
-                       break;
+/**
+ * @brief Interrupt handler for SMI#
+ *
+ * @param smm_revision revision of the smm state save map
+ */
+
+void southbridge_smi_handler(unsigned int node, smm_state_save_area_t *state_save)
+{
+       int i, dump = 0;
+       u32 smi_sts;
+
+       /* Update global variable pmbase */
+       pmbase = pcie_read_config16(PCI_DEV(0, 0x1f, 0), 0x40) & 0xfffc;
+
+       /* We need to clear the SMI status registers, or we won't see what's
+        * happening in the following calls.
+        */
+       smi_sts = reset_smi_status();
+
+       /* Filter all non-enabled SMI events */
+       // FIXME Double check, this clears MONITOR
+       // smi_sts &= inl(pmbase + SMI_EN);
+
+       /* Call SMI sub handler for each of the status bits */
+       for (i = 0; i < 31; i++) {
+               if (smi_sts & (1 << i)) {
+                       if (southbridge_smi[i])
+                               southbridge_smi[i](node, state_save);
+                       else {
+                               printk(BIOS_DEBUG, "SMI_STS[%d] occured, but no "
+                                               "handler available.\n", i);
+                               dump = 1;
+                       }
                }
        }
 
-       if (smi_sts & (1 << 4)) { // SLP_SMI
-               u32 reg32;
-               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");
+       if(dump) {
+               dump_smi_status(smi_sts);
        }
 
-
-       smi_release_lock();
-
-       /* De-assert SMI# signal to allow another SMI */
-       smi_set_eos();
 }