From a20cfd571f504f5c7f2d29516442a12c200441d5 Mon Sep 17 00:00:00 2001
From: John Hodge <tpg@mutabah.net>
Date: Sat, 14 Jan 2012 12:41:49 +0800
Subject: [PATCH] Kernel/x86 - Debugging SMP bugs, lead to some fixes and
 cleanups

---
 Kernel/Makefile             |  2 +-
 Kernel/arch/x86/desctab.asm | 33 -----------------
 Kernel/arch/x86/kpanic.c    |  2 +-
 Kernel/arch/x86/lib.c       | 70 ++++++++++---------------------------
 Kernel/arch/x86/main.c      |  3 +-
 Kernel/arch/x86/mm_virt.c   |  7 ++--
 Kernel/arch/x86/proc.asm    |  5 ++-
 Kernel/arch/x86/proc.c      | 36 +++++++++++++------
 Kernel/arch/x86/start.asm   | 41 ++++++++++++++++++----
 Kernel/arch/x86/vm8086.c    | 45 +++++++++++++-----------
 Kernel/debug.c              |  2 +-
 Kernel/heap.c               |  1 +
 Kernel/lib.c                |  4 ++-
 13 files changed, 119 insertions(+), 132 deletions(-)

diff --git a/Kernel/Makefile b/Kernel/Makefile
index c3fbc563..580d72e9 100644
--- a/Kernel/Makefile
+++ b/Kernel/Makefile
@@ -23,7 +23,7 @@ endif
 
 ASFLAGS         += -D ARCHDIR_IS_$(ARCHDIR)=1 -D PLATFORM_is_$(PLATFORM)=1
 CPPFLAGS	+= -I./include -I./arch/$(ARCHDIR)/include -D_MODULE_NAME_=\"Kernel\"
-CPPFLAGS	+= -D ARCH=$(ARCH) -D ARCHDIR=$(ARCHDIR) -D ARCHDIR_IS_$(ARCHDIR)=1 -D PLATFORM_is_$(PLATFORM)=1
+CPPFLAGS	+= -D ARCH=$(ARCH) -D ARCHDIR=$(ARCHDIR) -D PLATFORM=\"$(PLATFORM)\" -D ARCHDIR_IS_$(ARCHDIR)=1 -D PLATFORM_is_$(PLATFORM)=1
 CPPFLAGS	+= -D KERNEL_VERSION=$(KERNEL_VERSION)
 CFLAGS  	+= -Wall -fno-stack-protector -Wstrict-prototypes -g
 CFLAGS  	+= -Wshadow -Wpointer-arith -Wcast-align -Wwrite-strings -Wmissing-prototypes -Wmissing-declarations -Wredundant-decls -Wnested-externs -Winline -Wuninitialized
diff --git a/Kernel/arch/x86/desctab.asm b/Kernel/arch/x86/desctab.asm
index 0c19fc0a..8c7d320e 100644
--- a/Kernel/arch/x86/desctab.asm
+++ b/Kernel/arch/x86/desctab.asm
@@ -3,30 +3,8 @@
 ; desctab.asm
 [BITS 32]
 
-%if USE_MP
-MAX_CPUS	equ	8
-%else
-MAX_CPUS	equ	1
-%endif
-GDT_SIZE	equ	(1+2*2+1+MAX_CPUS)*8
 
 [section .data]
-; GDT
-[global gGDT]
-gGDT:
-	; PL0 - Kernel
-	; PL3 - User
-	dd 0x00000000, 0x00000000	; 00 NULL Entry
-	dd 0x0000FFFF, 0x00CF9A00	; 08 PL0 Code
-	dd 0x0000FFFF, 0x00CF9200	; 10 PL0 Data
-	dd 0x0000FFFF, 0x00CFFA00	; 18 PL3 Code
-	dd 0x0000FFFF, 0x00CFF200	; 20 PL3 Data
-	dd 26*4-1, 0x00408900	; 28 Double Fault TSS
-	times MAX_CPUS	dd 26*4-1, 0x00408900	; 30+ TSSes
-[global gGDTPtr]
-gGDTPtr:
-	dw	GDT_SIZE-1
-	dd	gGDT
 ; IDT
 ALIGN 8
 [global gIDT]
