From: John Hodge <tpg@mutabah.net>
Date: Sun, 18 Jul 2010 04:19:23 +0000 (+0800)
Subject: More MP work (now APs start and use the LAPIC timer)
X-Git-Tag: rel0.06~98
X-Git-Url: https://git.ucc.asn.au/?p=tpg%2Facess2.git;a=commitdiff_plain;h=83612bb37fbd8e84d90ecb9e6a7157aadd1e2175

More MP work (now APs start and use the LAPIC timer)
---

diff --git a/Kernel/arch/x86/desctab.asm b/Kernel/arch/x86/desctab.asm
index 1513984a..b80c355c 100644
--- a/Kernel/arch/x86/desctab.asm
+++ b/Kernel/arch/x86/desctab.asm
@@ -58,6 +58,7 @@ Desctab_Install:
 	mov	WORD [gIDT + %1*8], ax
 	shr eax, 16
 	mov	WORD [gIDT + %1*8+6], ax
+	; Enable
 	mov	ax, WORD [gIDT + %1*8 + 4]
 	or ax, 0x8000
 	mov	WORD [gIDT + %1*8 + 4], ax
@@ -77,7 +78,8 @@ Desctab_Install:
 	SETUSER	0xAC
 	
 	%if USE_MP
-	SETISR	239
+	SETISR	0xEE	; 0xEE Timer
+	SETISR	0xEF	; 0xEF Spurious Interrupt
 	%endif
 	
 	%assign	i	0xF0
@@ -127,7 +129,7 @@ Desctab_Install:
 %macro	ISR_ERRNO	1
 [global Isr%1]
 Isr%1:
-	;xchg bx, bx
+	xchg bx, bx
 	push	%1
 	jmp	ErrorCommon
 %endmacro
@@ -191,24 +193,33 @@ ISR_NOERR	31; 31: Reserved
 
 DEF_SYSCALL	0xAC	; Acess System Call
 
-; AP's Timer Interrupt
 %if USE_MP
-[global Isr239]
+[global Isr0xEE]
 [extern SchedulerBase]
-Isr239:
+; AP's Timer Interrupt
+Isr0xEE:
 	push 0
+	xchg bx, bx	; MAGIC BREAK
 	jmp SchedulerBase
+; Spurious Interrupt
+[global Isr0xEF]
+Isr0xEF:
+	xchg bx, bx	; MAGIC BREAK
+	iret
 %endif
 
 ; IRQs
 ; - Timer
 [global Isr240]
+[global Isr240.jmp]
 [extern SchedulerBase]
 [extern SetAPICTimerCount]
 Isr240:
 	push 0
+	;xchg bx, bx	; MAGIC BREAK
+Isr240.jmp:
 	%if USE_MP
-	jmp SetAPICTimerCount
+	jmp SetAPICTimerCount	; This is reset once the bus speed has been calculated
 	%else
 	jmp SchedulerBase
 	%endif
@@ -224,7 +235,7 @@ Isr240:
 ; ---------------------
 [extern ErrorHandler]
 ErrorCommon:
-	;xchg bx, bx
+	;xchg bx, bx	; MAGIC BREAK
 	pusha
 	push ds
 	push es
