/*
 * Copyright (c) 1996, by Steve Passe
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. The name of the developer may NOT be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $FreeBSD: src/sys/i386/i386/mp_machdep.c,v 1.115.2.15 2003/03/14 21:22:35 jhb Exp $
 */

#include "opt_cpu.h"

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/sysctl.h>
#include <sys/malloc.h>
#include <sys/memrange.h>
#include <sys/cons.h>   /* cngetc() */
#include <sys/machintr.h>

#include <vm/vm.h>
#include <vm/vm_param.h>
#include <vm/pmap.h>
#include <vm/vm_kern.h>
#include <vm/vm_extern.h>
#include <sys/lock.h>
#include <vm/vm_map.h>

#include <machine/smp.h>
#include <machine_base/isa/isa_intr.h>
#include <machine_base/apic/apicreg.h>
#include <machine_base/apic/apicvar.h>
#include <machine/atomic.h>
#include <machine/cpufunc.h>
#include <machine/cputypes.h>
#include <machine_base/apic/lapic.h>
#include <machine_base/apic/ioapic.h>
#include <machine/psl.h>
#include <machine/segments.h>
#include <machine/tss.h>
#include <machine/specialreg.h>
#include <machine/globaldata.h>
#include <machine/pmap_inval.h>
#include <machine/mptable.h>

#include <machine/md_var.h>             /* setidt() */
#include <machine_base/icu/icu.h>       /* IPIs */
#include <machine_base/apic/ioapic_abi.h>
#include <machine/intr_machdep.h>       /* IPIs */

extern u_int    base_memory;            /* in kilobytes, see machdep.c */
extern u_long   ebda_addr;
extern int      imcr_present;
extern int      naps;

static int      force_enable = 0;
TUNABLE_INT("hw.lapic_force_enable", &force_enable);

#define BIOS_BASE               (0xf0000)
#define BIOS_BASE2              (0xe0000)
#define BIOS_SIZE               (0x10000)
#define BIOS_COUNT              (BIOS_SIZE/4)

#define PROCENTRY_FLAG_EN       0x01
#define PROCENTRY_FLAG_BP       0x02
#define IOAPICENTRY_FLAG_EN     0x01


/* MP Floating Pointer Structure */
typedef struct MPFPS {
        char    signature[4];
        u_int32_t pap;
        u_char  length;
        u_char  spec_rev;
        u_char  checksum;
        u_char  mpfb1;
        u_char  mpfb2;
        u_char  mpfb3;
        u_char  mpfb4;
        u_char  mpfb5;
}      *mpfps_t;

/* MP Configuration Table Header */
typedef struct MPCTH {
        char    signature[4];
        u_short base_table_length;
        u_char  spec_rev;
        u_char  checksum;
        u_char  oem_id[8];
        u_char  product_id[12];
        u_int32_t oem_table_pointer;
        u_short oem_table_size;
        u_short entry_count;
        u_int32_t apic_address;
        u_short extended_table_length;
        u_char  extended_table_checksum;
        u_char  reserved;
}      *mpcth_t;


typedef struct PROCENTRY {
        u_char  type;
        u_char  apic_id;
        u_char  apic_version;
        u_char  cpu_flags;
        u_int32_t cpu_signature;
        u_int32_t feature_flags;
        u_int32_t reserved1;
        u_int32_t reserved2;
}      *proc_entry_ptr;

typedef struct BUSENTRY {
        u_char  type;
        u_char  bus_id;
        char    bus_type[6];
}      *bus_entry_ptr;

typedef struct IOAPICENTRY {
        u_char  type;
        u_char  apic_id;
        u_char  apic_version;
        u_char  apic_flags;
        u_int32_t apic_address;
}      *io_apic_entry_ptr;

typedef struct INTENTRY {
        u_char  type;
        u_char  int_type;
        u_short int_flags;
        u_char  src_bus_id;
        u_char  src_bus_irq;
        u_char  dst_apic_id;
        u_char  dst_apic_int;
}      *int_entry_ptr;

/* descriptions of MP basetable entries */
typedef struct BASETABLE_ENTRY {
        u_char  type;
        u_char  length;
        char    name[16];
}       basetable_entry;

struct mptable_pos {
        mpfps_t         mp_fps;
        mpcth_t         mp_cth;
        vm_size_t       mp_cth_mapsz;   
};

#define MPTABLE_POS_USE_DEFAULT(mpt) \
        ((mpt)->mp_fps->mpfb1 != 0 || (mpt)->mp_cth == NULL)

struct mptable_bus {
        int             mb_id;
        int             mb_type;        /* MPTABLE_BUS_ */
        TAILQ_ENTRY(mptable_bus) mb_link;
};

#define MPTABLE_BUS_ISA         0
#define MPTABLE_BUS_PCI         1

struct mptable_bus_info {
        TAILQ_HEAD(, mptable_bus) mbi_list;
};

struct mptable_pci_int {
        int             mpci_bus;
        int             mpci_dev;
        int             mpci_pin;

        int             mpci_ioapic_idx;
        int             mpci_ioapic_pin;
        TAILQ_ENTRY(mptable_pci_int) mpci_link;
};

struct mptable_ioapic {
        int             mio_idx;
        int             mio_apic_id;
        uint32_t        mio_addr;
        int             mio_gsi_base;
        int             mio_npin;
        TAILQ_ENTRY(mptable_ioapic) mio_link;
};

typedef int     (*mptable_iter_func)(void *, const void *, int);

static int      mptable_iterate_entries(const mpcth_t,
                    mptable_iter_func, void *);
static int      mptable_search(void);
static long     mptable_search_sig(u_int32_t target, int count);
static int      mptable_hyperthread_fixup(cpumask_t, int);
static int      mptable_map(struct mptable_pos *);
static void     mptable_unmap(struct mptable_pos *);
static void     mptable_bus_info_alloc(const mpcth_t,
                    struct mptable_bus_info *);