@@ -42,17 +20,6 @@ gIDTPtr:
 
 [global Desctab_Install]
 Desctab_Install:
-	; Set GDT
-	lgdt [gGDTPtr]
-	mov ax, 0x10	; PL0 Data
-	mov ss, ax
-	mov ds, ax
-	mov es, ax
-	mov gs, ax
-	mov fs, ax
-	jmp 0x08:.pl0code
-.pl0code:
-
 	; Set up IDT
 	; Helper Macros
 	; - Set an IDT entry to an ISR
diff --git a/Kernel/arch/x86/kpanic.c b/Kernel/arch/x86/kpanic.c
index 6a7b9b0f..9300c69e 100644
--- a/Kernel/arch/x86/kpanic.c
+++ b/Kernel/arch/x86/kpanic.c
@@ -105,7 +105,7 @@ void KernelPanic_SetMode(void)
 	for( i = 0; i < giNumCPUs; i ++ )
 	{
 		if(i == GetCPUNum())	continue ;
-		FB[i] = BGC|'A'+i;
+		FB[i] = BGC|('A'+i);
 		MP_SendIPIVector(i, 0xED);
 	}
 	#endif
diff --git a/Kernel/arch/x86/lib.c b/Kernel/arch/x86/lib.c
index 3858eff3..ecc575c1 100644
--- a/Kernel/arch/x86/lib.c
+++ b/Kernel/arch/x86/lib.c
@@ -22,6 +22,7 @@ extern struct sShortSpinlock	glThreadListLock;
 extern int	GetCPUNum(void);
 
 // === PROTOTYPES ==
+Uint64	__divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem);
 Uint64	__udivdi3(Uint64 Num, Uint64 Den);
 Uint64	__umoddi3(Uint64 Num, Uint64 Den);
 
@@ -330,9 +331,12 @@ void *memcpyd(void *Dest, const void *Src, size_t Num)
 	return Dest;
 }
 
+#include "../helpers.h"
+
+DEF_DIVMOD(64);
+
 Uint64 DivMod64U(Uint64 Num, Uint64 Div, Uint64 *Rem)
 {
-	Uint64	ret;
 	if( Div < 0x100000000ULL && Num < 0xFFFFFFFF * Div ) {
 		Uint32	rem, ret_32;
 		__asm__ __volatile__(
@@ -344,9 +348,7 @@ Uint64 DivMod64U(Uint64 Num, Uint64 Div, Uint64 *Rem)
 		return ret_32;
 	}
 
-	ret = __udivdi3(Num, Div);
-	if(Rem)	*Rem = __umoddi3(Num, Div);
-	return ret;
+	return __divmod64(Num, Div, Rem);
 }
 
 /**
@@ -355,11 +357,10 @@ Uint64 DivMod64U(Uint64 Num, Uint64 Div, Uint64 *Rem)
  */
 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 {
-	Uint64	P[2];
-	Uint64	q = 0;
-	 int	i;
-	
-	if(Den == 0)	__asm__ __volatile__ ("int $0x0");
+	if(Den == 0) {
+		__asm__ __volatile__ ("int $0x0");
+		return -1;
+	}
 	// Common speedups
 	if(Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF)
 		return (Uint32)Num / (Uint32)Den;
@@ -375,46 +376,8 @@ Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 	if(Num < Den)	return 0;
 	if(Num < Den*2)	return 1;
 	if(Num == Den*2)	return 2;
-	
-	#if 1
-	i = 0;	// Shut up
-	P[0] = Num;
-	P[1] = Den;
-	__asm__ __volatile__ (
-		"fildq %2\n\t"	// Num
-		"fildq %1\n\t"	// Den
-		"fdivp\n\t"
-		"fistpq %0"
-		: "=m" (q)
-		: "m" (P[0]), "m" (P[1])
-		);
-		
-	//Log("%llx / %llx = %llx\n", Num, Den, q);
-	#else
-	// 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;
-		}
-	}
-	#endif
-	
-	return q;
+
+	return __divmod64(Num, Den, NULL);
 }
 
 /**
@@ -423,7 +386,11 @@ Uint64 __udivdi3(Uint64 Num, Uint64 Den)
  */
 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 {
-	if(Den == 0)	__asm__ __volatile__ ("int $0x0");	// Call Div by Zero Error
+	Uint64	ret = 0;
+	if(Den == 0) {
+		__asm__ __volatile__ ("int $0x0");	// Call Div by Zero Error
+		return -1;
+	}
 	if(Den == 1)	return 0;	// Speed Hacks
 	if(Den == 2)	return Num & 1;	// Speed Hacks
 	if(Den == 4)	return Num & 3;	// Speed Hacks
@@ -437,7 +404,8 @@ Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 	if(Num >> 32 == 0 && Den >> 32 == 0)
 		return (Uint32)Num % (Uint32)Den;
 	
-	return Num - __udivdi3(Num, Den) * Den;
+	__divmod64(Num, Den, &ret);
+	return ret;
 }
 
 
