/*
 * $Id: smp.c,v 1.61 2009-01-28 14:38:05 potyra Exp $
 *
 * Copyright (C) 2003-2009 FAUmachine Team <info@faumachine.org>.
 * This program is free software. You can redistribute it and/or modify it
 * under the terms of the GNU General Public License, either version 2 of
 * the License, or (at your option) any later version. See COPYING.
 */

#include "build_config.h"

#ifdef CONFIG_SMP_SUPPORT

#include "compiler.h"

#include "assert.h"
#include "chipset.h"
#include "pci.h"
#include "stdio.h"

#define IOAPIC_BASE	0xfec00000
#define APIC_BASE	0xfee00000

/*
 * Constants for various Intel APICs. (local APIC, IOAPIC, etc.)
 *
 * Alan Cox <Alan.Cox@linux.org>, 1995.
 * Ingo Molnar <mingo@redhat.com>, 1999, 2000
 */

#define         APIC_ID         0x20
#define                 APIC_ID_MASK            (0x0F<<24)
#define                 GET_APIC_ID(x)          (((x)>>24)&0x0F)
#define         APIC_LVR        0x30
#define                 APIC_LVR_MASK           0xFF00FF
#define                 GET_APIC_VERSION(x)     ((x)&0xFF)
#define                 GET_APIC_MAXLVT(x)      (((x)>>16)&0xFF)
#define                 APIC_INTEGRATED(x)      ((x)&0xF0)
#define         APIC_TASKPRI    0x80
#define                 APIC_TPRI_MASK          0xFF
#define         APIC_ARBPRI     0x90
#define                 APIC_ARBPRI_MASK        0xFF
#define         APIC_PROCPRI    0xA0
#define         APIC_EOI        0xB0
#define                 APIC_EIO_ACK            0x0             /* Write this to the EOI register */
#define         APIC_RRR        0xC0
#define         APIC_LDR        0xD0
#define                 APIC_LDR_MASK           (0xFF<<24)
#define                 GET_APIC_LOGICAL_ID(x)  (((x)>>24)&0xFF)
#define                 SET_APIC_LOGICAL_ID(x)  (((x)<<24))
#define                 APIC_ALL_CPUS           0xFF
#define         APIC_DFR        0xE0
#define         APIC_SPIV       0xF0
#define                 APIC_SPIV_FOCUS_DISABLED        (1<<9)
#define                 APIC_SPIV_APIC_ENABLED          (1<<8)
#define         APIC_ISR        0x100
#define         APIC_TMR        0x180
#define         APIC_IRR        0x200
#define         APIC_ESR        0x280
#define                 APIC_ESR_SEND_CS        0x00001
#define                 APIC_ESR_RECV_CS        0x00002
#define                 APIC_ESR_SEND_ACC       0x00004
#define                 APIC_ESR_RECV_ACC       0x00008
#define                 APIC_ESR_SENDILL        0x00020
#define                 APIC_ESR_RECVILL        0x00040
#define                 APIC_ESR_ILLREGA        0x00080
#define         APIC_ICR        0x300
#define                 APIC_DEST_SELF          0x40000
#define                 APIC_DEST_ALLINC        0x80000
#define                 APIC_DEST_ALLBUT        0xC0000
#define                 APIC_ICR_RR_MASK        0x30000
#define                 APIC_ICR_RR_INVALID     0x00000
#define                 APIC_ICR_RR_INPROG      0x10000
#define                 APIC_ICR_RR_VALID       0x20000
#define                 APIC_INT_LEVELTRIG      0x08000
#define                 APIC_INT_ASSERT         0x04000
#define                 APIC_ICR_BUSY           0x01000
#define                 APIC_DEST_LOGICAL       0x00800
#define                 APIC_DM_FIXED           0x00000
#define                 APIC_DM_LOWEST          0x00100
#define                 APIC_DM_SMI             0x00200
#define                 APIC_DM_REMRD           0x00300
#define                 APIC_DM_NMI             0x00400
#define                 APIC_DM_INIT            0x00500
#define                 APIC_DM_STARTUP         0x00600
#define                 APIC_DM_EXTINT          0x00700
#define                 APIC_VECTOR_MASK        0x000FF
#define         APIC_ICR2       0x310
#define                 GET_APIC_DEST_FIELD(x)  (((x)>>24)&0xFF)
#define                 SET_APIC_DEST_FIELD(x)  ((x)<<24)
#define         APIC_LVTT       0x320
#define         APIC_LVTPC      0x340
#define         APIC_LVT0       0x350
#define                 APIC_LVT_TIMER_BASE_MASK        (0x3<<18)
#define                 GET_APIC_TIMER_BASE(x)          (((x)>>18)&0x3)
#define                 SET_APIC_TIMER_BASE(x)          (((x)<<18))
#define                 APIC_TIMER_BASE_CLKIN           0x0
#define                 APIC_TIMER_BASE_TMBASE          0x1
#define                 APIC_TIMER_BASE_DIV             0x2
#define                 APIC_LVT_TIMER_PERIODIC         (1<<17)
#define                 APIC_LVT_MASKED                 (1<<16)
#define                 APIC_LVT_LEVEL_TRIGGER          (1<<15)
#define                 APIC_LVT_REMOTE_IRR             (1<<14)
#define                 APIC_INPUT_POLARITY             (1<<13)
#define                 APIC_SEND_PENDING               (1<<12)
#define                 GET_APIC_DELIVERY_MODE(x)       (((x)>>8)&0x7)
#define                 SET_APIC_DELIVERY_MODE(x,y)     (((x)&~0x700)|((y)<<8))
#define                         APIC_MODE_FIXED         0x0
#define                         APIC_MODE_NMI           0x4
#define                         APIC_MODE_EXINT         0x7
#define         APIC_LVT1       0x360
#define         APIC_LVTERR     0x370
#define         APIC_TMICT      0x380
#define         APIC_TMCCT      0x390
#define         APIC_TDCR       0x3E0
#define                 APIC_TDR_DIV_TMBASE     (1<<2)
#define                 APIC_TDR_DIV_1          0xB
#define                 APIC_TDR_DIV_2          0x0
#define                 APIC_TDR_DIV_4          0x1
#define                 APIC_TDR_DIV_8          0x2
#define                 APIC_TDR_DIV_16         0x3
#define                 APIC_TDR_DIV_32         0x8
#define                 APIC_TDR_DIV_64         0x9
#define                 APIC_TDR_DIV_128        0xA


