/*
 * mp.c
 *
 * Copyright 2002, Minoru Murashima. All rights reserved.
 * Distributed under the terms of the BSD License.
 *
 * マルチプロセッサ用ルーチン
 */


#include<types.h>
#include<config.h>
#include<errno.h>
#include<lib.h>
#include<proc.h>
#include<interrupt.h>
#include<mm.h>
#include<time.h>
#include<lock.h>
#include<time.h>
#include<pci.h>
#include<console.h>
#include<sp.h>
#include<mp.h>


enum{
	/* MPテーブル関係定数 */
	FLOAT_HEADER_ADDRES=4,			/* MP configuration table address */
	FLOAT_CONFIG_TYPE=11,			/* MP System Configuration Type(8バイト) */
	FLOAT_IMCR_FLAG=12,				/* IMCR presence bit(7ビット目) */

	HEADER_LENGTH=44,				/* Header table length(バイト) */
	HEADER_TABLE_LENGTH=4,			/* Base configuration length(16バイト) */
	HEADER_ENTRY_COUNT=34,			/* Base configuration table count(16バイト) */
	HEADER_LOCAL_APIC_ADDRES=36,	/* Local APIC address */

	TABLE_ENTRY_TYPE=0,				/* Configuration table entry type(8バイト) */

	TYPE_PROCESSOR=0,				/* CPUタイプコード */
	TYPE_BUS=1,						/* BUSタイプコード */
	TYPE_IO_APIC=2,					/* IO_APICタイプコード */
	TYPE_IO_INTERRUPT=3,			/* IO_INTERRUPTタイプコード */
	TYPE_LOCAL_INTERRUPT=4,			/* LOCAL_INTERRUPTタイプコード */

	TABLE_PROCESSOR_LENGTH=20,		/* PROCESSOR table length(バイト) */
	TABLE_PROCESSOR_FLAG=3,			/* CPU enable flag(0ビット目) */
	TABLE_PROCESSOR_FAMILY=5,		/* CPU Family コード(8バイト)*/

	TABLE_IO_APIC_ID=1,				/* IO_APIC ID */
	TABLE_IO_APIC_FLAG=3,			/* IO_APIC enable flag(0ビット目) */
	TABLE_IO_APIC_ADDRESS=4,		/* IO_APIC address */

	/* 割り込みベクター */
	APIC_TIMER_VECT=	0x30,
	IRQ0_VECT=			0x2f,
	IRQ1_VECT=			0x2e,
	IRQ3_VECT=			0x2c,
	IRQ4_VECT=			0x2b,
	IRQ5_VECT=			0x2a,
	IRQ6_VECT=			0x29,
	IRQ7_VECT=			0x28,
	IRQ8_VECT=			0x27,
	IRQ9_VECT=			0x26,
	IRQ10_VECT=			0x25,
	IRQ11_VECT=			0x24,
	IRQ12_VECT=			0x23,
	IRQ13_VECT=			0x22,
	IRQ14_VECT=			0x21,
	IRQ15_VECT=			0x20,
};


uint MFPS_addres;			/* MFPSアドレス、setup.Sで代入 */
extern int *active_cpu;		/* 稼動CPU数。 */
extern void start_tasking();


/*****************************************************************************
 *
 * LOCAL APIC
 *
 *****************************************************************************/


enum{
	/* LOCAL_APICベースアドレス */
	LOCAL_APIC_BASE=0xfee00000,

	/* LOCAL APIC register */
	LOCAL_APIC_ID=	0x20/sizeof(int),
	LOCAL_APIC_TPR=	0x80/sizeof(int),
	LOCAL_APIC_EOI=	0xb0/sizeof(int),
	LOCAL_APIC_LDR=	0xd0/sizeof(int),
	LOCAL_APIC_DFR=	0xe0/sizeof(int),
	LOCAL_APIC_SVR=	0xf0/sizeof(int),
	LOCAL_APIC_ISR1=0x100/sizeof(int),
	LOCAL_APIC_ISR2=0x110/sizeof(int),
	LOCAL_APIC_TMR1=0x180/sizeof(int),
	LOCAL_APIC_TMR2=0x190/sizeof(int),
	LOCAL_APIC_IRR1=0x200/sizeof(int),
	LOCAL_APIC_IRR2=0x200/sizeof(int),
	LOCAL_APIC_ICR1=0x300/sizeof(int),
	LOCAL_APIC_ICR2=0x310/sizeof(int),
	LOCAL_APIC_TIME=0x320/sizeof(int),
	LOCAL_APIC_LNT0=0x350/sizeof(int),
	LOCAL_APIC_LNT1=0x360/sizeof(int),
	LOCAL_APIC_ICNT=0x380/sizeof(int),
	LOCAL_APIC_CCNT=0x390/sizeof(int),
	LOCAL_APIC_TDVC=0x3e0/sizeof(int),
};