diff --git a/Kernel/arch/x86/main.c b/Kernel/arch/x86/main.c
index d7c06e79..2106b6af 100644
--- a/Kernel/arch/x86/main.c
+++ b/Kernel/arch/x86/main.c
@@ -45,7 +45,7 @@ int kmain(Uint MbMagic, void *MbInfoPtr)
 	tMBoot_Module	*mods;
 	tMBoot_Info	*mbInfo;
 
-	LogF("Acess2 x86 v"EXPAND_STR(KERNEL_VERSION)"\n");
+	LogF("Acess2 x86-"PLATFORM" v"EXPAND_STR(KERNEL_VERSION)"\n");
 	LogF(" Build %i, Git Hash %s\n", BUILD_NUM, gsGitHash);
 	
 	Log("MbMagic = %08x, MbInfoPtr = %p", MbMagic, MbInfoPtr);
@@ -81,7 +81,6 @@ int kmain(Uint MbMagic, void *MbInfoPtr)
 	MM_InstallVirtual();	// Clean up virtual address space
 	Heap_Install();		// Create initial heap
 	
-	//Log_Log("Arch", "Starting Multitasking...");
 	// Start Multitasking
 	Threads_Init();
 	
diff --git a/Kernel/arch/x86/mm_virt.c b/Kernel/arch/x86/mm_virt.c
index 73e19d3b..137240c9 100644
--- a/Kernel/arch/x86/mm_virt.c
+++ b/Kernel/arch/x86/mm_virt.c
@@ -212,14 +212,15 @@ void MM_PageFault(tVAddr Addr, Uint ErrorCode, tRegs *Regs)
 			);
 	}
 	
-	Log("Code at %p accessed %p", Regs->eip, Addr);
+	Log("CPU %i - Code at %p accessed %p", GetCPUNum(), Regs->eip, Addr);
 	// Print Stack Backtrace
 	Error_Backtrace(Regs->eip, Regs->ebp);
-	
+
+	#if 0	
 	Log("gaPageDir[0x%x] = 0x%x", Addr>>22, gaPageDir[Addr>>22]);
 	if( gaPageDir[Addr>>22] & PF_PRESENT )
 		Log("gaPageTable[0x%x] = 0x%x", Addr>>12, gaPageTable[Addr>>12]);
-	
+	#endif
 	//MM_DumpTables(0, -1);	
 	
 	// Register Dump
diff --git a/Kernel/arch/x86/proc.asm b/Kernel/arch/x86/proc.asm
index 74a25d46..014cfed1 100644
--- a/Kernel/arch/x86/proc.asm
+++ b/Kernel/arch/x86/proc.asm
@@ -55,11 +55,14 @@ SwitchTasks:
 	
 	; Old IP
 	mov eax, [esp+0x20+16]
+	test eax, eax
+	jz .nosave
 	mov DWORD [eax], .restore
 	; Old SP
 	mov eax, [esp+0x20+8]
 	mov [eax], esp
 