static void     mptable_bus_info_free(struct mptable_bus_info *);

static int      mptable_lapic_probe(struct lapic_enumerator *);
static int      mptable_lapic_enumerate(struct lapic_enumerator *);
static void     mptable_lapic_default(void);

static int      mptable_ioapic_probe(struct ioapic_enumerator *);
static void     mptable_ioapic_enumerate(struct ioapic_enumerator *);

static basetable_entry basetable_entry_types[] =
{
        {0, 20, "Processor"},
        {1, 8, "Bus"},
        {2, 8, "I/O APIC"},
        {3, 8, "I/O INT"},
        {4, 8, "Local INT"}
};

static vm_paddr_t       mptable_fps_phyaddr;
static int              mptable_use_default;
static TAILQ_HEAD(mptable_pci_int_list, mptable_pci_int) mptable_pci_int_list =
        TAILQ_HEAD_INITIALIZER(mptable_pci_int_list);
static TAILQ_HEAD(mptable_ioapic_list, mptable_ioapic) mptable_ioapic_list =
        TAILQ_HEAD_INITIALIZER(mptable_ioapic_list);

static void
mptable_probe(void)
{
        struct mptable_pos mpt;
        int error;

        KKASSERT(mptable_fps_phyaddr == 0);

        mptable_fps_phyaddr = mptable_search();
        if (mptable_fps_phyaddr == 0)
                return;

        error = mptable_map(&mpt);
        if (error) {
                mptable_fps_phyaddr = 0;
                return;
        }

        if (MPTABLE_POS_USE_DEFAULT(&mpt)) {
                kprintf("MPTABLE: use default configuration\n");
                mptable_use_default = 1;
        }
        if (mpt.mp_fps->mpfb2 & 0x80)
                imcr_present = 1;

        mptable_unmap(&mpt);
}
SYSINIT(mptable_probe, SI_BOOT2_PRESMP, SI_ORDER_FIRST, mptable_probe, 0);

/*
 * Look for an Intel MP spec table (ie, SMP capable hardware).
 */
static int
mptable_search(void)
{
        long    x;
        u_int32_t target;
 
        /* see if EBDA exists */
        if (ebda_addr != 0) {
                /* search first 1K of EBDA */
                target = (u_int32_t)ebda_addr;
                if ((x = mptable_search_sig(target, 1024 / 4)) > 0)
                        return x;
        } else {
                /* last 1K of base memory, effective 'top of base' passed in */
                target = (u_int32_t)((base_memory - 1) * 1024);
                if ((x = mptable_search_sig(target, 1024 / 4)) > 0)
                        return x;
        }

        /* search the BIOS */
        target = (u_int32_t)BIOS_BASE;
        if ((x = mptable_search_sig(target, BIOS_COUNT)) > 0)
                return x;

        /* search the extended BIOS */
        target = (u_int32_t)BIOS_BASE2;
        if ((x = mptable_search_sig(target, BIOS_COUNT)) > 0)
                return x;

        /* nothing found */
        return 0;
}

static int
mptable_iterate_entries(const mpcth_t cth, mptable_iter_func func, void *arg)
{
        int count, total_size;
        const void *position;

        KKASSERT(cth->base_table_length >= sizeof(struct MPCTH));
        total_size = cth->base_table_length - sizeof(struct MPCTH);
        position = (const uint8_t *)cth + sizeof(struct MPCTH);
        count = cth->entry_count;

        while (count--) {
                int type, error;

                KKASSERT(total_size >= 0);
                if (total_size == 0) {
                        kprintf("invalid base MP table, "
                                "entry count and length mismatch\n");
                        return EINVAL;
                }

                type = *(const uint8_t *)position;
                switch (type) {
                case 0: /* processor_entry */
                case 1: /* bus_entry */
                case 2: /* io_apic_entry */
                case 3: /* int_entry */
                case 4: /* int_entry */
                        break;
                default:
                        kprintf("unknown base MP table entry type %d\n", type);
                        return EINVAL;
                }

                if (total_size < basetable_entry_types[type].length) {
                        kprintf("invalid base MP table length, "
                                "does not contain all entries\n");
                        return EINVAL;
                }
                total_size -= basetable_entry_types[type].length;

                error = func(arg, position, type);
                if (error)
                        return error;

                position = (const uint8_t *)position +
                    basetable_entry_types[type].length;
        }
        return 0;
}

/*
 * look for the MP spec signature
 */

/* string defined by the Intel MP Spec as identifying the MP table */
#define MP_SIG          0x5f504d5f      /* _MP_ */
#define NEXT(X)         ((X) += 4)
static long
mptable_search_sig(u_int32_t target, int count)
{
        vm_size_t map_size;
        u_int32_t *addr;
        int x, ret;

        KKASSERT(target != 0);

        map_size = count * sizeof(u_int32_t);
        addr = pmap_mapdev((vm_paddr_t)target, map_size);

        ret = 0;
        for (x = 0; x < count; NEXT(x)) {
                if (addr[x] == MP_SIG) {
                        /* make array index a byte index */
                        ret = target + (x * sizeof(u_int32_t));
                        break;
                }
        }

        pmap_unmapdev((vm_offset_t)addr, map_size);
        return ret;
}

static int processor_entry      (const struct PROCENTRY *entry, int cpu);

/*
 * Check if we should perform a hyperthreading "fix-up" to
 * enumerate any logical CPU's that aren't already listed
 * in the table.
 *
 * XXX: We assume that all of the physical CPUs in the
 * system have the same number of logical CPUs.
 *
 * XXX: We assume that APIC ID's are allocated such that
 * the APIC ID's for a physical processor are aligned
 * with the number of logical CPU's in the processor.
 */