static uint volatile *local_apic = (uint*)LOCAL_APIC_BASE;	/* LOCAL APIC base address */
static uint task_count;										/* 1タスク稼動時間のAPICタイマーカウント値 */


/*
 * APIC TIMER handler
 * smpシステムのタスキングを行う
 * returns : Task switch on
 */
static int apictimer_handler(INTERRUPT_FRAME frame)
{
/*****************************************************************************/
	{
		static char value[MAX_CPU] = {'0','0','0','0','0','0','0','0'};
		int cpu = get_current_cpu();

		if ('9' < value[cpu]){
			value[cpu] = '0';
		}
		displayTop(60 + cpu, &value[cpu],1);
		++value[cpu];
		
//		if (get_current_task() == (PROC*)0x150800){
//			printDebug(80 * 1 + 70, "%x",frame.eip );
//		}
	}
/****************************************************************************/
	local_apic[LOCAL_APIC_ICNT]=task_count;	/* restart APIC TIMER */

	return 1;
}


/*
 * APIC_BASE_MSRに書き込む
 * 引数 : 書き込む値
 */
static void wr_apic_msr(uint base)
{
	enum{APICBASE=27};


	asm volatile(
		"xorl	%%edx,%%edx\n"
		"wrmsr"
		::"a"(base|0x800),"c"(APICBASE)
	);
}


/*
 * APIC TIMERの１sのカウント数を測定する
 * return : １sのカウント数
 */
static uint count_apictimer()
{
	int low,high;
	int apictimer_count;
	double time;


	outb(PIT_CTR,0x30);		/* CLK0,LSB.MSB,モード0,binary */
	outb(PIT_CNR0,0);		/* LSB */
	outb(PIT_CNR0,0);		/* MSB */

	local_apic[LOCAL_APIC_ICNT]=0xffffffff;								/* APIC TIMER始動 */
	for(;;)if(0xffffffff-local_apic[LOCAL_APIC_CCNT]>0x200000)break;	/* 遅延 */

	apictimer_count=local_apic[LOCAL_APIC_CCNT];

	low=inb(PIT_CNR0);
	high=inb(PIT_CNR0);

	time=(0xffff-low-(high<<8))/(double)PIT_HZ;
	apictimer_count=(0xffffffff-apictimer_count)/time;

	return apictimer_count;  /* 1sのカウント回数 */
}


/*
 * PUBLIC
 * Virtual Wireモードを無効にする 
 */
static void ignoreVirtualWireMode()
{
	local_apic[LOCAL_APIC_LNT0] = 0x10000;		/* LINT0をマスク */
	local_apic[LOCAL_APIC_LNT1] = 0x10000;		/* LINT1をマスク */
}


/*
 * Init LOCAL APIC
 * Require atomic handlig
 * parameters : Cpu number
 */
static void init_localapic(int volatile cpu)
{
	uint value;


	/* LOCAL_APICベースアドレスの設定 */
	wr_apic_msr(LOCAL_APIC_BASE);		/* LOCAL_APICハードイネーブル */

	/* LOCAL_APICソフトイネーブル */
	local_apic[LOCAL_APIC_SVR]=0x12f;

	/* LOCAL_APIC_IDを設定する */
	local_apic[LOCAL_APIC_ID]=cpu<<24;

	/* 論理モードをフラットモデル(8cpu)に設定する */
	local_apic[LOCAL_APIC_LDR]=0x1000000<<cpu;
	local_apic[LOCAL_APIC_DFR]=0xffffffff;

	/* タスクプライオリティを初期化する */
	local_apic[LOCAL_APIC_TPR]=0;

	/* タイマー割り込みの設定 */
	local_apic[LOCAL_APIC_TDVC]=0xb;					/* 分周値1 */
	local_apic[LOCAL_APIC_TIME]=APIC_TIMER_VECT;		/* ワンショットモード|マスク */
	if (cpu == 0)
	{
		set_idt(APIC_TIMER_VECT,apictimer,IDT_TRAP);	/* idtの設定 */
		value=count_apictimer();						/* APICタイマーHZの測定 */
		printk("APIC TIMER = %d HZ\n",value);
		task_count=value/(1000/TASK_TIME);				/* タスク稼動時間 */
		irq_entry[APIC_TIMER]=apictimer_handler;		/* APICタイマーをInterrupt tableに設定する */
	}
	local_apic[LOCAL_APIC_ICNT] = task_count;			/* タイマー始動 */
}