+.nosave:
 	mov ecx, [esp+0x20+12]	; New IP
 	mov eax, [esp+0x20+20]	; New CR3
 	mov esp, [esp+0x20+ 4]	; New SP
@@ -376,4 +379,4 @@ User_Syscall_Exit:
 	mov ebx, [esp+4]
 	int 0xAC
 
-; vim: ft=nasm, ts=8
+; vim: ft=nasm ts=8
diff --git a/Kernel/arch/x86/proc.c b/Kernel/arch/x86/proc.c
index 1cd11067..7d54d0c2 100644
--- a/Kernel/arch/x86/proc.c
+++ b/Kernel/arch/x86/proc.c
@@ -14,9 +14,9 @@
 #include <hal_proc.h>
 
 // === FLAGS ===
-#define DEBUG_TRACE_SWITCH	1
+#define DEBUG_TRACE_SWITCH	0
 #define DEBUG_DISABLE_DOUBLEFAULT	1
-#define DEBUG_VERY_SLOW_SWITCH	0
+#define DEBUG_VERY_SLOW_PERIOD	0
 
 // === CONSTANTS ===
 // Base is 1193182
@@ -165,6 +165,7 @@ void ArchThreads_Init(void)
 	{
 		 int	i;
 	 	tMPTable_Ent	*ents;
+		#if DUMP_MP_TABLE
 		Log("gMPFloatPtr = %p", gMPFloatPtr);
 		Log("*gMPFloatPtr = {");
 		Log("\t.Sig = 0x%08x", gMPFloatPtr->Sig);
@@ -178,8 +179,10 @@ void ArchThreads_Init(void)
 			gMPFloatPtr->Features[4]
 			);
 		Log("}");
-		
+		#endif		
+
 		mptable = (void*)( KERNEL_BASE|gMPFloatPtr->MPConfig );
+		#if DUMP_MP_TABLE
 		Log("mptable = %p", mptable);
 		Log("*mptable = {");
 		Log("\t.Sig = 0x%08x", mptable->Sig);
@@ -195,6 +198,7 @@ void ArchThreads_Init(void)
 		Log("\t.ExtendedTableLen = 0x%04x", mptable->ExtendedTableLen);
 		Log("\t.ExtendedTableChecksum = 0x%02x", mptable->ExtendedTableChecksum);
 		Log("}");
+		#endif
 		
 		gpMP_LocalAPIC = (void*)MM_MapHWPages(mptable->LocalAPICMemMap, 1);
 		
@@ -208,13 +212,14 @@ void ArchThreads_Init(void)
 			{
 			case 0:	// Processor
 				entSize = 20;
+				#if DUMP_MP_TABLE
 				Log("%i: Processor", i);
 				Log("\t.APICID = %i", ents->Proc.APICID);
 				Log("\t.APICVer = 0x%02x", ents->Proc.APICVer);
 				Log("\t.CPUFlags = 0x%02x", ents->Proc.CPUFlags);
 				Log("\t.CPUSignature = 0x%08x", ents->Proc.CPUSignature);
 				Log("\t.FeatureFlags = 0x%08x", ents->Proc.FeatureFlags);
-				
+				#endif
 				
 				if( !(ents->Proc.CPUFlags & 1) ) {
 					Log("DISABLED");
@@ -240,7 +245,7 @@ void ArchThreads_Init(void)
 				
 				break;
 			
-			#if DUMP_MP_TABLES
+			#if DUMP_MP_TABLE >= 2
 			case 1:	// Bus
 				entSize = 8;
 				Log("%i: Bus", i);
@@ -942,11 +947,22 @@ void Proc_Reschedule(void)
 		Proc_DisableSSE();
 	}
 
-	SwitchTasks(
-		nextthread->SavedState.ESP, &curthread->SavedState.ESP,
-		nextthread->SavedState.EIP, &curthread->SavedState.EIP,
-		nextthread->MemState.CR3
-		);
+	if( curthread )
+	{
+		SwitchTasks(
+			nextthread->SavedState.ESP, &curthread->SavedState.ESP,
+			nextthread->SavedState.EIP, &curthread->SavedState.EIP,
+			nextthread->MemState.CR3
+			);
+	}
+	else
+	{
+		SwitchTasks(
+			nextthread->SavedState.ESP, 0,
+			nextthread->SavedState.EIP, 0,
+			nextthread->MemState.CR3
+			);
+	}
 
 	return ;
 }