static int
mptable_hyperthread_fixup(cpumask_t id_mask, int cpu_count)
{
        int i, id, lcpus_max, logical_cpus;

        if ((cpu_feature & CPUID_HTT) == 0)
                return 0;

        lcpus_max = (cpu_procinfo & CPUID_HTT_CORES) >> 16;
        if (lcpus_max <= 1)
                return 0;

        if (cpu_vendor_id == CPU_VENDOR_INTEL) {
                /*
                 * INSTRUCTION SET REFERENCE, A-M (#253666)
                 * Page 3-181, Table 3-20
                 * "The nearest power-of-2 integer that is not smaller
                 *  than EBX[23:16] is the number of unique initial APIC
                 *  IDs reserved for addressing different logical
                 *  processors in a physical package."
                 */
                for (i = 0; ; ++i) {
                        if ((1 << i) >= lcpus_max) {
                                lcpus_max = 1 << i;
                                break;
                        }
                }
        }

        KKASSERT(cpu_count != 0);
        if (cpu_count == lcpus_max) {
                /* We have nothing to fix */
                return 0;
        } else if (cpu_count == 1) {
                /* XXX this may be incorrect */
                logical_cpus = lcpus_max;
        } else {
                int cur, prev, dist;

                /*
                 * Calculate the distances between two nearest
                 * APIC IDs.  If all such distances are same,
                 * then it is the number of missing cpus that
                 * we are going to fill later.
                 */
                dist = cur = prev = -1;
                for (id = 0; id < MAXCPU; ++id) {
                        if (CPUMASK_TESTBIT(id_mask, id) == 0)
                                continue;

                        cur = id;
                        if (prev >= 0) {
                                int new_dist = cur - prev;

                                if (dist < 0)
                                        dist = new_dist;

                                /*
                                 * Make sure that all distances
                                 * between two nearest APIC IDs
                                 * are same.
                                 */
                                if (dist != new_dist)
                                        return 0;
                        }
                        prev = cur;
                }
                if (dist == 1)
                        return 0;

                /* Must be power of 2 */
                if (dist & (dist - 1))
                        return 0;

                /* Can't exceed CPU package capacity */
                if (dist > lcpus_max)
                        logical_cpus = lcpus_max;
                else
                        logical_cpus = dist;
        }

        /*
         * For each APIC ID of a CPU that is set in the mask,
         * scan the other candidate APIC ID's for this
         * physical processor.  If any of those ID's are
         * already in the table, then kill the fixup.
         */
        for (id = 0; id < MAXCPU; id++) {
                if (CPUMASK_TESTBIT(id_mask, id) == 0)
                        continue;
                /* First, make sure we are on a logical_cpus boundary. */
                if (id % logical_cpus != 0)
                        return 0;
                for (i = id + 1; i < id + logical_cpus; i++)
                        if (CPUMASK_TESTBIT(id_mask, i) != 0)
                                return 0;
        }
        return logical_cpus;
}

static int
mptable_map(struct mptable_pos *mpt)
{
        mpfps_t fps = NULL;
        mpcth_t cth = NULL;
        vm_size_t cth_mapsz = 0;

        KKASSERT(mptable_fps_phyaddr != 0);

        bzero(mpt, sizeof(*mpt));

        fps = pmap_mapdev(mptable_fps_phyaddr, sizeof(*fps));
        if (fps->pap != 0) {
                /*
                 * Map configuration table header to get
                 * the base table size
                 */
                cth = pmap_mapdev(fps->pap, sizeof(*cth));
                cth_mapsz = cth->base_table_length;
                pmap_unmapdev((vm_offset_t)cth, sizeof(*cth));

                if (cth_mapsz < sizeof(*cth)) {
                        kprintf("invalid base MP table length %d\n",
                                (int)cth_mapsz);
                        pmap_unmapdev((vm_offset_t)fps, sizeof(*fps));
                        return EINVAL;
                }

                /*
                 * Map the base table
                 */
                cth = pmap_mapdev(fps->pap, cth_mapsz);
        }

        mpt->mp_fps = fps;
        mpt->mp_cth = cth;
        mpt->mp_cth_mapsz = cth_mapsz;

        return 0;
}

static void
mptable_unmap(struct mptable_pos *mpt)
{
        if (mpt->mp_cth != NULL) {
                pmap_unmapdev((vm_offset_t)mpt->mp_cth, mpt->mp_cth_mapsz);
                mpt->mp_cth = NULL;
                mpt->mp_cth_mapsz = 0;
        }
        if (mpt->mp_fps != NULL) {
                pmap_unmapdev((vm_offset_t)mpt->mp_fps, sizeof(*mpt->mp_fps));
                mpt->mp_fps = NULL;
        }
}

static int
processor_entry(const struct PROCENTRY *entry, int cpu)
{
        KKASSERT(cpu > 0);

        /* check for usability */
        if (!(entry->cpu_flags & PROCENTRY_FLAG_EN))
                return 0;

        /* check for BSP flag */
        if (entry->cpu_flags & PROCENTRY_FLAG_BP) {
                lapic_set_cpuid(0, entry->apic_id);
                return 0;       /* its already been counted */
        }

        /* add another AP to list, if less than max number of CPUs */
        else if (cpu < MAXCPU) {
                lapic_set_cpuid(cpu, entry->apic_id);
                return 1;
        }

        return 0;
}