/*
 * Issue interrupt to cpu
 * parameters : Cpu number,Interrupt vecter
 */
void issue_interrupt_to_cpu(int cpu,int vecter)
{
	local_apic[LOCAL_APIC_ICR2]=cpu<<24;
	local_apic[LOCAL_APIC_ICR1]=0x00004000|vecter;	/* デスティネーションモード物理 */
	while(local_apic[LOCAL_APIC_ICR1]&0x1000);		/* 伝達ステータスの確認 */
}


/*
 * Page directory flash handler
 * returns : Task switch on
 */
int flash_pagedir_handler(INTERRUPT_FRAME frame)
{
	updateVm();
	local_apic[LOCAL_APIC_EOI]=0;		/* issue EOI */

	return 1;
}


/*****************************************************************************
 *
 * IO APIC
 *
 *****************************************************************************/


enum{
	/* IOAPIC register */
	IO_APIC_ID=		0x0,
	IO_APIC_VER=	0x1,
	IO_APIC_ARBT=	0x2,
	IO_APIC_LRT0=	0x10,
	IO_APIC_HRT0=	0x11,
	IO_APIC_LRT1=	0x12,
	IO_APIC_HRT1=	0x13,
	IO_APIC_LRT2=	0x14,
	IO_APIC_HRT2=	0x15,
	IO_APIC_LRT3=	0x16,
	IO_APIC_HRT3=	0x17,
	IO_APIC_LRT4=	0x18,
	IO_APIC_HRT4=	0x19,
	IO_APIC_LRT5=	0x1a,
	IO_APIC_HRT5=	0x1b,
	IO_APIC_LRT6=	0x1c,
	IO_APIC_HRT6=	0x1d,
	IO_APIC_LRT7=	0x1e,
	IO_APIC_HRT7=	0x1f,
	IO_APIC_LRT8=	0x20,
	IO_APIC_HRT8=	0x21,
	IO_APIC_LRT9=	0x22,
	IO_APIC_HRT9=	0x23,
	IO_APIC_LRT10=	0x24,
	IO_APIC_HRT10=	0x25,
	IO_APIC_LRT11=	0x26,
	IO_APIC_HRT11=	0x27,
	IO_APIC_LRT12=	0x28,
	IO_APIC_HRT12=	0x29,
	IO_APIC_LRT13=	0x2a,
	IO_APIC_HRT13=	0x2b,
	IO_APIC_LRT14=	0x2c,
	IO_APIC_HRT14=	0x2d,
	IO_APIC_LRT15=	0x2e,
	IO_APIC_HRT15=	0x2f,
	IO_APIC_LRT16=	0x30,
	IO_APIC_HRT16=	0x31,
	IO_APIC_LRT17=	0x32,
	IO_APIC_HRT17=	0x33,
	IO_APIC_LRT18=	0x34,
	IO_APIC_HRT18=	0x35,
	IO_APIC_LRT19=	0x36,
	IO_APIC_HRT19=	0x37,

	/* IO APIC register */
	IO_APIC_REGSEL=	0xfec00000,
	IO_APIC_WIN=	0xfec00010,

	/* IO APIC redirection table. */
	IOREDTBL_MASK=	0x10000,
	IOREDTBL_POLLOW=1<<13,
	IOREDTBL_MODE_PHYS=0,
	IOREDTBL_MODE_LOGIC=1<<11,
	IOREDTBL_DELIV_FIX=0,
	IOREDTBL_DELIV_LOWPRT=1<<8,
	IOREDTBL_DELIV_EXTINT=7<<8,