diff --git a/Kernel/arch/x86/start.asm b/Kernel/arch/x86/start.asm
index 68bdc55d..b6026de6 100644
--- a/Kernel/arch/x86/start.asm
+++ b/Kernel/arch/x86/start.asm
@@ -68,17 +68,23 @@ start:
 	; Start Paging
 	mov ecx, gaInitPageDir - KERNEL_BASE
 	mov cr3, ecx
-	
 	mov ecx, cr0
 	or ecx, 0x80010000	; PG and WP
 	mov cr0, ecx
 	
 	mov WORD [0xB8002], 0x0763	; 'c'
-	mov WORD [0xB8004], 0x0765	; 'e'
 	
-	lea ecx, [.higherHalf]
-	jmp ecx
-.higherHalf:
+	; Set GDT
+	lgdt [gGDTPtr]
+	mov cx, 0x10	; PL0 Data
+	mov ss, cx
+	mov ds, cx
+	mov es, cx
+	mov gs, cx
+	mov fs, cx
+	mov WORD [0xB8004], 0x0765	; 'e'
+	jmp 0x08:.higher_half
+.higher_half:
 	
 	mov WORD [0xB8006], 0x0773	; 's'
 	mov WORD [0xB8008], 0x0773	; 's'
@@ -99,8 +105,6 @@ start:
 ; Multiprocessing AP Startup Code (Must be within 0 - 0x10FFF0)
 ;
 %if USE_MP
-[extern gGDT]
-[extern gGDTPtr]
 [extern gIDTPtr]
 [extern gpMP_LocalAPIC]
 [extern giMP_TimerCount]
@@ -108,6 +112,7 @@ start:
 [extern gaCPUs]
 [extern giNumInitingCPUs]
 [extern MM_NewKStack]
+[extern Proc_InitialiseSSE]
 
 lGDTPtr:	; Local GDT Pointer
 	dw	3*8-1
@@ -195,6 +200,9 @@ APStartup:
 	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)
+
+	; Initialise SSE support
+	call Proc_InitialiseSSE
 	
 	; CPU is now marked as initialised
 
@@ -230,6 +238,25 @@ CallWithArgArray:
 	pop ebp
 	ret
 
+[section .data]
+; GDT
+GDT_SIZE	equ	(1+2*2+1+MAX_CPUS)*8
+[global gGDT]
+gGDT:
+	; PL0 - Kernel
+	; PL3 - User
+	dd 0x00000000, 0x00000000	; 00 NULL Entry
+	dd 0x0000FFFF, 0x00CF9A00	; 08 PL0 Code
+	dd 0x0000FFFF, 0x00CF9200	; 10 PL0 Data
+	dd 0x0000FFFF, 0x00CFFA00	; 18 PL3 Code
+	dd 0x0000FFFF, 0x00CFF200	; 20 PL3 Data
+	dd 26*4-1, 0x00408900	; 28 Double Fault TSS
+	times MAX_CPUS	dd 26*4-1, 0x00408900	; 30+ TSSes
+[global gGDTPtr]
+gGDTPtr:
+	dw	GDT_SIZE-1
+	dd	gGDT
+
 [section .initpd]
 [global gaInitPageDir]
 [global gaInitPageTable]