static int
mptable_bus_info_callback(void *xarg, const void *pos, int type)
{
        struct mptable_bus_info *bus_info = xarg;
        const struct BUSENTRY *ent;
        struct mptable_bus *bus;

        if (type != 1)
                return 0;

        ent = pos;
        TAILQ_FOREACH(bus, &bus_info->mbi_list, mb_link) {
                if (bus->mb_id == ent->bus_id) {
                        kprintf("mptable_bus_info_alloc: duplicated bus id "
                                "(%d)\n", bus->mb_id);
                        return EINVAL;
                }
        }

        bus = NULL;
        if (strncmp(ent->bus_type, "PCI", 3) == 0) {
                bus = kmalloc(sizeof(*bus), M_TEMP, M_WAITOK | M_ZERO);
                bus->mb_type = MPTABLE_BUS_PCI;
        } else if (strncmp(ent->bus_type, "ISA", 3) == 0) {
                bus = kmalloc(sizeof(*bus), M_TEMP, M_WAITOK | M_ZERO);
                bus->mb_type = MPTABLE_BUS_ISA;
        }

        if (bus != NULL) {
                bus->mb_id = ent->bus_id;
                TAILQ_INSERT_TAIL(&bus_info->mbi_list, bus, mb_link);
        }
        return 0;
}

static void
mptable_bus_info_alloc(const mpcth_t cth, struct mptable_bus_info *bus_info)
{
        int error;

        bzero(bus_info, sizeof(*bus_info));
        TAILQ_INIT(&bus_info->mbi_list);

        error = mptable_iterate_entries(cth, mptable_bus_info_callback, bus_info);
        if (error)
                mptable_bus_info_free(bus_info);
}

static void
mptable_bus_info_free(struct mptable_bus_info *bus_info)
{
        struct mptable_bus *bus;

        while ((bus = TAILQ_FIRST(&bus_info->mbi_list)) != NULL) {
                TAILQ_REMOVE(&bus_info->mbi_list, bus, mb_link);
                kfree(bus, M_TEMP);
        }
}

struct mptable_lapic_cbarg1 {
        int     cpu_count;
        int     ht_fixup;
        u_int   ht_apicid_mask;
};

static int
mptable_lapic_pass1_callback(void *xarg, const void *pos, int type)
{
        const struct PROCENTRY *ent;
        struct mptable_lapic_cbarg1 *arg = xarg;

        if (type != 0)
                return 0;
        ent = pos;

        if ((ent->cpu_flags & PROCENTRY_FLAG_EN) == 0)
                return 0;

        arg->cpu_count++;
        if (ent->apic_id < 64) {
                arg->ht_apicid_mask |= 1UL << ent->apic_id;
        } else if (arg->ht_fixup) {
                kprintf("MPTABLE: lapic id > 64, disable HTT fixup\n");
                arg->ht_fixup = 0;
        }
        return 0;
}

struct mptable_lapic_cbarg2 {
        int     cpu;
        int     logical_cpus;
        int     found_bsp;
};

static int
mptable_lapic_pass2_callback(void *xarg, const void *pos, int type)
{
        const struct PROCENTRY *ent;
        struct mptable_lapic_cbarg2 *arg = xarg;

        if (type != 0)
                return 0;
        ent = pos;

        if (ent->cpu_flags & PROCENTRY_FLAG_BP) {
                KKASSERT(!arg->found_bsp);
                arg->found_bsp = 1;
        }

        if (processor_entry(ent, arg->cpu))
                arg->cpu++;

        if (arg->logical_cpus) {
                struct PROCENTRY proc;
                int i;

                /*
                 * Create fake mptable processor entries
                 * and feed them to processor_entry() to
                 * enumerate the logical CPUs.
                 */
                bzero(&proc, sizeof(proc));
                proc.type = 0;
                proc.cpu_flags = (force_enable) ? PROCENTRY_FLAG_EN : ent->cpu_flags;
                proc.apic_id = ent->apic_id;

                for (i = 1; i < arg->logical_cpus; i++) {
                        proc.apic_id++;
                        processor_entry(&proc, arg->cpu);
                        arg->cpu++;
                }
        }
        return 0;
}

static void
mptable_lapic_default(void)
{
        int ap_apicid, bsp_apicid;

        naps = 1; /* exclude BSP */

        /* Map local apic before the id field is accessed */
        lapic_map(DEFAULT_APIC_BASE);

        bsp_apicid = LAPIC_READID;
        ap_apicid = (bsp_apicid == 0) ? 1 : 0;

        /* BSP */
        lapic_set_cpuid(0, bsp_apicid);
        /* one and only AP */
        lapic_set_cpuid(1, ap_apicid);
}

/*
 * Configure:
 *     naps
 *     APIC ID <-> CPU ID mappings
 */
static int
mptable_lapic_enumerate(struct lapic_enumerator *e)
{
        struct mptable_pos mpt;
        struct mptable_lapic_cbarg1 arg1;
        struct mptable_lapic_cbarg2 arg2;
        mpcth_t cth;
        int error, logical_cpus = 0;
        vm_paddr_t lapic_addr;

        if (mptable_use_default) {
                mptable_lapic_default();
                return 0;
        }
 
        error = mptable_map(&mpt);
        if (error)
                panic("mptable_lapic_enumerate mptable_map failed");
        KKASSERT(!MPTABLE_POS_USE_DEFAULT(&mpt));

        cth = mpt.mp_cth;
 
        /* Save local apic address */
        lapic_addr = cth->apic_address;
        KKASSERT(lapic_addr != 0);
 
        /*
         * Find out how many CPUs do we have
         */
        bzero(&arg1, sizeof(arg1));
        arg1.ht_fixup = 1; /* Apply ht fixup by default */

        error = mptable_iterate_entries(cth,
                    mptable_lapic_pass1_callback, &arg1);
        if (error)
                panic("mptable_iterate_entries(lapic_pass1) failed");
        KKASSERT(arg1.cpu_count != 0);
 
        /* See if we need to fixup HT logical CPUs. */
        /*
         * XXX fixup for cpus >= 32 ? XXX
         */
        if (arg1.ht_fixup) {
                cpumask_t mask;

                CPUMASK_ASSZERO(mask);
                mask.ary[0] = arg1.ht_apicid_mask;
                logical_cpus = mptable_hyperthread_fixup(mask, arg1.cpu_count);
                if (logical_cpus != 0)
                        arg1.cpu_count *= logical_cpus;
        }
        naps = arg1.cpu_count - 1;      /* subtract the BSP */
 
        /*
         * Link logical CPU id to local apic id
         */
        bzero(&arg2, sizeof(arg2));
        arg2.cpu = 1;
        arg2.logical_cpus = logical_cpus;

        error = mptable_iterate_entries(cth,
                    mptable_lapic_pass2_callback, &arg2);
        if (error)
                panic("mptable_iterate_entries(lapic_pass2) failed");
        KKASSERT(arg2.found_bsp);

        /* Map local apic */
        lapic_map(lapic_addr);

        mptable_unmap(&mpt);

        return 0;
}