/* Some usefull definitions from include/i386/mpspec.h */

struct mp_config_table {
	char mpc_signature[4];
#define MPC_SIGNATURE	"PCMP"
	uint16_t mpc_length;		/* Size of table */
	uint8_t  mpc_spec;		/* Version 1.4: 0x04 */
	uint8_t  mpc_checksum;
	char  mpc_oem[8];
#define MPC_OEM		"FAU Inf3"
	char  mpc_productid[12];
#define MPC_PRODUCTID	"FAUmachine"
	uint32_t mpc_oemptr;		/* 0 if not present */
	uint16_t mpc_oemsize;		/* 0 if not present */
	uint16_t mpc_entrycount;
	uint32_t mpc_lapic;		/* APIC address */
	uint16_t mpc_extlength;
	uint8_t mpc_extchecksum;
	uint8_t mpc_reserved;
};

/* Followed by entries */

#define	MP_PROCESSOR	0
#define	MP_BUS		1
#define	MP_IOAPIC	2
#define	MP_INTSRC	3
#define	MP_LINTSRC	4

struct mpc_config_processor {
	uint8_t mpc_type;
	uint8_t mpc_apicid;		/* Local APIC number */
	uint8_t mpc_apicver;		/* Its versions */
	uint8_t mpc_cpuflag;
#define CPU_ENABLED		1	/* Processor is available */
#define CPU_BOOTPROCESSOR	2	/* Processor is the BP */
	uint32_t mpc_cpusignature;		
	uint32_t mpc_featureflags;	/* CPUID feature value */
	uint32_t mpc_reserved[2];
};

struct mpc_config_bus {
	uint8_t mpc_type;
	uint8_t mpc_busid;
	uint8_t mpc_bustype[6];
};