diff --git a/Kernel/arch/x86/vm8086.c b/Kernel/arch/x86/vm8086.c
index 879cec64..839a640b 100644
--- a/Kernel/arch/x86/vm8086.c
+++ b/Kernel/arch/x86/vm8086.c
@@ -59,12 +59,15 @@ Uint32	gaVM8086_MemBitmap[VM8086_BLOCKCOUNT/32];
 int VM8086_Install(char **Arguments)
 {
 	tPID	pid;	
+
+	Semaphore_Init(&gVM8086_TasksToDo, 0, 10, "VM8086", "TasksToDo");
 	
 	// Lock to avoid race conditions
 	Mutex_Acquire( &glVM8086_Process );
 	
 	// Create BIOS Call process
 	pid = Proc_Clone(CLONE_VM);
+	Log_Debug("VM8086", "pid = %i", pid);
 	if(pid == -1)
 	{
 		Log_Error("VM8086", "Unable to clone kernel into VM8086 worker");
@@ -75,17 +78,16 @@ int VM8086_Install(char **Arguments)
 		Uint	* volatile stacksetup;	// Initialising Stack
 		Uint16	* volatile rmstack;	// Real Mode Stack
 		 int	i;
-		
+
+		Log_Debug("VM8086", "Initialising worker");	
+	
 		// Set Image Name
 		Threads_SetName("VM8086");
 
-//		Log_Debug("VM8086", "Mapping memory");	
-	
 		// Map ROM Area
 		for(i=0xA0;i<0x100;i++) {
 			MM_Map( i * 0x1000, i * 0x1000 );
 		}
-//		Log_Debug("VM8086", "ROM area mapped");
 		MM_Map( 0, 0 );	// IVT / BDA
 		// Map (but allow allocation) of 0x1000 - 0x9F000
 		// - So much hack, it isn't funny
@@ -102,7 +104,6 @@ int VM8086_Install(char **Arguments)
 			gVM8086_WorkerPID = 0;
 			Threads_Exit(0, 1);
 		}
-//		Log_Debug("VM8086", "Mapped low memory");
 		
 		*(Uint8*)(0x100000) = VM8086_OP_IRET;
 		*(Uint8*)(0x100001) = 0x07;	// POP ES
@@ -148,9 +149,10 @@ int VM8086_Install(char **Arguments)
 	}
 	
 	gVM8086_WorkerPID = pid;