	IO_APIC_PCIIRQ=16,		/* Begin of PCI IRQ number. */
};


/* IRQごとのIOAPIC LRTアドレス */
uint ioapic_lrt[]=
{
	IO_APIC_LRT2 ,
	IO_APIC_LRT1 ,
	0            ,
	IO_APIC_LRT3 ,
	IO_APIC_LRT4 ,
	IO_APIC_LRT5,
	IO_APIC_LRT6 ,
	IO_APIC_LRT7 ,
	IO_APIC_LRT8 ,
	IO_APIC_LRT9 ,
	IO_APIC_LRT10,
	IO_APIC_LRT11,
	IO_APIC_LRT12,
	IO_APIC_LRT13,
	IO_APIC_LRT14,
	IO_APIC_LRT15,
	IO_APIC_LRT16,
	IO_APIC_LRT17,
	IO_APIC_LRT18,
	IO_APIC_LRT19,
};

/* IRQごとのIOAPIC HRTアドレス */
static uint ioapic_hrt[]=
{
	IO_APIC_HRT2 ,
	IO_APIC_HRT1 ,
	0            ,
	IO_APIC_HRT3 ,
	IO_APIC_HRT4 ,
	IO_APIC_HRT5,
	IO_APIC_HRT6 ,
	IO_APIC_HRT7 ,
	IO_APIC_HRT8 ,
	IO_APIC_HRT9 ,
	IO_APIC_HRT10,
	IO_APIC_HRT11,
	IO_APIC_HRT12,
	IO_APIC_HRT13,
	IO_APIC_HRT14,
	IO_APIC_HRT15,
	IO_APIC_HRT16,
	IO_APIC_HRT17,
	IO_APIC_HRT18,
	IO_APIC_HRT19,
};
static uchar ioapic_intr_vect[]=
{
	IRQ0_VECT ,
	IRQ1_VECT ,
	0         ,
	IRQ3_VECT ,
	IRQ4_VECT ,
	IRQ5_VECT ,
	IRQ6_VECT ,
	IRQ7_VECT ,
	IRQ8_VECT ,
	IRQ9_VECT ,
	IRQ10_VECT,
	IRQ11_VECT,
	IRQ12_VECT,
	IRQ13_VECT,
	IRQ14_VECT,
	IRQ15_VECT,
};
static uint volatile *io_apic_regsel=(uint*)IO_APIC_REGSEL;	/* IOAPIC REGSEL address */
static uint volatile *io_apic_win=(uint*)IO_APIC_WIN;		/* IOAPIC WIN address */


/*
 * Read IOAPIC
 * 引数 : アドレス
 * 返り値 : 読み込み値
 */
static inline uint read_ioapic(uint addr)
{
	*io_apic_regsel=addr;
	return *io_apic_win;
}

/*
 * Write IOAPIC
 * 引数 : アドレス、書き込む値
 */
static inline void write_ioapic(uint addr,uint value)
{
	*io_apic_regsel=addr;
	*io_apic_win=value;
}


/*
 * Release interrupt mask
 * prameters : IRQ number
 */
void release_ioapic_mask(uchar irq)
{
	write_ioapic(ioapic_lrt[irq],read_ioapic(ioapic_lrt[irq])&~IOREDTBL_MASK);
}


/*
 * Set interrupt mask
 * prameters : IRQ number
 */
void set_ioapic_mask(uchar irq)
{
	write_ioapic(ioapic_lrt[irq],read_ioapic(ioapic_lrt[irq])|IOREDTBL_MASK);
}


/*
 * GLOBAL
 * Set PCI irq int IO APIC.
 * parameters : PCI_INFO address,interrupt vecter
 * return : 0 or error number
 */
int SetPciIrqInIoapic(PCI_INFO *pci,int vecter)
{
	int pci_irq;


	if ((pci_irq = getPciIrq(pci)) == -1){
		return -EDNOFUNC;
	}
	pci_irq += IO_APIC_PCIIRQ;
	write_ioapic(ioapic_lrt[pci_irq],read_ioapic(ioapic_lrt[pci_irq]) | ioapic_intr_vect[vecter]);

	return 0;
}


/*
 * GLOBAL
 * Release PCI irq interrupt mask.
 * parameters : PCI_INFO address
 */