/* List of Bus Type string values, Intel MP Spec. */
#define BUSTYPE_EISA	"EISA"
#define BUSTYPE_ISA	"ISA"
#define BUSTYPE_INTERN	"INTERN"	/* Internal BUS */
#define BUSTYPE_MCA	"MCA"
#define BUSTYPE_VL	"VL"		/* Local bus */
#define BUSTYPE_PCI	"PCI"
#define BUSTYPE_PCMCIA	"PCMCIA"
#define BUSTYPE_CBUS	"CBUS"
#define BUSTYPE_CBUSII	"CBUSII"
#define BUSTYPE_FUTURE	"FUTURE"
#define BUSTYPE_MBI	"MBI"
#define BUSTYPE_MBII	"MBII"
#define BUSTYPE_MPI	"MPI"
#define BUSTYPE_MPSA	"MPSA"
#define BUSTYPE_NUBUS	"NUBUS"
#define BUSTYPE_TC	"TC"
#define BUSTYPE_VME	"VME"
#define BUSTYPE_XPRESS	"XPRESS"

struct mpc_config_ioapic
{
	uint8_t mpc_type;
	uint8_t mpc_apicid;
	uint8_t mpc_apicver;
	uint8_t mpc_flags;
#define MPC_APIC_USABLE         0x01
	uint32_t mpc_apicaddr;
};

struct mpc_config_intsrc
{
	uint8_t mpc_type;
	uint8_t mpc_irqtype;
	uint16_t mpc_irqflag;
	uint8_t mpc_srcbus;
	uint8_t mpc_srcbusirq;
	uint8_t mpc_dstapic;
	uint8_t mpc_dstirq;
};

enum mp_irq_source_types {
	mp_INT = 0,
	mp_NMI = 1,
	mp_SMI = 2,
	mp_ExtINT = 3
};

struct mpc_config_lintsrc
{
	uint8_t mpc_type;
	uint8_t mpc_irqtype;
	uint16_t mpc_irqflag;
	uint8_t mpc_srcbusid;
	uint8_t mpc_srcbusirq;
	uint8_t mpc_destapic;
#define MP_APIC_ALL     0xFF
	uint8_t mpc_destapiclint;
};

struct intel_mp_config {
	struct mp_config_table config_table;
	struct mpc_config_processor config_processor[2];
	struct mpc_config_bus config_bus[2];
	struct mpc_config_ioapic config_ioapic;
	struct mpc_config_intsrc config_intsrc[1 + 15 + 16];
	struct mpc_config_lintsrc config_lintsrc[2];
};

struct intel_mp_floating {
	char mpf_signature[4];		/* "_MP_" 			*/
#define MPF_SIGNATURE	"_MP_"
	uint32_t mpf_physptr;		/* Configuration table address	*/
	uint8_t mpf_length;		/* Our length (paragraphs)	*/
	uint8_t mpf_specification;	/* Specification version	*/
	uint8_t mpf_checksum;		/* Checksum (makes sum 0)	*/
	uint8_t mpf_feature1;		/* Standard or configuration ? 	*/
	uint8_t mpf_feature2;		/* Bit7 set for IMCR|PIC	*/
	uint8_t mpf_feature3;		/* Unused (0)			*/
	uint8_t mpf_feature4;		/* Unused (0)			*/
	uint8_t mpf_feature5;		/* Unused (0)			*/
};

#ifdef INIT_RM
extern const uint8_t mpc[sizeof(struct mp_config_table)
		+ 2 * sizeof(struct mpc_config_processor)
		+ 2 * sizeof(struct mpc_config_bus)
		+ 1 * sizeof(struct mpc_config_ioapic)
		+ (1+15+16+1) * sizeof(struct mpc_config_intsrc)
		+ 2 * sizeof(struct mpc_config_lintsrc)];
#endif /* INIT_RM */
#ifdef RUNTIME_RM
const uint8_t mpc[sizeof(struct mp_config_table)
		+ 2 * sizeof(struct mpc_config_processor)
		+ 2 * sizeof(struct mpc_config_bus)
		+ 1 * sizeof(struct mpc_config_ioapic)
		+ (1+15+16+1) * sizeof(struct mpc_config_intsrc)
		+ 2 * sizeof(struct mpc_config_lintsrc)] = { 0, /* ... */};
