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


#include"types.h"
#include"config.h"
#include"lib.h"
#include"proc.h"
#include"interrupt.h"
#include"mm.h"
#include"time.h"
#include"lock.h"
#include"time.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_FALG=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 */

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

	/* IO APIC register */
	IO_APIC_REGSEL=	0xfec00000,
	IO_APIC_WIN=	0xfec00010,
	IO_APIC_MASK=	0x10000,
	IO_APIC_ID=		0,
	IO_APIC_VER=	1
};


uint ioapic_lrt[16]=									/* IRQごとの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
};
uint ioapic_hrt[16]=									/* IRQごとの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
};
uchar ioapic_intr_vect[16]=
{
	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
};
uint MFPS_addres;										/* MFPSアドレス、setup.Sで代入 */
int activ_cpu=0;										/* Numbers of active cpu */
uint volatile *local_apic=(uint*)LOCAL_APIC_BASE;		/* LOCAL APIC base address */
uint volatile *io_apic_regsel=(uint*)IO_APIC_REGSEL;	/* IOAPIC REGSEL address */
uint volatile *io_apic_win=(uint*)IO_APIC_WIN;			/* IOAPIC WIN address */
static uint task_count;									/* 1タスク稼動時間のAPICタイマーカウント値 */

extern void start_tasking();
static void mp_eoi(uchar);
static void release_ioapic_mask(uchar);
static void set_ioapic_mask(uchar);
static void init_localapic(int);
static void init_ioapic(int);
static void boot_acpu();
static uint count_apictimer();
static void wr_apic_msr(uint);
static int apictimer_handler();
static int flash_pagedir_handler(INTERRUPT_FRAME);


/*
 * APIC TIMERの１sのカウント数を測定する
 * return : １sのカウント数
 */
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のカウント回数 */
}


/*
 * 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);		/* 伝達ステータスの確認 */
}


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

		cpu=get_current_cpu();
		*(p+cpu*2)=(++value[cpu]%10)+'0';
	}
/********************************************/
	local_apic[LOCAL_APIC_ICNT]=task_count;	/* restart APIC TIMER */

	return 1;
}


/*
 * Page directory flash handler
 * returns : Task switch on
 */
int flash_pagedir_handler(INTERRUPT_FRAME frame)
{
	flash_tbl(cputask[get_current_cpu()].current_task->pagedir);
	local_apic[LOCAL_APIC_EOI]=0;		/* issue EOI */

	return 1;
}


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


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


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


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


/*
 * Boot Application prosseser
 */
void boot_acpu()
{
	enum{
		CMOS_SEL=0x70,		/* CMOSセレクトポートアドレス */
		CMOS_RW=0x71,		/* CMOS読み書きポートアドレス */
		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の設定 */
	outb(CMOS_SEL,0xf);
	outb(CMOS_RW,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_FALG)&1)++cpu_num;
		if(cpu_num>MAX_CPU)return -1;
	}

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

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

	/* Boot application 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(activ_cpu!=cpu_num)
		if(rdtsc()>timeout)
		{
			printk("Booting application cpu isn't complete!\n");
			break;
		}
	init_ioapic(cpu_num);

	/* ハード割り込みのEOI関数を設定する */
	irq_eoi=mp_eoi;

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

	return 0;
}


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

	uint *pagedir;
	int cpu;


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

	asm volatile(				/* spin lock */
			"movl	$1,%%edx\n"
		"1:	xchgl	%%edx,(%%eax)\n"
			"orl	%%edx,%%edx\n"
			"jnz	1b"
		::"a"(&spingate)
	);
	{
		cpu=activ_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());

		++activ_cpu;
	}
	spingate=0;

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

	/* ページングの設定 */
	if((pagedir=init_idle_page())==NULL)
	{
		printk("Fail init_idle_page!\n");
		idle();
	}

	if(set_idle_proc(cpu,pagedir)==-1)
	{
		printk("Fail set_idle_proc! cpu=%d.\n",cpu);
		idle();
	}

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


/*
 * Init LOCAL APIC
 * Require atomic handlig
 * parameters : Cpu number
 */
void init_localapic(int 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;				/* タイマー始動 */
}


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


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


/*
 * 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モードを無効にする */
		local_apic[LOCAL_APIC_LNT0]=0x10000;		/* LINT0をマスク */
		local_apic[LOCAL_APIC_LNT1]=0x10000;		/* LINT1をマスク */
	}

	/* 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,0x10900|IRQ0_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT0,0xff000000);
	write_ioapic(IO_APIC_LRT1,0x10900|IRQ1_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT1,0xff000000);
	write_ioapic(IO_APIC_LRT2,0x10900|IRQ0_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT2,0xff000000);
	write_ioapic(IO_APIC_LRT3,0x10900|IRQ3_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT3,0xff000000);
	write_ioapic(IO_APIC_LRT4,0x10900|IRQ4_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT4,0xff000000);
	write_ioapic(IO_APIC_LRT5,0x10900|IRQ5_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT5,0xff000000);
	write_ioapic(IO_APIC_LRT6,0x10000|IRQ6_VECT);	/* Physical Mode */
	write_ioapic(IO_APIC_HRT6,0x00000000);
	write_ioapic(IO_APIC_LRT7,0x10900|IRQ7_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT7,0xff000000);
	write_ioapic(IO_APIC_LRT8,0x10900|IRQ8_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT8,0xff000000);
	write_ioapic(IO_APIC_LRT9,0x10900|IRQ9_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT9,0xff000000);
	write_ioapic(IO_APIC_LRT10,0x10900|IRQ10_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT10,0xff000000);
	write_ioapic(IO_APIC_LRT11,0x10900|IRQ11_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT11,0xff000000);
	write_ioapic(IO_APIC_LRT12,0x10900|IRQ12_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT12,0xff000000);
	write_ioapic(IO_APIC_LRT13,0x10900|IRQ13_VECT);	/* Logcal mode,Lowest priority */
	write_ioapic(IO_APIC_HRT13,0xff000000);
	write_ioapic(IO_APIC_LRT14,0x10000|IRQ14_VECT);	/* Physical Mode */
	write_ioapic(IO_APIC_HRT14,0x00000000);
	write_ioapic(IO_APIC_LRT15,0x10000|IRQ15_VECT);	/* Physical Mode */
	write_ioapic(IO_APIC_HRT15,0x00000000);
}