void releasePciIrqIoapicMask(PCI_INFO *pci)
{
	int pci_irq;


	if ((pci_irq = getPciIrq(pci)) == -1){
		return;
	}
	pci_irq += IO_APIC_PCIIRQ;
	write_ioapic(ioapic_lrt[pci_irq], read_ioapic(ioapic_lrt[pci_irq]) & ~IOREDTBL_MASK);
}


/*
 * Init IO APIC
 * param : cpu number
 */
void init_ioapic(int cpu)
{
	enum{
		IMCR_SEL=0x22,		/* IMCRセレクトポートアドレス */
		IMCR_WRI=0x23		/* IMCR書き込みポートアドレス */
	};

	char *mp_floating;


	/* 割り込みをSymmetricモードに切り替える */
	outb(OCW1S,0xff);				/* picスレーブ割り込みをマスクする */
	outb(OCW1M,0xff);				/* picマスター割り込みをマスクする */
	mp_floating=(char*)MFPS_addres;
	if(mp_floating[FLOAT_IMCR_FLAG]&(1<<7))
	{
		/*PICモードを無効にする */
		outb(IMCR_SEL,0x70);		/* IMCRセレクト */
		outb(IMCR_WRI,1);			/* PICモード無効 */
	}
	else
		/* Virtual Wireモードを無効にする */
		ignoreVirtualWireMode();

	/* IO_APIC_IDの設定 */
	write_ioapic(IO_APIC_ID,cpu<<24);

	/* idtの設定 */
	set_idt(IRQ0_VECT,irq0,IDT_TRAP);
	set_idt(IRQ1_VECT,irq1,IDT_TRAP);
	set_idt(IRQ3_VECT,irq3,IDT_TRAP);
	set_idt(IRQ4_VECT,irq4,IDT_TRAP);
	set_idt(IRQ5_VECT,irq5,IDT_TRAP);
	set_idt(IRQ6_VECT,irq6,IDT_TRAP);
	set_idt(IRQ7_VECT,irq7,IDT_TRAP);
	set_idt(IRQ8_VECT,irq8,IDT_TRAP);
	set_idt(IRQ9_VECT,irq9,IDT_TRAP);
	set_idt(IRQ10_VECT,irq10,IDT_TRAP);
	set_idt(IRQ11_VECT,irq11,IDT_TRAP);
	set_idt(IRQ12_VECT,irq12,IDT_TRAP);
	set_idt(IRQ13_VECT,irq13,IDT_TRAP);
	set_idt(IRQ14_VECT,irq14,IDT_TRAP);
	set_idt(IRQ15_VECT,irq15,IDT_TRAP);

	/* IOテーブルの設定,All maske */
	write_ioapic(IO_APIC_LRT0 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ0_VECT);
	write_ioapic(IO_APIC_HRT0 ,0xff000000);
	write_ioapic(IO_APIC_LRT1 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ1_VECT);
	write_ioapic(IO_APIC_HRT1 ,0xff000000);
	write_ioapic(IO_APIC_LRT2 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ0_VECT);
	write_ioapic(IO_APIC_HRT2 ,0xff000000);
	write_ioapic(IO_APIC_LRT3 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ3_VECT);
	write_ioapic(IO_APIC_HRT3 ,0xff000000);
	write_ioapic(IO_APIC_LRT4 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ4_VECT);
	write_ioapic(IO_APIC_HRT4 ,0xff000000);
	write_ioapic(IO_APIC_LRT5 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ5_VECT);
	write_ioapic(IO_APIC_HRT5 ,0xff000000);
	write_ioapic(IO_APIC_LRT6 ,IOREDTBL_MASK|IOREDTBL_MODE_PHYS |IOREDTBL_DELIV_FIX   |IRQ6_VECT);
	write_ioapic(IO_APIC_HRT6 ,0x00000000);
	write_ioapic(IO_APIC_LRT7 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ7_VECT);
	write_ioapic(IO_APIC_HRT7 ,0xff000000);
	write_ioapic(IO_APIC_LRT8 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ8_VECT);
	write_ioapic(IO_APIC_HRT8 ,0xff000000);
	write_ioapic(IO_APIC_LRT9 ,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ9_VECT);
	write_ioapic(IO_APIC_HRT9 ,0xff000000);
	write_ioapic(IO_APIC_LRT10,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ10_VECT);
	write_ioapic(IO_APIC_HRT10,0xff000000);
	write_ioapic(IO_APIC_LRT11,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ11_VECT);
	write_ioapic(IO_APIC_HRT11,0xff000000);
	write_ioapic(IO_APIC_LRT12,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ12_VECT);
	write_ioapic(IO_APIC_HRT12,0xff000000);
	write_ioapic(IO_APIC_LRT13,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IRQ13_VECT);
	write_ioapic(IO_APIC_HRT13,0xff000000);
	write_ioapic(IO_APIC_LRT14,IOREDTBL_MASK|IOREDTBL_MODE_PHYS |IOREDTBL_DELIV_FIX   |IRQ14_VECT);
	write_ioapic(IO_APIC_HRT14,0x00000000);
	write_ioapic(IO_APIC_LRT15,IOREDTBL_MASK|IOREDTBL_MODE_PHYS |IOREDTBL_DELIV_FIX   |IRQ15_VECT);
	write_ioapic(IO_APIC_HRT15,0x00000000);
	write_ioapic(IO_APIC_LRT16,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IOREDTBL_POLLOW);
	write_ioapic(IO_APIC_HRT16,0xff000000);
	write_ioapic(IO_APIC_LRT17,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IOREDTBL_POLLOW);
	write_ioapic(IO_APIC_HRT17,0xff000000);
	write_ioapic(IO_APIC_LRT18,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IOREDTBL_POLLOW);
	write_ioapic(IO_APIC_HRT18,0xff000000);
	write_ioapic(IO_APIC_LRT19,IOREDTBL_MASK|IOREDTBL_MODE_LOGIC|IOREDTBL_DELIV_LOWPRT|IOREDTBL_POLLOW);
	write_ioapic(IO_APIC_HRT19,0xff000000);
}