struct mptable_lapic_probe_cbarg {
        int     cpu_count;
        int     found_bsp;
};

static int
mptable_lapic_probe_callback(void *xarg, const void *pos, int type)
{
        const struct PROCENTRY *ent;
        struct mptable_lapic_probe_cbarg *arg = xarg;

        if (type != 0)
                return 0;
        ent = pos;

        if ((ent->cpu_flags & PROCENTRY_FLAG_EN) == 0)
                return 0;
        arg->cpu_count++;

        if (ent->apic_id == APICID_MAX) {
                kprintf("MPTABLE: invalid LAPIC apic id %d\n",
                    ent->apic_id);
                return EINVAL;
        }

        if (ent->cpu_flags & PROCENTRY_FLAG_BP) {
                if (arg->found_bsp) {
                        kprintf("more than one BSP in base MP table\n");
                        return EINVAL;
                }
                arg->found_bsp = 1;
        }
        return 0;
}

static int
mptable_lapic_probe(struct lapic_enumerator *e)
{
        struct mptable_pos mpt;
        struct mptable_lapic_probe_cbarg arg;
        mpcth_t cth;
        int error;

        if (mptable_fps_phyaddr == 0)
                return ENXIO;

        if (mptable_use_default)
                return 0;

        error = mptable_map(&mpt);
        if (error)
                return error;
        KKASSERT(!MPTABLE_POS_USE_DEFAULT(&mpt));

        error = EINVAL;
        cth = mpt.mp_cth;

        if (cth->apic_address == 0)
                goto done;

        bzero(&arg, sizeof(arg));
        error = mptable_iterate_entries(cth,
                    mptable_lapic_probe_callback, &arg);
        if (!error) {
                if (arg.cpu_count == 0) {
                        kprintf("MP table contains no processor entries\n");
                        error = EINVAL;
                } else if (!arg.found_bsp) {
                        kprintf("MP table does not contains BSP entry\n");
                        error = EINVAL;
                }
        }
done:
        mptable_unmap(&mpt);
        return error;
}

static struct lapic_enumerator  mptable_lapic_enumerator = {
        .lapic_prio = LAPIC_ENUM_PRIO_MPTABLE,
        .lapic_probe = mptable_lapic_probe,
        .lapic_enumerate = mptable_lapic_enumerate
};

static void
mptable_lapic_enum_register(void)
{
        lapic_enumerator_register(&mptable_lapic_enumerator);
}
SYSINIT(mptable_lapic, SI_BOOT2_PRESMP, SI_ORDER_ANY,
        mptable_lapic_enum_register, 0);

static int
mptable_ioapic_list_callback(void *xarg, const void *pos, int type)
{
        const struct IOAPICENTRY *ent;
        struct mptable_ioapic *nioapic, *ioapic;

        if (type != 2)
                return 0;
        ent = pos;

        if ((ent->apic_flags & IOAPICENTRY_FLAG_EN) == 0)
                return 0;

        if (ent->apic_address == 0) {
                kprintf("mptable_ioapic_create_list: zero IOAPIC addr\n");
                return EINVAL;
        }
        if (ent->apic_id == APICID_MAX) {
                kprintf("mptable_ioapic_create_list: "
                    "invalid IOAPIC apic id %d\n", ent->apic_id);
                return EINVAL;
        }

        TAILQ_FOREACH(ioapic, &mptable_ioapic_list, mio_link) {
                if (ioapic->mio_apic_id == ent->apic_id) {
                        kprintf("mptable_ioapic_create_list: duplicated "
                                "apic id %d\n", ioapic->mio_apic_id);
                        return EINVAL;
                }
                if (ioapic->mio_addr == ent->apic_address) {
                        kprintf("mptable_ioapic_create_list: overlapped "
                                "IOAPIC addr 0x%08x", ioapic->mio_addr);
                        return EINVAL;
                }
        }

        nioapic = kmalloc(sizeof(*nioapic), M_DEVBUF, M_WAITOK | M_ZERO);
        nioapic->mio_apic_id = ent->apic_id;
        nioapic->mio_addr = ent->apic_address;

        /*
         * Create IOAPIC list in ascending order of APIC ID
         */
        TAILQ_FOREACH_REVERSE(ioapic, &mptable_ioapic_list,
            mptable_ioapic_list, mio_link) {
                if (nioapic->mio_apic_id > ioapic->mio_apic_id) {
                        TAILQ_INSERT_AFTER(&mptable_ioapic_list,
                            ioapic, nioapic, mio_link);
                        break;
                }
        }
        if (ioapic == NULL)
                TAILQ_INSERT_HEAD(&mptable_ioapic_list, nioapic, mio_link);

        return 0;
}