#endif /* RUNTIME_RM */

#ifdef INIT_RM
extern const char mpf[sizeof(struct intel_mp_floating)];
#endif /* INIT_RM */
#ifdef RUNTIME_RM
const char __attribute__((section(".rodata.table.mp")))
		mpf[sizeof(struct intel_mp_floating)] = { 0, 0 /* ... */};
#endif /* RUNTIME_RM */

#ifdef INIT_RM
static struct {
	unsigned int apicver;
	unsigned int cpuflag;
	uint32_t cpusignature;
	uint32_t featureflags;
} smp_cpuinfo[2];
static unsigned int smp_ncpus;
#endif /* INIT_RM */

#ifdef RUNTIME_RM
CODE16;

/*
 * Handler for APIC Spurious Interrupt
 */
void
apic_spv_irq(void)
{
	/* nothing */
	return;
}

#endif

#ifdef INIT_RM
CODE16;

/*
 * CAUTION:
 *
 * Make sure gcc uses register indirect addressing mode to access
 * (I/O-) APIC. Otherwise gas will use "short address" mode and clip
 * upper address bits.
 */
static uint32_t
ioapic_read(unsigned int reg)
{
	uint32_t v;

	asm volatile (
		"movl %1, (%2)\n\t"
		"movl 0x10(%2), %0\n\t"
		: "=r" (v)
		: "r" (reg), "r" (IOAPIC_BASE)
	);
	return v;
}

static void
apic_write(unsigned int reg, uint32_t v)
{
	asm volatile (
		"movl %0, (%1)\n\t"
		: /*No Output*/
		: "r" (v), "r" (APIC_BASE + reg)
	);
}

static uint32_t
apic_read(unsigned int reg)
{
	uint32_t v;

	asm volatile (
		"movl (%1), %0\n\t"
		: "=r" (v)
		: "r" (APIC_BASE + reg)
	);
	return v;
}

static void
smp_apic_init(void)
{
	uint32_t v;

	/* Disable Timer */
	v = apic_read(APIC_LVTT);
	v |= APIC_LVT_MASKED;
	apic_write(APIC_LVTT, v);

	/* Use LINT0 for external interrupts. */
	v = apic_read(APIC_LVT0);
	v &= ~APIC_LVT_MASKED;
	v = SET_APIC_DELIVERY_MODE(v, APIC_MODE_EXINT);
	v &= ~0xff;
	v |= 0x80; /* Any vector should do... */
	apic_write(APIC_LVT0, v);

	/* Use LINT1 for NMI. */
	v = apic_read(APIC_LVT1);
	v &= ~APIC_LVT_MASKED;
	v = SET_APIC_DELIVERY_MODE(v, APIC_MODE_NMI);
	v &= ~0xff; /* No vector needed. */
	apic_write(APIC_LVT1, v);

	/* Enable APIC. */
	v = apic_read(APIC_SPIV);
	v &= ~APIC_VECTOR_MASK;
	v |= APIC_SPIV_APIC_ENABLED;
	v |= APIC_SPIV_FOCUS_DISABLED;
	v |= 0xff; /* Vector */
	apic_write(APIC_SPIV, v);
}

static inline struct mp_config_table * __attribute__((always_inline))
smp_mpc_get(void)
{
	struct mp_config_table *mpcp;

	asm volatile (
		""
		: "=r" (mpcp)
		: "0" (mpc)
	);

	return mpcp;
}

static inline struct intel_mp_floating * __attribute__((always_inline))
smp_mpf_get(void)
{
	struct intel_mp_floating *mpfp;

	asm volatile (
		""
		: "=r" (mpfp)
		: "0" (&mpf)
	);

	return mpfp;
}


#if 0
static void
smp_ap(void)
{
	smp_add_proc();
}
#endif

extern void init_rm_ap(void);