diff --git a/Kernel/arch/x86/lib.c b/Kernel/arch/x86/lib.c
index 6522574f..c499c7a8 100644
--- a/Kernel/arch/x86/lib.c
+++ b/Kernel/arch/x86/lib.c
@@ -121,10 +121,15 @@ void *memcpyd(void *Dest, const void *Src, size_t Num)
  */
 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 {
-	Uint64	ret = 0;
+	Uint64	P[2];
+	Uint64	q = 0;
+	 int	i;
 	
-	if(Den == 0)	__asm__ __volatile__ ("int $0x0");	// Call Div by Zero Error
-	if(Den == 1)	return Num;	// Speed Hacks
+	if(Den == 0)	__asm__ __volatile__ ("int $0x0");
+	// Common speedups
+	if(Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF)
+		return (Uint32)Num / (Uint32)Den;
+	if(Den == 1)	return Num;
 	if(Den == 2)	return Num >> 1;	// Speed Hacks
 	if(Den == 4)	return Num >> 2;	// Speed Hacks
 	if(Den == 8)	return Num >> 3;	// Speed Hacks
@@ -133,19 +138,33 @@ Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 	if(Den == 1024)	return Num >> 10;	// Speed Hacks
 	if(Den == 2048)	return Num >> 11;	// Speed Hacks
 	if(Den == 4096)	return Num >> 12;
+	if(Num < Den)	return 0;
+	if(Num < Den*2)	return 1;
+	if(Num == Den*2)	return 2;
 	
-	if(Num >> 32 == 0 && Den >> 32 == 0)
-		return (Uint32)Num / (Uint32)Den;
-	
-	//Log("__udivdi3: (Num={0x%x:%x}, Den={0x%x:%x})",
-	//	Num>>32, Num&0xFFFFFFFF,
-	//	Den>>32, Den&0xFFFFFFFF);
-	
-	while(Num > Den) {
-		ret ++;
-		Num -= Den;
+	// Restoring division, from wikipedia
+	// http://en.wikipedia.org/wiki/Division_(digital)
+	P[0] = Num;	P[1] = 0;
+	for( i = 64; i--; )
+	{
+		// P <<= 1;
+		P[1] = (P[1] << 1) | (P[0] >> 63);
+		P[0] = P[0] << 1;
+		
+		// P -= Den << 64
+		P[1] -= Den;
+		
+		// P >= 0
+		if( !(P[1] & (1ULL<<63)) ) {
+			q |= (Uint64)1 << (63-i);
+		}
+		else {
+			//q |= 0 << (63-i);
+			P[1] += Den;
+		}
 	}
-	return ret;
+	
+	return q;
 }
 
 /**
@@ -168,9 +187,7 @@ Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 	if(Num >> 32 == 0 && Den >> 32 == 0)
 		return (Uint32)Num % (Uint32)Den;
 	
-	while(Num > Den)
-		Num -= Den;
-	return Num;
+	return Num - __udivdi3(Num, Den) * Den;
 }
 
 Uint16 LittleEndian16(Uint16 Val)
diff --git a/Kernel/arch/x86/proc.asm b/Kernel/arch/x86/proc.asm
index bcb0f586..41623bdd 100644
--- a/Kernel/arch/x86/proc.asm
+++ b/Kernel/arch/x86/proc.asm
@@ -8,9 +8,10 @@ KERNEL_BASE	equ 0xC0000000
 KSTACK_USERSTATE_SIZE	equ	(4+8+1+5)*4	; SRegs, GPRegs, CPU, IRET
 
 [section .text]
+%if USE_MP
 [extern giMP_TimerCount]
 [extern gpMP_LocalAPIC]
-[extern Isr240]
+[extern Isr240.jmp]
 [global SetAPICTimerCount]
 SetAPICTimerCount:
 	pusha
@@ -27,7 +28,7 @@ SetAPICTimerCount:
 	
 	mov eax, [gpMP_LocalAPIC]
 	mov ecx, [eax+0x320]
-	test ecx, 0x0001000
+	test ecx, 0x00010000
 	jz .setTime
 	mov DWORD [eax+0x380], 0xFFFFFFFF	; Set Initial Count
 	mov DWORD [eax+0x320], 0x000000F0	; Enable the timer on IVT#0xEF (One Shot)
@@ -39,22 +40,28 @@ SetAPICTimerCount:
 	sub ecx, [eax+0x390]
 	mov DWORD [giMP_TimerCount], ecx
 	; Disable APIC Timer
-	mov DWORD [eax+0x320], 0x00010000
+	mov DWORD [eax+0x320], 0x000100EF
+	mov DWORD [eax+0x380], 0
 
 	; Update Timer IRQ to the IRQ code
 	mov eax, SchedulerBase
-	sub eax, Isr240+5+5+1
-	mov DWORD [Isr240+5+5+1], eax
+	sub eax, Isr240.jmp+5
+	mov DWORD [Isr240.jmp+1], eax
 
+	;xchg bx, bx	; MAGIC BREAK
 .ret:
 	mov dx, 0x20
 	mov al, 0x20
 	out dx, al		; ACK IRQ
+	pop gs
+	pop fs
+	pop es
+	pop ds
 	popa
 	add esp, 4	; CPU ID
 	; No Error code / int num
 	iret
-
+%endif
 ; --------------
 ; Task Scheduler
 ; --------------
@@ -73,21 +80,39 @@ SchedulerBase:
 	mov fs, ax
 	mov gs, ax
 	
+	%if USE_MP
 	call GetCPUNum
+	mov ebx, eax
 	push eax	; Push as argument
+	%else
+	push 0
+	%endif
 	
 	call Proc_Scheduler
 	
 	add esp, 4	; Remove Argument
+
+	%if USE_MP
+	test ebx, ebx
+	jnz .sendEOI
+	%endif
+	
+	mov dx, 0x20
+	mov al, 0x20
+	out dx, al		; ACK IRQ
+	%if USE_MP
+	jmp .ret
 	
+.sendEOI:
+	mov eax, DWORD [gpMP_LocalAPIC]
+	mov DWORD [eax+0x0B0], 1
+	%endif
+.ret:
 	pop gs
 	pop fs
 	pop es
 	pop ds
-
-	mov dx, 0x20
-	mov al, 0x20
-	out dx, al		; ACK IRQ
+	
 	popa
 	add esp, 4	; CPU ID
 	; No Error code / int num
diff --git a/Kernel/arch/x86/proc.c b/Kernel/arch/x86/proc.c
index 065e6de3..0708540e 100644
--- a/Kernel/arch/x86/proc.c
+++ b/Kernel/arch/x86/proc.c
@@ -17,6 +17,7 @@
 // === CONSTANTS ===
 #define	SWITCH_MAGIC	0xFFFACE55	// There is no code in this area
 // Base is 1193182
+#define TIMER_BASE	1193182
 #define TIMER_DIVISOR	11931	//~100Hz
 
 // === TYPES ===
@@ -73,7 +74,7 @@ void	Proc_Scheduler(int CPU);
 #if USE_MP
 volatile int	giNumInitingCPUs = 0;
 tMPInfo	*gMPFloatPtr = NULL;
-Uint32	giMP_TimerCount;	// Start Count for Local APIC Timer
+volatile Uint32	giMP_TimerCount;	// Start Count for Local APIC Timer
 tAPIC	*gpMP_LocalAPIC = NULL;
 Uint8	gaAPIC_to_CPU[256] = {0};
 tCPU	gaCPUs[MAX_CPUS];
@@ -88,13 +89,13 @@ Uint32	*gPML4s[4] = NULL;
 tTSS	*gTSSs = NULL;	// Pointer to TSS array
 tTSS	gTSS0 = {0};
 // --- Error Recovery ---
-char	gaDoubleFaultStack[1024];
+char	gaDoubleFaultStack[1024] __attribute__ ((section(".padata")));
 tTSS	gDoubleFault_TSS = {
-	.ESP0 = (Uint)&gaDoubleFaultStack[1023],
+	.ESP0 = (Uint)&gaDoubleFaultStack[1024],
 	.SS0 = 0x10,
 	.CR3 = (Uint)gaInitPageDir - KERNEL_BASE,
 	.EIP = (Uint)Isr8,
-	.ESP = (Uint)&gaDoubleFaultStack[1023],
+	.ESP = (Uint)&gaDoubleFaultStack[1024],
 	.CS = 0x08,	.SS = 0x10,
 	.DS = 0x10,	.ES = 0x10,
 	.FS = 0x10,	.GS = 0x10,
@@ -228,6 +229,8 @@ void ArchThreads_Init(void)
 				}
 				
 				break;
+			
+			#if DUMP_MP_TABLES
 			case 1:	// Bus
 				entSize = 8;
 				Log("%i: Bus", i);
@@ -265,6 +268,7 @@ void ArchThreads_Init(void)
 			default:
 				Log("%i: Unknown (%i)", i, ents->Type);
 				break;
+			#endif
 			}
 			ents = (void*)( (Uint)ents + entSize );
 		}
@@ -286,6 +290,7 @@ void ArchThreads_Init(void)
 	MM_FinishVirtualInit();
 	#endif
 	
+	#if 0
 	// Initialise Double Fault TSS
 	gGDT[5].BaseLow = (Uint)&gDoubleFault_TSS & 0xFFFF;
 	gGDT[5].BaseMid = (Uint)&gDoubleFault_TSS >> 16;
@@ -296,19 +301,34 @@ void ArchThreads_Init(void)
 	gIDT[8].CS = 5<<3;
 	gIDT[8].Flags = 0x8500;
 	gIDT[8].OffsetHi = 0;
+	#endif
 	
 	// Set timer frequency
 	outb(0x43, 0x34);	// Set Channel 0, Low/High, Rate Generator
 	outb(0x40, TIMER_DIVISOR&0xFF);	// Low Byte of Divisor
 	outb(0x40, (TIMER_DIVISOR>>8)&0xFF);	// High Byte
+	
+	#if USE_MP
 	// Get the count setting for APIC timer
 	Log("Determining APIC Count");
 	__asm__ __volatile__ ("sti");
 	while( giMP_TimerCount == 0 )	__asm__ __volatile__ ("hlt");
 	__asm__ __volatile__ ("cli");
-	Log("APIC Count %i\n", giMP_TimerCount);
+	Log("APIC Count %i", giMP_TimerCount);
+	{
+		Uint64	freq = giMP_TimerCount;
+		freq /= TIMER_DIVISOR;
+		freq *= TIMER_BASE;
+		if( (freq /= 1000) < 2*1000)
+			Log("Bus Frequency %i KHz", freq);
+		else if( (freq /= 1000) < 2*1000)
+			Log("Bus Frequency %i MHz", freq);
+		else if( (freq /= 1000) < 2*1000)
+			Log("Bus Frequency %i GHz", freq);
+		else
+			Log("Bus Frequency %i THz", freq);
+	}
 	
-	#if USE_MP
 	// Initialise Normal TSS(s)
 	for(pos=0;pos<giNumCPUs;pos++)
 	{
@@ -333,7 +353,10 @@ void ArchThreads_Init(void)
 	}
 	
 	Log("Waiting for APs to come up\n");
+	//__asm__ __volatile__ ("xchg %bx, %bx");
+	__asm__ __volatile__ ("sti");
 	while( giNumInitingCPUs )	__asm__ __volatile__ ("hlt");
+	__asm__ __volatile__ ("cli");
 	MM_FinishVirtualInit();
 	//Panic("Uh oh... MP Table Parsing is unimplemented\n");
 	#endif
diff --git a/Kernel/arch/x86/start.asm b/Kernel/arch/x86/start.asm
index 74b5c710..f79c647e 100644
--- a/Kernel/arch/x86/start.asm
+++ b/Kernel/arch/x86/start.asm
@@ -4,6 +4,7 @@
 [bits 32]
 
 KERNEL_BASE	equ 0xC0000000
+%define MAX_CPUS	16
 
 [extern __load_addr]
 [extern __bss_start]
@@ -97,6 +98,8 @@ start:
 [extern gaAPIC_to_CPU]
 [extern gaCPUs]
 [extern giNumInitingCPUs]
+[extern MM_NewKStack]
+
 lGDTPtr:	; Local GDT Pointer
 	dw	3*8-1
 	dd	gGDT-KERNEL_BASE
@@ -141,24 +144,40 @@ APStartup:
 	lgdt [gGDTPtr]
 	lidt [gIDTPtr]
 
-	mov eax, [gpMP_LocalAPIC]
-	mov ecx, [eax+0x20]	; Read ID
-	shr ecx, 24
+	mov ebp, [gpMP_LocalAPIC]
+	mov esi, [eax+0x20]	; Read ID
+	shr esi, 24
 	;xchg bx, bx	; MAGIC BREAK
 	; CL is now local APIC ID
-	mov cl, BYTE [gaAPIC_to_CPU+ecx]
+	mov cl, BYTE [gaAPIC_to_CPU+esi]
 	; CL is now the CPU ID
-	mov BYTE [gaCPUs+ecx*8+1], 1
+	mov BYTE [gaCPUs+esi*8+1], 1
 	; Decrement the remaining CPU count
 	dec DWORD [giNumInitingCPUs]
+	
+	; Create a stack
+	lea edx, [esi+1]
+	shl edx, 5+2	; *32 *4
+	lea esp, [gInitAPStacks+edx]
+	call MM_NewKStack
+	mov esp, eax
+	
 	; Set TSS
-	shl cx, 3
-	add cx, 0x30
+	lea ecx, [esi*8+0x30]
 	ltr cx
-	; Enable Local APIC Timer
-	mov ecx, [giMP_TimerCount]
-	mov [eax+0x380], ecx	; Set Initial Count
-	mov DWORD [eax+0x320], 0x000200EF	; Enable the timer on IVT#0xEF
+	
+	;xchg bx, bx	; MAGIC_BREAK
+	; Enable Local APIC
+	mov DWORD [ebp+0x0F0], 0x000001EF	; Spurious Interrupt Vector (0xEF)
+	mov ecx, DWORD [giMP_TimerCount]
+	mov DWORD [ebp+0x380], ecx	; Set Initial Count
+	mov DWORD [ebp+0x320], 0x000200EE	; Enable timer on IVT#0xEE
+	mov DWORD [ebp+0x330], 0x000100E0	; ##Enable thermal sensor on IVT#0xE0
+	mov DWORD [ebp+0x340], 0x000100D0	; ##Enable performance counters on IVT#0xD0
+	mov DWORD [ebp+0x350], 0x000100D1	; ##Enable LINT0 on IVT#0xD1
+	mov DWORD [ebp+0x360], 0x000100D2	; ##Enable LINT1 on IVT#0xD2
+	mov DWORD [ebp+0x370], 0x000100E1	; ##Enable Error on IVT#0xE1
+	mov DWORD [ebp+0x0B0], 0	; Send an EOI (just in case)
 	
 	; CPU is now marked as initialised
 	sti
@@ -213,4 +232,6 @@ gaInitPageTable:
 ALIGN 0x1000
 	times 1024	dd	0
 Kernel_Stack_Top:
+gInitAPStacks:
+	times 32*MAX_CPUS	dd	0
 	
diff --git a/Modules/Storage/FDD/fdd.c b/Modules/Storage/FDD/fdd.c
index 2784b3c5..cc75bfcd 100644
--- a/Modules/Storage/FDD/fdd.c
+++ b/Modules/Storage/FDD/fdd.c
@@ -217,7 +217,7 @@ void FDD_UnloadModule()
 	LOCK(&glFDD);
 	for(i=0;i<4;i++) {
 		Time_RemoveTimer(gFDD_Devices[i].timer);
-		FDD_int_StopMotor((void*)i);
+		FDD_int_StopMotor((void *)(Uint)i);
 	}
 	RELEASE(&glFDD);
 	//IRQ_Clear(6);
@@ -777,7 +777,7 @@ void FDD_Reset(int id)
  */
 void FDD_int_TimerCallback(void *Arg)
 {
-	 int	disk = (int)Arg;
+	 int	disk = (Uint)Arg;
 	ENTER("iarg", disk);
 	if(gFDD_Devices[disk].motorState == 1)
 		gFDD_Devices[disk].motorState = 2;
@@ -806,7 +806,7 @@ void FDD_int_StartMotor(int disk)
  */
 void FDD_int_StopMotor(void *Arg)
 {
-	Uint8	state, disk = (int)Arg;
+	Uint8	state, disk = (Uint)Arg;
 	if( IS_LOCKED(&glFDD) )	return ;
 	ENTER("iDisk", disk);