static void
mptable_ioapic_create_list(void)
{
        struct mptable_ioapic *ioapic;
        struct mptable_pos mpt;
        int idx, error;

        if (mptable_fps_phyaddr == 0)
                return;

        if (mptable_use_default) {
                ioapic = kmalloc(sizeof(*ioapic), M_DEVBUF, M_WAITOK | M_ZERO);
                ioapic->mio_idx = 0;
                ioapic->mio_apic_id = 0;        /* NOTE: any value is ok here */
                ioapic->mio_addr = 0xfec00000;  /* XXX magic number */

                TAILQ_INSERT_HEAD(&mptable_ioapic_list, ioapic, mio_link);
                return;
        }

        error = mptable_map(&mpt);
        if (error)
                panic("mptable_ioapic_create_list: mptable_map failed");
        KKASSERT(!MPTABLE_POS_USE_DEFAULT(&mpt));

        error = mptable_iterate_entries(mpt.mp_cth,
                    mptable_ioapic_list_callback, NULL);
        if (error) {
                while ((ioapic = TAILQ_FIRST(&mptable_ioapic_list)) != NULL) {
                        TAILQ_REMOVE(&mptable_ioapic_list, ioapic, mio_link);
                        kfree(ioapic, M_DEVBUF);
                }
                goto done;
        }

        /*
         * Assign index number for each IOAPIC
         */
        idx = 0;
        TAILQ_FOREACH(ioapic, &mptable_ioapic_list, mio_link) {
                ioapic->mio_idx = idx;
                ++idx;
        }
done:
        mptable_unmap(&mpt);
}
SYSINIT(mptable_ioapic_list, SI_BOOT2_PRESMP, SI_ORDER_SECOND,
        mptable_ioapic_create_list, 0);

static int
mptable_pci_int_callback(void *xarg, const void *pos, int type)
{
        const struct mptable_bus_info *bus_info = xarg;
        const struct mptable_ioapic *ioapic;
        const struct mptable_bus *bus;
        struct mptable_pci_int *pci_int;
        const struct INTENTRY *ent;
        int pci_pin, pci_dev;

        if (type != 3)
                return 0;
        ent = pos;

        if (ent->int_type != 0)
                return 0;

        TAILQ_FOREACH(bus, &bus_info->mbi_list, mb_link) {
                if (bus->mb_type == MPTABLE_BUS_PCI &&
                    bus->mb_id == ent->src_bus_id)
                        break;
        }
        if (bus == NULL)
                return 0;

        TAILQ_FOREACH(ioapic, &mptable_ioapic_list, mio_link) {
                if (ioapic->mio_apic_id == ent->dst_apic_id)
                        break;
        }
        if (ioapic == NULL) {
                if (bootverbose) {
                        static char intdis_warned[64];
                        int apic_id = ent->dst_apic_id;
                        int warn = 0;

                        if (apic_id < 0 || apic_id >= sizeof(intdis_warned)) {
                                warn = 1;
                        } else if (intdis_warned[apic_id] == 0) {
                                intdis_warned[apic_id] = 1;
                                warn = 1;
                        }
                        if (warn) {
                                kprintf("MPTABLE: warning PCI int dst apic id "
                                    "%d does not exist\n", apic_id);
                        }
                }
                return 0;
        }

        pci_pin = ent->src_bus_irq & 0x3;
        pci_dev = (ent->src_bus_irq >> 2) & 0x1f;

        TAILQ_FOREACH(pci_int, &mptable_pci_int_list, mpci_link) {
                if (pci_int->mpci_bus == ent->src_bus_id &&
                    pci_int->mpci_dev == pci_dev &&
                    pci_int->mpci_pin == pci_pin) {
                        if (pci_int->mpci_ioapic_idx == ioapic->mio_idx &&
                            pci_int->mpci_ioapic_pin == ent->dst_apic_int) {
                                kprintf("MPTABLE: warning duplicated "
                                        "PCI int entry for "
                                        "bus %d, dev %d, pin %d\n",
                                        pci_int->mpci_bus,
                                        pci_int->mpci_dev,
                                        pci_int->mpci_pin);
                                return 0;
                        } else {
                                kprintf("mptable_pci_int_register: "
                                        "conflict PCI int entry for "
                                        "bus %d, dev %d, pin %d, "
                                        "IOAPIC %d.%d -> %d.%d\n",
                                        pci_int->mpci_bus,
                                        pci_int->mpci_dev,
                                        pci_int->mpci_pin,
                                        pci_int->mpci_ioapic_idx,
                                        pci_int->mpci_ioapic_pin,
                                        ioapic->mio_idx,
                                        ent->dst_apic_int);
                                return EINVAL;
                        }
                }
        }

        pci_int = kmalloc(sizeof(*pci_int), M_DEVBUF, M_WAITOK | M_ZERO);

        pci_int->mpci_bus = ent->src_bus_id;
        pci_int->mpci_dev = pci_dev;
        pci_int->mpci_pin = pci_pin;
        pci_int->mpci_ioapic_idx = ioapic->mio_idx;
        pci_int->mpci_ioapic_pin = ent->dst_apic_int;

        TAILQ_INSERT_TAIL(&mptable_pci_int_list, pci_int, mpci_link);

        return 0;
}