static int
smp_check(void)
{
	uint32_t eax;
	uint32_t ebx;
	uint32_t ecx;
	uint32_t edx;

	/*
	 * Check for SMP Support
	 */
	/* Try to set/clear ID bit in %eflags. */
	asm volatile (
		"\tpushfl\n"

		"\tpushfl\n"
		"\torl $1 << 21, (%%esp)\n"
		"\tpopfl\n"
		"\tpushfl\n"
		"\tpopl %0\n"

		"\tpushfl\n"
		"\tandl $~(1 << 21), (%%esp)\n"
		"\tpopfl\n"
		"\tpushfl\n"
		"\tpopl %1\n"

		"\tpopfl\n"

		: "=r" (eax), "=r" (ebx)
		: /* No input */
	);
	if (((eax >> 21) & 1) == ((ebx >> 21) & 1)) {
		/* No ID bit in %eflags. */
		/* => No cpuid support. */
		/* => No APIC support. */
		/* => No SMP support. */
		return 0;
	}

	/* Get CPU information. */
	asm volatile (
		"\tcpuid\n"
		: "=a" (eax), "=b" (ebx), "=c" (ecx), "=d" (edx)
		: "0" (1)
	);
	if (! ((edx >> 9) & 1)) {
		/* No APIC support. */
		return 0;
	}

	return 1;
}

void
smp_register(void)
{
	unsigned int apicid;
	unsigned int apicver;
	unsigned int bsp;
	uint32_t eax;
	uint32_t ebx;
	uint32_t ecx;
	uint32_t edx;

	if (! smp_check()) {
		return;
	}

	apicid = GET_APIC_ID(apic_read(APIC_ID));
	assert(/* 0 <= apicid
	    && */ apicid < sizeof(smp_cpuinfo) / sizeof(smp_cpuinfo[0]));

	apicver = GET_APIC_VERSION(apic_read(APIC_LVR));
	smp_cpuinfo[apicid].apicver = apicver;

	asm volatile (
		"rdmsr\n"
		: "=a" (eax), "=d" (edx)
		: "c" (0x1b) /*MSR_IA32_APICBASE*/
	);
	bsp = (eax >> 8) & 1;
	smp_cpuinfo[apicid].cpuflag = (bsp << 1) | (1 << 0);

	asm volatile (
		"cpuid\n"
		: "=a" (eax), "=b" (ebx), "=c" (ecx), "=d" (edx)
		: "0" (1)
	);
	smp_cpuinfo[apicid].cpusignature = eax;
	smp_cpuinfo[apicid].featureflags = edx;

	smp_ncpus++;
}

void
smp_aps_start(void)
{
	unsigned int i;

	if (! smp_check()) {
		return;
	}

	apic_write(APIC_ICR2, 0xff << 24);	/* Destination */
	apic_write(APIC_ICR, (3 << 18)	/* All Excluding Self */
			| (1 << 15)	/* Trigger Mode */
			| (1 << 14)	/* Level */
			| (0 << 11)	/* Physical Destination Mode */
			| (6 << 8)	/* Startup Delivery Mode */
			| (uint32_t) init_rm_ap / 4096); /* Vector */

	for (i = 0; i < 10000; i++) {
		asm volatile ("\tpause\n");
	}
}