-//	Log_Log("VM8086", "gVM8086_WorkerPID = %i", pid);
-	while( gpVM8086_State != NULL )
-		Threads_Yield();	// Yield to allow the child to initialise
+
+	// It's released when the GPF fires
+	Mutex_Acquire( &glVM8086_Process );
+	Mutex_Release( &glVM8086_Process );
 	
 	// Worker killed itself
 	if( gVM8086_WorkerPID != pid ) {
@@ -164,7 +166,7 @@ void VM8086_GPF(tRegs *Regs)
 {
 	Uint8	opcode;
 	
-	//Log_Log("VM8086", "GPF - %04x:%04x", Regs->cs, Regs->eip);
+//	Log_Log("VM8086", "GPF - %04x:%04x", Regs->cs, Regs->eip);
 	
 	if(Regs->eip == VM8086_MAGIC_IP && Regs->cs == VM8086_MAGIC_CS
 	&& Threads_GetPID() == gVM8086_WorkerPID)
@@ -174,8 +176,8 @@ void VM8086_GPF(tRegs *Regs)
 			gpVM8086_State = NULL;
 			Mutex_Release( &glVM8086_Process );	// Release lock obtained in VM8086_Install
 		}
-		//Log_Log("VM8086", "gpVM8086_State = %p, gVM8086_CallingThread = %i",
-		//	gpVM8086_State, gVM8086_CallingThread);
+//		Log_Log("VM8086", "gpVM8086_State = %p, gVM8086_CallingThread = %i",
+//			gpVM8086_State, gVM8086_CallingThread);
 		if( gpVM8086_State ) {
 			gpVM8086_State->AX = Regs->eax;	gpVM8086_State->CX = Regs->ecx;
 			gpVM8086_State->DX = Regs->edx;	gpVM8086_State->BX = Regs->ebx;
@@ -192,12 +194,12 @@ void VM8086_GPF(tRegs *Regs)
 		Semaphore_Wait(&gVM8086_TasksToDo, 1);
 		
 		//Log_Log("VM8086", "We have a task (%p)", gpVM8086_State);
-		Regs->esp -= 2;	*(Uint16*volatile)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = VM8086_MAGIC_CS;
-		Regs->esp -= 2;	*(Uint16*volatile)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = VM8086_MAGIC_IP;
-		Regs->esp -= 2;	*(Uint16*volatile)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->CS;
-		Regs->esp -= 2;	*(Uint16*volatile)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->IP;
-		Regs->esp -= 2;	*(Uint16*volatile)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->DS;
-		Regs->esp -= 2;	*(Uint16*volatile)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->ES;
+		Regs->esp -= 2;	*(Uint16*)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = VM8086_MAGIC_CS;
+		Regs->esp -= 2;	*(Uint16*)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = VM8086_MAGIC_IP;
+		Regs->esp -= 2;	*(Uint16*)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->CS;
+		Regs->esp -= 2;	*(Uint16*)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->IP;
+		Regs->esp -= 2;	*(Uint16*)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->DS;
+		Regs->esp -= 2;	*(Uint16*)( (Regs->ss<<4) + (Regs->esp&0xFFFF) ) = gpVM8086_State->ES;
 		
 		// Set Registers
 		Regs->eip = 0x11;	Regs->cs = 0xFFFF;
@@ -222,8 +224,9 @@ void VM8086_GPF(tRegs *Regs)
 		#endif
 		break;
 	case VM8086_OP_POPF:	//POPF
-		Regs->eflags &= 0xFFFF0002;
-		Regs->eflags |= *(Uint16*)( Regs->ss*16 + (Regs->esp&0xFFFF) ) & 0xFFFD;	// Changing IF is not allowed
+		// Changing IF is not allowed
+		Regs->eflags &= 0xFFFF0202;
+		Regs->eflags |= *(Uint16*)( Regs->ss*16 + (Regs->esp&0xFFFF) );
 		Regs->esp += 2;
 		#if TRACE_EMU
 		Log_Debug("VM8086", "Emulated POPF");
@@ -236,8 +239,8 @@ void VM8086_GPF(tRegs *Regs)
 		id = *(Uint8*)( Regs->cs*16 +(Regs->eip&0xFFFF));
 		Regs->eip ++;
 		
-		Regs->esp -= 2;	*(Uint16*volatile)( Regs->ss*16 + (Regs->esp&0xFFFF) ) = Regs->cs;
-		Regs->esp -= 2;	*(Uint16*volatile)( Regs->ss*16 + (Regs->esp&0xFFFF) ) = Regs->eip;
+		Regs->esp -= 2;	*(Uint16*)( Regs->ss*16 + (Regs->esp&0xFFFF) ) = Regs->cs;
+		Regs->esp -= 2;	*(Uint16*)( Regs->ss*16 + (Regs->esp&0xFFFF) ) = Regs->eip;
 		
 		Regs->cs = *(Uint16*)(4*id + 2);
 		Regs->eip = *(Uint16*)(4*id);
diff --git a/Kernel/debug.c b/Kernel/debug.c
index 25ffda19..355624d5 100644
--- a/Kernel/debug.c
+++ b/Kernel/debug.c
@@ -9,7 +9,7 @@
 
 #define	DEBUG_MAX_LINE_LEN	256
 
-#define	LOCK_DEBUG_OUTPUT	0
+#define	LOCK_DEBUG_OUTPUT	1
 
 #define TRACE_TO_KTERM	0
 
diff --git a/Kernel/heap.c b/Kernel/heap.c
index 5593c68f..921cc6b9 100644
--- a/Kernel/heap.c
+++ b/Kernel/heap.c
@@ -700,6 +700,7 @@ void Heap_Stats(void)
 	
 	// Scan and get distribution
 	#if 1
+	if(nBlocks > 0)
 	{
 		struct {
 			Uint	Size;
diff --git a/Kernel/lib.c b/Kernel/lib.c
index 953264b7..76cc4286 100644
--- a/Kernel/lib.c
+++ b/Kernel/lib.c
@@ -210,8 +210,10 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args)
 	size_t	pos = 0;
 	// Flags
 	 int	bPadLeft = 0;
+	
+	auto void _putch(char ch);
 
-	inline void _putch(char ch)
+	void _putch(char ch)
 	{
 		if(pos < __maxlen)
 		{
-- 
2.20.1