static void
mptable_pci_int_register(void)
{
        struct mptable_bus_info bus_info;
        const struct mptable_bus *bus;
        struct mptable_pci_int *pci_int;
        struct mptable_pos mpt;
        int error, force_pci0, npcibus;
        mpcth_t cth;

        if (mptable_fps_phyaddr == 0)
                return;

        if (mptable_use_default)
                return;

        if (TAILQ_EMPTY(&mptable_ioapic_list))
                return;

        error = mptable_map(&mpt);
        if (error)
                panic("mptable_pci_int_register: mptable_map failed");
        KKASSERT(!MPTABLE_POS_USE_DEFAULT(&mpt));

        cth = mpt.mp_cth;

        mptable_bus_info_alloc(cth, &bus_info);
        if (TAILQ_EMPTY(&bus_info.mbi_list))
                goto done;

        force_pci0 = 0;
        npcibus = 0;
        TAILQ_FOREACH(bus, &bus_info.mbi_list, mb_link) {
                if (bus->mb_type == MPTABLE_BUS_PCI)
                        ++npcibus;
        }
        if (npcibus == 0) {
                mptable_bus_info_free(&bus_info);
                goto done;
        } else if (npcibus == 1) {
                force_pci0 = 1;
        }

        error = mptable_iterate_entries(cth,
                    mptable_pci_int_callback, &bus_info);

        mptable_bus_info_free(&bus_info);

        if (error) {
                while ((pci_int = TAILQ_FIRST(&mptable_pci_int_list)) != NULL) {
                        TAILQ_REMOVE(&mptable_pci_int_list, pci_int, mpci_link);
                        kfree(pci_int, M_DEVBUF);
                }
                goto done;
        }

        if (force_pci0) {
                TAILQ_FOREACH(pci_int, &mptable_pci_int_list, mpci_link)
                        pci_int->mpci_bus = 0;
        }
done:
        mptable_unmap(&mpt);
}
SYSINIT(mptable_pci, SI_BOOT2_PRESMP, SI_ORDER_ANY,
        mptable_pci_int_register, 0);

struct mptable_ioapic_probe_cbarg {
        const struct mptable_bus_info *bus_info;
};

static int
mptable_ioapic_probe_callback(void *xarg, const void *pos, int type)
{
        struct mptable_ioapic_probe_cbarg *arg = xarg;
        const struct mptable_ioapic *ioapic;
        const struct mptable_bus *bus;
        const struct INTENTRY *ent;

        if (type != 3)
                return 0;
        ent = pos;

        if (ent->int_type != 0)
                return 0;

        TAILQ_FOREACH(bus, &arg->bus_info->mbi_list, mb_link) {
                if (bus->mb_type == MPTABLE_BUS_ISA &&
                    bus->mb_id == ent->src_bus_id)
                        break;
        }
        if (bus == NULL)
                return 0;

        TAILQ_FOREACH(ioapic, &mptable_ioapic_list, mio_link) {
                if (ioapic->mio_apic_id == ent->dst_apic_id)
                        break;
        }
        if (ioapic == NULL) {
                kprintf("MPTABLE: warning ISA int dst apic id %d "
                        "does not exist\n", ent->dst_apic_id);
                return 0;
        }

        /* XXX magic number */
        if (ent->src_bus_irq >= ISA_IRQ_CNT) {
                kprintf("mptable_ioapic_probe: invalid ISA irq (%d)\n",
                        ent->src_bus_irq);
                return EINVAL;
        }
        return 0;
}

static int
mptable_ioapic_probe(struct ioapic_enumerator *e)
{
        struct mptable_ioapic_probe_cbarg arg;
        struct mptable_bus_info bus_info;
        struct mptable_pos mpt;
        mpcth_t cth;
        int error;

        if (mptable_fps_phyaddr == 0)
                return ENXIO;

        if (mptable_use_default)
                return 0;

        if (TAILQ_EMPTY(&mptable_ioapic_list))
                return ENXIO;

        error = mptable_map(&mpt);
        if (error)
                panic("mptable_ioapic_probe: mptable_map failed");
        KKASSERT(!MPTABLE_POS_USE_DEFAULT(&mpt));

        cth = mpt.mp_cth;

        mptable_bus_info_alloc(cth, &bus_info);

        bzero(&arg, sizeof(arg));
        arg.bus_info = &bus_info;

        error = mptable_iterate_entries(cth,
                    mptable_ioapic_probe_callback, &arg);

        mptable_bus_info_free(&bus_info);
        mptable_unmap(&mpt);

        return error;
}

struct mptable_ioapic_int_cbarg {
        const struct mptable_bus_info *bus_info;
        int     ioapic_nint;
};

static int
mptable_ioapic_int_callback(void *xarg, const void *pos, int type)
{
        struct mptable_ioapic_int_cbarg *arg = xarg;
        const struct mptable_ioapic *ioapic;
        const struct mptable_bus *bus;
        const struct INTENTRY *ent;
        int gsi;

        if (type != 3)
                return 0;

        arg->ioapic_nint++;

        ent = pos;
        if (ent->int_type != 0)
                return 0;

        TAILQ_FOREACH(bus, &arg->bus_info->mbi_list, mb_link) {
                if (bus->mb_type == MPTABLE_BUS_ISA &&
                    bus->mb_id == ent->src_bus_id)
                        break;
        }
        if (bus == NULL)
                return 0;

        TAILQ_FOREACH(ioapic, &mptable_ioapic_list, mio_link) {
                if (ioapic->mio_apic_id == ent->dst_apic_id)
                        break;
        }
        if (ioapic == NULL) {
                kprintf("MPTABLE: warning ISA int dst apic id %d "
                        "does not exist\n", ent->dst_apic_id);
                return 0;
        }

        if (ent->dst_apic_int >= ioapic->mio_npin) {
                panic("mptable_ioapic_enumerate: invalid I/O APIC "
                      "pin %d, should be < %d",
                      ent->dst_apic_int, ioapic->mio_npin);
        }
        gsi = ioapic->mio_gsi_base + ent->dst_apic_int;

        if (ent->src_bus_irq != gsi) {
                if (bootverbose) {
                        kprintf("MPTABLE: INTSRC irq %d -> GSI %d\n",
                                ent->src_bus_irq, gsi);
                }
                ioapic_intsrc(ent->src_bus_irq, gsi,
                    INTR_TRIGGER_EDGE, INTR_POLARITY_HIGH);
        }
        return 0;
}