/* Fix MP configuration table. */
void
smp_init(void)
{
	struct mp_config_table *mpcp;
	unsigned int length;
	unsigned int entrycount;
	struct mpc_config_processor *mpc_procp;
	struct mpc_config_bus *mpc_busp;
	struct mpc_config_ioapic *mpc_ioapicp;
	struct mpc_config_intsrc *mpc_intsrcp;
	struct mpc_config_lintsrc *mpc_lintsrcp;
	struct intel_mp_floating *mpfp;
	unsigned int apicver;
	unsigned int dev;
	unsigned int func;
	uint8_t *p;
	unsigned int i;
	uint8_t sum;

	/*
	 * Check for SMP Support
	 */
	if (! smp_check()) {
		return;
	}

	/* Init APIC of BSP processor. */
	smp_apic_init();

	mpcp = smp_mpc_get();
	mpfp = smp_mpf_get();

	/* Make runtime BIOS read-write. */
	chipset_bios_readwrite();

	/*
	 * Fill config mp struct.
	 */
	length = 0;
	entrycount = 0;

	/* Processor Entries */
	mpc_procp = (struct mpc_config_processor *) (mpcp + 1);

	mpc_procp->mpc_type = MP_PROCESSOR;
	mpc_procp->mpc_apicid = 0;
	mpc_procp->mpc_apicver = smp_cpuinfo[0].apicver;
	mpc_procp->mpc_cpuflag = smp_cpuinfo[0].cpuflag;
	mpc_procp->mpc_cpusignature = smp_cpuinfo[0].cpusignature;
	mpc_procp->mpc_featureflags = smp_cpuinfo[0].featureflags;
	mpc_procp->mpc_reserved[0] = 0;
	mpc_procp->mpc_reserved[1] = 0;
	mpc_procp++;
	length += sizeof(*mpc_procp);
	entrycount++;

	mpc_procp->mpc_type = MP_PROCESSOR;
	mpc_procp->mpc_apicid = 1;
	mpc_procp->mpc_apicver = smp_cpuinfo[1].apicver;
	mpc_procp->mpc_cpuflag = smp_cpuinfo[1].cpuflag;
	mpc_procp->mpc_cpusignature = smp_cpuinfo[1].cpusignature;
	mpc_procp->mpc_featureflags = smp_cpuinfo[1].featureflags;
	mpc_procp->mpc_reserved[0] = 0;
	mpc_procp->mpc_reserved[1] = 0;
	mpc_procp++;
	length += sizeof(*mpc_procp);
	entrycount++;

	/* Bus Entries */
	mpc_busp = (struct mpc_config_bus *) mpc_procp;

	mpc_busp->mpc_type = MP_BUS;
	mpc_busp->mpc_busid = 0;
	mpc_busp->mpc_bustype[0] = 'P';
	mpc_busp->mpc_bustype[1] = 'C';
	mpc_busp->mpc_bustype[2] = 'I';
	mpc_busp->mpc_bustype[3] = ' ';
	mpc_busp->mpc_bustype[4] = ' ';
	mpc_busp->mpc_bustype[5] = ' ';
	mpc_busp++;
	length += sizeof(*mpc_busp);
	entrycount++;

	mpc_busp->mpc_type = MP_BUS;
	mpc_busp->mpc_busid = 1;
	mpc_busp->mpc_bustype[0] = 'I';
	mpc_busp->mpc_bustype[1] = 'S';
	mpc_busp->mpc_bustype[2] = 'A';
	mpc_busp->mpc_bustype[3] = ' ';
	mpc_busp->mpc_bustype[4] = ' ';
	mpc_busp->mpc_bustype[5] = ' ';
	mpc_busp++;
	length += sizeof(*mpc_busp);
	entrycount++;

	/* I/O-APIC Entry */
	mpc_ioapicp = (struct mpc_config_ioapic *) mpc_busp;

	mpc_ioapicp->mpc_type = MP_IOAPIC;
	mpc_ioapicp->mpc_apicid = 2;
	apicver = (ioapic_read(1) >> 0) & 0xff;
	mpc_ioapicp->mpc_apicver = apicver;
	mpc_ioapicp->mpc_flags = MPC_APIC_USABLE;
	mpc_ioapicp->mpc_apicaddr = IOAPIC_BASE,
	mpc_ioapicp++;
	length += sizeof(*mpc_ioapicp);
	entrycount++;

	/* Interrupt Source Entries */
	mpc_intsrcp = (struct mpc_config_intsrc *) mpc_ioapicp;

	mpc_intsrcp->mpc_type = MP_INTSRC;	/* Output of PIC0 */
	mpc_intsrcp->mpc_irqtype = mp_ExtINT;
	mpc_intsrcp->mpc_srcbus = 1;
	mpc_intsrcp->mpc_srcbusirq = 0;
	mpc_intsrcp->mpc_dstapic = 2;
	mpc_intsrcp->mpc_dstirq = 0;
	mpc_intsrcp++;
	length += sizeof(*mpc_intsrcp);
	entrycount++;

	for (i = 1; i < 16; i++) {		/* 15 ISA Bus IRQs */
		mpc_intsrcp->mpc_type = MP_INTSRC;
		mpc_intsrcp->mpc_irqtype = mp_INT;
		mpc_intsrcp->mpc_irqflag = 0;
		mpc_intsrcp->mpc_srcbus = 1;
		mpc_intsrcp->mpc_srcbusirq = (i == 2) ? 0 : i;
		mpc_intsrcp->mpc_dstapic = 2;
		mpc_intsrcp->mpc_dstirq = i;
		mpc_intsrcp++;
		length += sizeof(*mpc_intsrcp);
		entrycount++;
	}

	mpc_intsrcp->mpc_type = MP_INTSRC;	/* INTA Southbridge */
	mpc_intsrcp->mpc_irqtype = mp_INT;	/* Power Management */
	mpc_intsrcp->mpc_irqflag = 0xf;
	mpc_intsrcp->mpc_srcbus = 0;
	mpc_intsrcp->mpc_srcbusirq = (7 << 2) | (0 << 0); /* Correct? */
	mpc_intsrcp->mpc_dstapic = 2;
	mpc_intsrcp->mpc_dstirq = 18;		/* Correct? */
	mpc_intsrcp++;
	length += sizeof(*mpc_intsrcp);
	entrycount++;
	
	mpc_intsrcp->mpc_type = MP_INTSRC;	/* INTD Southbridge */
	mpc_intsrcp->mpc_irqtype = mp_INT;	/* USB Controller */
	mpc_intsrcp->mpc_irqflag = 0xf;
	mpc_intsrcp->mpc_srcbus = 0;
	mpc_intsrcp->mpc_srcbusirq = (7 << 2) | (1 << 0); /* Correct? */
	mpc_intsrcp->mpc_dstapic = 2;
	mpc_intsrcp->mpc_dstirq = 16;		/* Correct? */
	mpc_intsrcp++;
	length += sizeof(*mpc_intsrcp);
	entrycount++;

	for (dev = 8; dev < 12; dev++) {	/* 4 PCI Slots */
		for (func = 0; func < 8; func++) {
			unsigned int irq;

			irq = pci_creadb(0, dev, func, PCI_INTERRUPT_PIN);
			if (irq == 0
			 || irq == 0xff) {
				/* No Interrupt */
				continue;
			}
			irq--;

			mpc_intsrcp->mpc_type = MP_INTSRC;
			mpc_intsrcp->mpc_irqtype = mp_INT;
			mpc_intsrcp->mpc_irqflag = 0xf;
			mpc_intsrcp->mpc_srcbus = 0;
			mpc_intsrcp->mpc_srcbusirq = (dev << 2) | (irq << 0);
			mpc_intsrcp->mpc_dstapic = 2;
			mpc_intsrcp->mpc_dstirq = 16 + ((dev + irq) & 3);
			mpc_intsrcp++;
			length += sizeof(*mpc_intsrcp);
			entrycount++;
		}
	}
	
	mpc_intsrcp->mpc_type = MP_INTSRC;	/* SMI */
	mpc_intsrcp->mpc_irqtype = mp_SMI;
	mpc_intsrcp->mpc_irqflag = 0;
	mpc_intsrcp->mpc_srcbus = 1;
	mpc_intsrcp->mpc_srcbusirq = 0;
	mpc_intsrcp->mpc_dstapic = 2;
	mpc_intsrcp->mpc_dstirq = 23;
	mpc_intsrcp++;
	length += sizeof(*mpc_intsrcp);
	entrycount++;

	/* Lint Source Entries */
	mpc_lintsrcp = (struct mpc_config_lintsrc *) mpc_intsrcp;

	mpc_lintsrcp->mpc_type = MP_LINTSRC;	/* Lint0 */
	mpc_lintsrcp->mpc_irqtype = mp_ExtINT;
	mpc_lintsrcp->mpc_irqflag = 0;
	mpc_lintsrcp->mpc_srcbusid = 0;
	mpc_lintsrcp->mpc_srcbusirq = 0;
	mpc_lintsrcp->mpc_destapic = MP_APIC_ALL;
	mpc_lintsrcp->mpc_destapiclint = 0;
	mpc_lintsrcp++;
	length += sizeof(*mpc_lintsrcp);
	entrycount++;

	mpc_lintsrcp->mpc_type = MP_LINTSRC;	/* Lint1 */
	mpc_lintsrcp->mpc_irqtype = mp_NMI;
	mpc_lintsrcp->mpc_irqflag = 0;
	mpc_lintsrcp->mpc_srcbusid = 0;
	mpc_lintsrcp->mpc_srcbusirq = 0;
	mpc_lintsrcp->mpc_destapic = MP_APIC_ALL;
	mpc_lintsrcp->mpc_destapiclint = 1;
	mpc_lintsrcp++;
	length += sizeof(*mpc_lintsrcp);
	entrycount++;

	assert((const void *) mpc_lintsrcp <= (const void *) &mpc[sizeof(mpc)]);

	/*
	 * Fill config mp struct.
	 */
	mpcp->mpc_signature[0] = 'P';
	mpcp->mpc_signature[1] = 'C';
	mpcp->mpc_signature[2] = 'M';
	mpcp->mpc_signature[3] = 'P';
	mpcp->mpc_length = length;
	mpcp->mpc_spec = 1;	/* 1.1 */
	mpcp->mpc_oem[0] = 'F';
	mpcp->mpc_oem[1] = 'A';
	mpcp->mpc_oem[2] = 'U';
	mpcp->mpc_oem[3] = ' ';
	mpcp->mpc_oem[4] = 'I';
	mpcp->mpc_oem[5] = 'n';
	mpcp->mpc_oem[6] = 'f';
	mpcp->mpc_oem[7] = '3';
	mpcp->mpc_productid[0] = 'F';
	mpcp->mpc_productid[1] = 'A';
	mpcp->mpc_productid[2] = 'U';
	mpcp->mpc_productid[3] = 'm';
	mpcp->mpc_productid[4] = 'a';
	mpcp->mpc_productid[5] = 'c';
	mpcp->mpc_productid[6] = 'h';
	mpcp->mpc_productid[7] = 'i';
	mpcp->mpc_productid[8] = 'n';
	mpcp->mpc_productid[9] = 'e';
	mpcp->mpc_productid[10] = ' ';
	mpcp->mpc_productid[11] = ' ';
	mpcp->mpc_oemptr = 0;
	mpcp->mpc_oemsize = 0;
	mpcp->mpc_entrycount = entrycount;
	mpcp->mpc_lapic = APIC_BASE;
	mpcp->mpc_extlength = 0;
	mpcp->mpc_extchecksum = 0;
	mpcp->mpc_reserved = 0;

	/* Fix checksum. */
	sum = 0;
	for (p = (uint8_t *) mpcp, i = 0;
	    i < length;
	    p++, i++) {
		sum += *p;
	}
	mpcp->mpc_checksum -= sum;

	/*
	 * Fill floating mp struct.
	 */
	mpfp->mpf_signature[0] = '_';
	mpfp->mpf_signature[1] = 'M';
	mpfp->mpf_signature[2] = 'P';
	mpfp->mpf_signature[3] = '_';
	mpfp->mpf_physptr = (uint32_t) mpcp;
	mpfp->mpf_length = sizeof(*mpfp) / 16;
	mpfp->mpf_specification = 1;	/* 1.1 */
	mpfp->mpf_feature1 = 0;		/* No standard configuration. */
	mpfp->mpf_feature2 = 0;		/* Virtual wire mode. */
	mpfp->mpf_feature3 = 0;		/* Reserved */
	mpfp->mpf_feature4 = 0;		/* Reserved */
	mpfp->mpf_feature5 = 0;		/* Reserved */

	/* Fix checksum. */
	sum = 0;
	for (p = (uint8_t *) mpfp; p < (uint8_t *) (mpfp + 1); p++) {
		sum += *p;
	}
	mpfp->mpf_checksum -= sum;

	/* Make runtime BIOS read-only again. */
	chipset_bios_readonly();
}

#endif /* INIT_RM */

#endif /* CONFIG_SMP_SUPPORT */