/*
 * ハード割り込み用EOI関数
 */
void mp_eoi(uchar i)
{
	local_apic[LOCAL_APIC_EOI]=0;		/* issue EOI */
}


/*
 * IOAPICの割り込みを物理モードにする。
 * parameters : IRQ
 */
void set_physical_intr(int irq)
{
	write_ioapic(ioapic_hrt[irq],0|ioapic_intr_vect[irq]);
}


/*
 * IOAPICの割り込みを論理モードにする。
 * parameters : IRQ
 */
void set_logical_intr(int irq)
{
	write_ioapic(ioapic_hrt[irq],0x900|ioapic_intr_vect[irq]);
}


/*
 * IOAPICの割り込みcpuを特定する。
 * parameters : IRQ,cpu number
 */
void set_intr_cpu(int irq,int cpu)
{
	write_ioapic(ioapic_hrt[irq],cpu<<24);
}


/*
 * IOAPICの割り込みcpuを任意にする。
 * parameters : IRQ,cpu number
 */
void set_intr_anony(int irq)
{
	write_ioapic(ioapic_hrt[irq],0xff000000);
}


/********************************************************************************************************
 *
 * Boot Application prosseser
 *
 *********************************************************************************************************/


/*
 * Boot Application prosseser
 */
static void boot_acpu()
{
	enum{
		WARMBOOT_IP=0x467,	/* Warm bootコードポインタ代入アドレス */
		WARMBOOT_CS=0x469	/* Warm bootコードCS代入アドレス */
	};

	extern void mpstart();				/* setup.S内の初期化コード */
	extern void switch_protect_mode();	/* setup.S内プロテクトモード移行コード */

	ushort *p;
	uint value;


	/*
	 * アプリケーションプロセッサの起動。
	 * まず、2つのINIT_IPIを送ってSTARTUP_IPIが受け付けられるようにする。そしてSTARTUP_IPIを送ってBIOS起動コードにジャンプさせる。
	 * BIOSコードを通ることによって、プロセッサは正しいクロックスピードで動くようになるようだ。そして、WarmBootで設定した
	 * 初期化コードにジャンプさせる。
	 */
	/* Warm bootの設定 */
	write_cmos(0xf,0xa);
	p=(ushort*)WARMBOOT_IP;
	*p=(ushort)switch_protect_mode&0xf;
	p=(ushort*)WARMBOOT_CS;
	*p=(ushort)switch_protect_mode>>4;

	/* まずINIT_IPI Assert modeを送る */
	local_apic[LOCAL_APIC_ICR2]=0xf000000;
	value=local_apic[LOCAL_APIC_ICR1];
	value=(value&0xfff32000)|0xcc500;
	local_apic[LOCAL_APIC_ICR1]=value;
	while(local_apic[LOCAL_APIC_ICR1]&0x1000);

	/* 次にINIT_IPI DeASSERT modeを送る */
	value=local_apic[LOCAL_APIC_ICR1];
	value=(value&0xfff32000)|0xc8500;
	local_apic[LOCAL_APIC_ICR1]=value;
	while(local_apic[LOCAL_APIC_ICR1]&0x1000);

	/* 10ms待つ */
	mili_timer(10);

	/*
	 * 最後にSTARTUP_IPIを送る
	 * 2回送ることになっているが1回で起動するようだ
	 */
	value=local_apic[LOCAL_APIC_ICR1];
	value=(value&0xfff32000)|((uint)mpstart>>12)|0xc0600;
	local_apic[LOCAL_APIC_ICR1]=value;
	while(local_apic[LOCAL_APIC_ICR1]&0x1000);
}