static void
mptable_ioapic_enumerate(struct ioapic_enumerator *e)
{
        struct mptable_bus_info bus_info;
        struct mptable_ioapic *ioapic;
        struct mptable_pos mpt;
        mpcth_t cth;
        int error;

        KKASSERT(mptable_fps_phyaddr != 0);
        KKASSERT(!TAILQ_EMPTY(&mptable_ioapic_list));

        TAILQ_FOREACH(ioapic, &mptable_ioapic_list, mio_link) {
                const struct mptable_ioapic *prev_ioapic;
                uint32_t ver;
                void *addr;

                addr = ioapic_map(ioapic->mio_addr);

                ver = ioapic_read(addr, IOAPIC_VER);
                ioapic->mio_npin = ((ver & IOART_VER_MAXREDIR)
                                    >> MAXREDIRSHIFT) + 1;

                prev_ioapic = TAILQ_PREV(ioapic,
                                mptable_ioapic_list, mio_link);
                if (prev_ioapic == NULL) {
                        ioapic->mio_gsi_base = 0;
                } else {
                        ioapic->mio_gsi_base =
                                prev_ioapic->mio_gsi_base +
                                prev_ioapic->mio_npin;
                }
                ioapic_add(addr, ioapic->mio_gsi_base, ioapic->mio_npin);

                if (bootverbose) {
                        kprintf("MPTABLE: IOAPIC addr 0x%08x, "
                                "apic id %d, idx %d, gsi base %d, npin %d\n",
                                ioapic->mio_addr,
                                ioapic->mio_apic_id,
                                ioapic->mio_idx,
                                ioapic->mio_gsi_base,
                                ioapic->mio_npin);
                }
        }

        if (mptable_use_default) {
                if (bootverbose)
                        kprintf("MPTABLE: INTSRC irq 0 -> GSI 2 (default)\n");
                ioapic_intsrc(0, 2, INTR_TRIGGER_EDGE, INTR_POLARITY_HIGH);
                return;
        }

        error = mptable_map(&mpt);
        if (error)
                panic("mptable_ioapic_probe: mptable_map failed");
        KKASSERT(!MPTABLE_POS_USE_DEFAULT(&mpt));

        cth = mpt.mp_cth;

        mptable_bus_info_alloc(cth, &bus_info);

        if (TAILQ_EMPTY(&bus_info.mbi_list)) {
                if (bootverbose)
                        kprintf("MPTABLE: INTSRC irq 0 -> GSI 2 (no bus)\n");
                ioapic_intsrc(0, 2, INTR_TRIGGER_EDGE, INTR_POLARITY_HIGH);
        } else {
                struct mptable_ioapic_int_cbarg arg;

                bzero(&arg, sizeof(arg));
                arg.bus_info = &bus_info;

                error = mptable_iterate_entries(cth,
                            mptable_ioapic_int_callback, &arg);
                if (error)
                        panic("mptable_ioapic_int failed");

                if (arg.ioapic_nint == 0) {
                        if (bootverbose) {
                                kprintf("MPTABLE: INTSRC irq 0 -> GSI 2 "
                                        "(no int)\n");
                        }
                        ioapic_intsrc(0, 2, INTR_TRIGGER_EDGE,
                            INTR_POLARITY_HIGH);
                }
        }

        mptable_bus_info_free(&bus_info);

        mptable_unmap(&mpt);
}

static struct ioapic_enumerator mptable_ioapic_enumerator = {
        .ioapic_prio = IOAPIC_ENUM_PRIO_MPTABLE,
        .ioapic_probe = mptable_ioapic_probe,
        .ioapic_enumerate = mptable_ioapic_enumerate
};

static void
mptable_ioapic_enum_register(void)
{
        ioapic_enumerator_register(&mptable_ioapic_enumerator);
}
SYSINIT(mptable_ioapic, SI_BOOT2_PRESMP, SI_ORDER_ANY,
        mptable_ioapic_enum_register, 0);

void
mptable_pci_int_dump(void)
{
        const struct mptable_pci_int *pci_int;

        if (!bootverbose)
                return;

        TAILQ_FOREACH(pci_int, &mptable_pci_int_list, mpci_link) {
                kprintf("MPTABLE: %d:%d INT%c -> IOAPIC %d.%d\n",
                        pci_int->mpci_bus,
                        pci_int->mpci_dev,
                        pci_int->mpci_pin + 'A',
                        pci_int->mpci_ioapic_idx,
                        pci_int->mpci_ioapic_pin);
        }
}

int
mptable_pci_int_route(int bus, int dev, int pin, int intline)
{
        const struct mptable_pci_int *pci_int;
        int irq = -1;

        KKASSERT(pin >= 1);
        --pin;  /* zero based */

        TAILQ_FOREACH(pci_int, &mptable_pci_int_list, mpci_link) {
                if (pci_int->mpci_bus == bus &&
                    pci_int->mpci_dev == dev &&
                    pci_int->mpci_pin == pin)
                        break;
        }
        if (pci_int != NULL) {
                int gsi;

                gsi = ioapic_gsi(pci_int->mpci_ioapic_idx,
                        pci_int->mpci_ioapic_pin);
                if (gsi >= 0) {
                        irq = machintr_legacy_intr_find_bygsi(gsi,
                                INTR_TRIGGER_LEVEL, INTR_POLARITY_LOW);
                }
        }

        if (irq < 0 && intline >= 0) {
                kprintf("MPTABLE: fixed interrupt routing "
                    "for %d:%d INT%c\n", bus, dev, pin + 'A');
                irq = machintr_legacy_intr_find(intline,
                        INTR_TRIGGER_LEVEL, INTR_POLARITY_LOW);
        }

        if (irq >= 0 && bootverbose) {
                kprintf("MPTABLE: %d:%d INT%c routed to irq %d\n",
                        bus, dev, pin + 'A', irq);
        }
        return irq;
}