/********************************************************************************************************
 *
 * 初期化
 *
 *********************************************************************************************************/


/*
 * SMPシステムの初期化
 * return : 0 or Error=-1
 */
int init_mp()
{
	char *mp_floating,*mp_header,*mp_table;
	char *p;
	int cpu_num;
	uint64 timeout;


	mp_floating=(char*)MFPS_addres;
	mp_header=(char*)*(char**)(mp_floating+FLOAT_HEADER_ADDRES);
	mp_table=mp_header+HEADER_LENGTH;

	/* cpuをカウントする */
	for(p=mp_table,cpu_num=0;*p==TYPE_PROCESSOR;p+=TABLE_PROCESSOR_LENGTH)
	{
		if(*(p+TABLE_PROCESSOR_FLAG)&1)++cpu_num;
		if(cpu_num>MAX_CPU)return -1;
	}

	++*active_cpu;	/* 稼働cpuのカウント */

	/* Init local apic */
	init_localapic(0);

	/* ハード割り込みのEOI関数を設定する。(アプリケーションcpu起動前) */
	irq_eoi=mp_eoi;

	/* アプリケーションcpu起動 */
	boot_acpu();

	/* pagingで使うpage directory更新割り込みの設定 */
	set_idt(FLASH_PAGEDIR_VECT,flash_pagedir,IDT_TRAP);
	irq_entry[FLASH_PAGEDIR]=flash_pagedir_handler;

	/* Init IO apic、すべてのcpuの起動後に発行 */
	timeout=rdtsc()+clock_1m*1000;
	while(*active_cpu!=cpu_num)
		if(rdtsc()>timeout)
		{
			printk("Booting application cpu isn't complete!\n");
			break;
		}
	init_ioapic(cpu_num);

	/* ハード割り込みマスク解除関数,マスク関数を設定する */
	release_irq_mask=release_ioapic_mask;
	set_irq_mask=set_ioapic_mask;

	return 0;
}


/*
 * Application cpuを設定する
 */
void init_application_cpu()
{
	static int spingate = 0;
	int cpu;


	/* fpuの初期化 */
	asm("fninit");

	enter_spinlock(&spingate);
	{
		cpu = *active_cpu;

		/* TSSディスクリプタをロードする */
		set_tss(TSS_DES+cpu*8);

		/* Init parameters relatibe to task */
		init_cputask(cpu);

		/* cpu clockの計測 */
		printk("cpu%d clock = %d HZ\n",cpu,count_cpu_clock());

		++*active_cpu;
	}
	exit_spinlock(&spingate);

	/* LOCAL_APICの初期化 */
	init_localapic(cpu);

	/* アイドルプロセスを生成する。 */
	if (set_idle_proc(cpu) == -1)
	{
		printk("Fail set_idle_proc! cpu=%d.\n",cpu);
		idle();
	}

	/* タスキング開始 */
	start_tasking();
}

/*********************************************************************************/
void test_mp()
{
	printk("IRQ14REG LOW=%x HIGH=%x\n",read_ioapic(IO_APIC_LRT14),read_ioapic(IO_APIC_HRT14));
}
/**********************************************************************************/
