X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FKernel%2Fheap.c;h=ba4c9adbdd9add234a3bd0c7649056f33f920bb6;hb=3c283c4831c40c14d308a54cefb997832a860bca;hp=4c7ffd955f21efdb5e49b4473de0d1a40eae3068;hpb=e02f66c7125bf18f77c6c53587238cbd49da2c89;p=tpg%2Facess2.git

diff --git a/KernelLand/Kernel/heap.c b/KernelLand/Kernel/heap.c
old mode 100644
new mode 100755
index 4c7ffd95..ba4c9adb
--- a/KernelLand/Kernel/heap.c
+++ b/KernelLand/Kernel/heap.c
@@ -1,14 +1,20 @@
 /*
- * AcessOS Microkernel Version
+ * Acess2 Kernel
+ * - By John Hodge (thePowersGang)
+ *
  * heap.c
+ * - Dynamic memory allocation
  */
 #include <acess.h>
 #include <mm_virt.h>
 #include <heap_int.h>
+#include <limits.h>
+#include <debug_hooks.h>
 
-#define WARNINGS	1
-#define	DEBUG_TRACE	0
-#define	VERBOSE_DUMP	0
+#define WARNINGS	1	// Warn and dump on heap errors
+#define	DEBUG_TRACE	0	// Enable tracing of allocations
+#define	VERBOSE_DUMP	0	// Set to 1 to enable a verbose dump when heap errors are encountered
+#define VALIDATE_ON_ALLOC	1	// Set to 1 to enable validation of the heap on all malloc() calls
 
 // === CONSTANTS ===
 #define	HEAP_INIT_SIZE	0x8000	// 32 KiB
@@ -26,69 +32,90 @@
 
 // === PROTOTYPES ===
 void	Heap_Install(void);
-void	*Heap_Extend(int Bytes);
+void	*Heap_Extend(size_t Bytes);
 void	*Heap_Merge(tHeapHead *Head);
+static const size_t	Heap_int_GetBlockSize(size_t AllocSize);
 //void	*Heap_Allocate(const char *File, int Line, size_t Bytes);
 //void	*Heap_AllocateZero(const char *File, int Line, size_t Bytes);
 //void	*Heap_Reallocate(const char *File, int Line, void *Ptr, size_t Bytes);
-//void	Heap_Deallocate(void *Ptr);
-void	Heap_Dump(void);
-void	Heap_Stats(void);
+//void	Heap_Deallocate(const char *File, int Line, void *Ptr);
+ int	Heap_int_ApplyWatchpont(void *Addr, bool Enabled);
+void	Heap_int_WatchBlock(tHeapHead *Head, bool Enabled);
+//void	Heap_Dump(void);
+void	Heap_ValidateDump(int bVerbose);
+//void	Heap_Stats(void);
 
 // === GLOBALS ===
 tMutex	glHeap;
-void	*gHeapStart;
-void	*gHeapEnd;
+tHeapHead	*gHeapStart;
+tHeapHead	*gHeapEnd;
 
 // === CODE ===
 void Heap_Install(void)
 {
-	gHeapStart	= (void*)MM_KHEAP_BASE;
-	gHeapEnd	= (void*)MM_KHEAP_BASE;
+	gHeapStart = (void*)MM_KHEAP_BASE;
+	gHeapEnd   = gHeapStart;
 	Heap_Extend(HEAP_INIT_SIZE);
 }
 
+static inline tHeapHead *Heap_NextHead(tHeapHead *Head) {
+	return (void*)( (char*)Head + Head->Size );
+}
+static inline tHeapFoot *Heap_ThisFoot(tHeapHead *Head) {
+	return (void*)( (char*)Head + Head->Size - sizeof(tHeapFoot) );
+}
+static inline tHeapFoot *Heap_PrevFoot(tHeapHead *Head) {
+	//ASSERT(Head != gHeapStart);
+	return (void*)( (char*)Head - sizeof(tHeapFoot) );
+}
+
 /**
  * \brief Extend the size of the heap
  */
-void *Heap_Extend(int Bytes)
+void *Heap_Extend(size_t Bytes)
 {
-	Uint	i;
-	tHeapHead	*head = gHeapEnd;
-	tHeapFoot	*foot;
+	//Debug("Heap_Extend(0x%x)", Bytes);
 	
 	// Bounds Check
-	if( (tVAddr)gHeapEnd == MM_KHEAP_MAX )
+	if( gHeapEnd == (tHeapHead*)MM_KHEAP_MAX ) {
+		Log_Error("Heap", "Heap limit reached (%p)", (void*)MM_KHEAP_MAX);
 		return NULL;
+	}
 	
 	if( Bytes == 0 ) {
 		Log_Warning("Heap", "Heap_Extend called with Bytes=%i", Bytes);
 		return NULL;
 	}
 	
+	const Uint	pages = (Bytes + 0xFFF) >> 12;
+	tHeapHead	*new_end = (void*)( (tVAddr)gHeapEnd + (pages << 12) );
 	// Bounds Check
-	if( (tVAddr)gHeapEnd + ((Bytes+0xFFF)&~0xFFF) > MM_KHEAP_MAX ) {
-//		Bytes = MM_KHEAP_MAX - (tVAddr)gHeapEnd;
+	if( new_end > (tHeapHead*)MM_KHEAP_MAX )
+	{
+		Log_Error("Heap", "Heap limit exceeded (%p)", (void*)new_end);
+		// TODO: Clip allocation to available space, and have caller check returned block
 		return NULL;
 	}
 	
 	// Heap expands in pages
-	for( i = 0; i < (Bytes+0xFFF) >> 12; i ++ )
+	for( Uint i = 0; i < pages; i ++ )
 	{
-		if( !MM_Allocate( (tVAddr)gHeapEnd+(i<<12) ) )
+		if( !MM_Allocate( (tPage*)gHeapEnd + i ) )
 		{
-			Warning("OOM - Heap_Extend");
+			Warning("OOM - Heap_Extend (%i bytes)");
+			Heap_Dump();
 			return NULL;
 		}
 	}
 	
-	// Increas heap end
-	gHeapEnd = (Uint8*)gHeapEnd + (i << 12);
+	// Increase heap end
+	tHeapHead	*head = gHeapEnd;
+	gHeapEnd = new_end;
 	
 	// Create Block
 	head->Size = (Bytes+0xFFF)&~0xFFF;
 	head->Magic = MAGIC_FREE;
-	foot = (void*)( (Uint)gHeapEnd - sizeof(tHeapFoot) );
+	tHeapFoot *foot = Heap_ThisFoot(head);
 	foot->Head = head;
 	foot->Magic = MAGIC_FOOT;
 	
@@ -96,23 +123,19 @@ void *Heap_Extend(int Bytes)
 }
 
 /**
- * \brief Merges two ajacent heap blocks
+ * \brief Merges two adjacent heap blocks
  */
 void *Heap_Merge(tHeapHead *Head)
 {
-	tHeapFoot	*foot;
-	tHeapFoot	*thisFoot;
-	tHeapHead	*head;
-	
 	//Log("Heap_Merge: (Head=%p)", Head);
+	tHeapFoot *thisFoot = Heap_ThisFoot(Head);
 	
-	thisFoot = (void*)( (Uint)Head + Head->Size - sizeof(tHeapFoot) );
-	if((Uint)thisFoot > (Uint)gHeapEnd)	return NULL;
+	ASSERT( Heap_NextHead(Head) <= gHeapEnd );
 	
 	// Merge Left (Down)
-	foot = (void*)( (Uint)Head - sizeof(tHeapFoot) );
-	if( ((Uint)foot < (Uint)gHeapEnd && (Uint)foot > (Uint)gHeapStart)
-	&& foot->Head->Magic == MAGIC_FREE) {
+	tHeapFoot *foot = Heap_PrevFoot(Head);
+	if( Head > gHeapStart && foot->Head->Magic == MAGIC_FREE)
+	{
 		foot->Head->Size += Head->Size;	// Increase size
 		thisFoot->Head = foot->Head;	// Change backlink
 		Head->Magic = 0;	// Clear old head
@@ -123,22 +146,34 @@ void *Heap_Merge(tHeapHead *Head)
 	}
 	
 	// Merge Right (Upwards)
-	head = (void*)( (Uint)Head + Head->Size );
-	if((Uint)head < (Uint)gHeapEnd && head->Magic == MAGIC_FREE)
+	tHeapHead *nexthead = Heap_NextHead(Head);
+	if(nexthead < gHeapEnd && nexthead->Magic == MAGIC_FREE)
 	{
-		Head->Size += head->Size;
-		foot = (void*)( (Uint)Head + Head->Size - sizeof(tHeapFoot) );
+		Head->Size += nexthead->Size;
+		foot = Heap_ThisFoot(Head);
 		foot->Head = Head;	// Update Backlink
 		thisFoot->Head = NULL;	// Clear old footer
 		thisFoot->Magic = 0;
-		head->Size = 0;		// Clear old header
-		head->Magic = 0;
+		nexthead->Size = 0;		// Clear old header
+		nexthead->Magic = 0;
 	}
 	
 	// Return new address
 	return Head;
 }
 
+static const size_t Heap_int_GetBlockSize(size_t AllocSize)
+{
+	size_t Bytes;
+	#if POW2_SIZES
+	Bytes = AllocSize + sizeof(tHeapHead) + sizeof(tHeapFoot);
+	Bytes = 1UUL << LOG2(Bytes);
+	#else
+	Bytes = (AllocSize + sizeof(tHeapHead) + sizeof(tHeapFoot) + MIN_SIZE-1) & ~(MIN_SIZE-1);
+	#endif
+	return Bytes;
+}
+
 /**
  * \param File	Allocating source file
  * \param Line	Source line
@@ -146,33 +181,29 @@ void *Heap_Merge(tHeapHead *Head)
  */
 void *Heap_Allocate(const char *File, int Line, size_t __Bytes)
 {
-	tHeapHead	*head, *newhead;
+	tHeapHead	*newhead;
 	tHeapFoot	*foot, *newfoot;
 	tHeapHead	*best = NULL;
-	Uint	bestSize = 0;	// Speed hack
+	Uint	bestSize = UINT_MAX;	// Speed hack
 	size_t	Bytes;
 
 	if( __Bytes == 0 ) {
-		//return NULL;	// TODO: Return a known un-mapped range.
-		return INVLPTR;
+		return NULL;	// TODO: Return a known un-mapped range.
+//		return INVLPTR;
 	}
+
+	#if VALIDATE_ON_ALLOC
+	Heap_Validate();
+	#endif
 	
 	// Get required size
-	#if POW2_SIZES
-	Bytes = __Bytes + sizeof(tHeapHead) + sizeof(tHeapFoot);
-	Bytes = 1UUL << LOG2(__Bytes);
-	#else
-	Bytes = (__Bytes + sizeof(tHeapHead) + sizeof(tHeapFoot) + MIN_SIZE-1) & ~(MIN_SIZE-1);
-	#endif
+	Bytes = Heap_int_GetBlockSize(__Bytes);
 	
 	// Lock Heap
 	Mutex_Acquire(&glHeap);
 	
 	// Traverse Heap
-	for( head = gHeapStart;
-		(Uint)head < (Uint)gHeapEnd;
-		head = (void*)((Uint)head + head->Size)
-		)
+	for( tHeapHead *head = gHeapStart; head < gHeapEnd; head = Heap_NextHead(head) )
 	{
 		// Alignment Check
 		#if POW2_SIZES
@@ -182,9 +213,30 @@ void *Heap_Allocate(const char *File, int Line, size_t __Bytes)
 		#endif
 			Mutex_Release(&glHeap);	// Release spinlock
 			#if WARNINGS
-			Log_Warning("Heap", "Size of heap address %p is invalid - not aligned (0x%x) [at paddr 0x%x]",
+			Log_Warning("Heap", "Size of heap address %p is invalid"
+				" - not aligned (0x%x) [at paddr 0x%x]",
 				head, head->Size, MM_GetPhysAddr(&head->Size));
-			Heap_Dump();
+			Heap_ValidateDump(VERBOSE_DUMP);
+			#endif
+			return NULL;
+		}
+		if( head->Size < MIN_SIZE ) {
+			Mutex_Release(&glHeap);
+			#if WARNINGS
+			Log_Warning("Heap", "Size of heap address %p is invalid"
+				" - Too small (0x%x) [at paddr 0x%x]",
+				head, head->Size, MM_GetPhysAddr(&head->Size));
+			Heap_ValidateDump(VERBOSE_DUMP);
+			#endif
+			return NULL;
+		}
+		if( head->Size > (2<<30) ) {
+			Mutex_Release(&glHeap);
+			#if WARNINGS
+			Log_Warning("Heap", "Size of heap address %p is invalid"
+				" - Over 2GiB (0x%x) [at paddr 0x%x]",
+				head, head->Size, MM_GetPhysAddr(&head->Size));
+			Heap_ValidateDump(VERBOSE_DUMP);
 			#endif
 			return NULL;
 		}
@@ -197,7 +249,7 @@ void *Heap_Allocate(const char *File, int Line, size_t __Bytes)
 			#if WARNINGS
 			Log_Warning("Heap", "Magic of heap address %p is invalid (%p = 0x%x)",
 				head, &head->Magic, head->Magic);
-			Heap_Dump();
+			Heap_ValidateDump(VERBOSE_DUMP);
 			#endif
 			return NULL;
 		}
@@ -214,8 +266,8 @@ void *Heap_Allocate(const char *File, int Line, size_t __Bytes)
 			head->AllocateTime = now();
 			Mutex_Release(&glHeap);	// Release spinlock
 			#if DEBUG_TRACE
-			Debug("[Heap   ] Malloc'd %p (%i bytes), returning to %p",
-				head->Data, head->Size,  __builtin_return_address(0));
+			Log_Debug("Heap", "Malloc'd %p (0x%x bytes), returning to %s:%i",
+				head->Data, head->Size, File, Line);
 			#endif
 			return head->Data;
 		}
@@ -240,6 +292,7 @@ void *Heap_Allocate(const char *File, int Line, size_t __Bytes)
 		best = Heap_Extend( Bytes );
 		// Check for errors
 		if(!best) {
+			Warning("OOM when allocating 0x%x bytes", Bytes);
 			Mutex_Release(&glHeap);	// Release spinlock
 			return NULL;
 		}
@@ -252,32 +305,41 @@ void *Heap_Allocate(const char *File, int Line, size_t __Bytes)
 			best->AllocateTime = now();
 			Mutex_Release(&glHeap);	// Release spinlock
 			#if DEBUG_TRACE
-			Debug("[Heap   ] Malloc'd %p (%i bytes), returning to %s:%i", best->Data, best->Size, File, Line);
+			Log_Debug("Heap", "Malloc'd %p (0x%x bytes), returning to %s:%i",
+				best->Data, best->Size, File, Line);
 			#endif
 			return best->Data;
 		}
 	}
 	
 	// Split Block
-	newhead = (void*)( (Uint)best + Bytes );
-	newfoot = (void*)( (Uint)best + Bytes - sizeof(tHeapFoot) );
-	foot = (void*)( (Uint)best + best->Size - sizeof(tHeapFoot) );
-	
-	newfoot->Head = best;	// Create new footer
-	newfoot->Magic = MAGIC_FOOT;
-	newhead->Size = best->Size - Bytes;	// Create new header
-	newhead->Magic = MAGIC_FREE;
-	foot->Head = newhead;	// Update backlink in old footer
+	// - Save size for new block
+	size_t	newsize = best->Size - Bytes;
+	// - Allocate beginning of old block
 	best->Size = Bytes;		// Update size in old header
 	best->ValidSize = __Bytes;
 	best->Magic = MAGIC_USED;	// Mark block as used
 	best->File = File;
 	best->Line = Line;
 	best->AllocateTime = now();
+	// - Create a new foot on old block
+	newfoot = Heap_ThisFoot(best);
+	newfoot->Head = best;	// Create new footer
+	newfoot->Magic = MAGIC_FOOT;
+	// - Create a new header after resized old
+	newhead = Heap_NextHead(best);
+	newhead->Size = newsize;
+	newhead->Magic = MAGIC_FREE;
+	newhead->ValidSize = 0;
+	newhead->File = NULL;
+	newhead->Line = 0;
+	// - Update footer
+	foot = Heap_ThisFoot(newhead);
+	foot->Head = newhead;
 	
 	Mutex_Release(&glHeap);	// Release spinlock
 	#if DEBUG_TRACE
-	Debug("[Heap   ] Malloc'd %p (0x%x bytes), returning to %s:%i",
+	Log_Debug("Heap", "Malloc'd %p (0x%x bytes), returning to %s:%i",
 		best->Data, best->Size, File, Line);
 	#endif
 	return best->Data;
@@ -286,21 +348,16 @@ void *Heap_Allocate(const char *File, int Line, size_t __Bytes)
 /**
  * \brief Free an allocated memory block
  */
-void Heap_Deallocate(void *Ptr)
+void Heap_Deallocate(const char *File, int Line, void *Ptr)
 {
-	tHeapHead	*head = (void*)( (Uint)Ptr - sizeof(tHeapHead) );
-	tHeapFoot	*foot;
-	
 	// INVLPTR is returned from Heap_Allocate when the allocation
 	// size is zero.
 	if( Ptr == INVLPTR )	return;
-	
-	#if DEBUG_TRACE
-	Debug("[Heap   ] free: %p freed by %p (%i old)", Ptr, __builtin_return_address(0), now()-head->AllocateTime);
-	#endif
+	// free(NULL) is a no-op
+	if( Ptr == NULL )	return;
 	
 	// Alignment Check
-	if( (Uint)Ptr & (sizeof(Uint)-1) ) {
+	if( (tVAddr)Ptr % sizeof(void*) != 0 ) {
 		Log_Warning("Heap", "free - Passed a non-aligned address (%p)", Ptr);
 		return;
 	}
@@ -314,9 +371,12 @@ void Heap_Deallocate(void *Ptr)
 	}
 	
 	// Check memory block - Header
-	head = (void*)( (Uint)Ptr - sizeof(tHeapHead) );
+	tHeapHead *head = (tHeapHead*)Ptr - 1;
 	if(head->Magic == MAGIC_FREE) {
-		Log_Warning("Heap", "free - Passed a freed block (%p) by %p", head, __builtin_return_address(0));
+		Log_Warning("Heap", "free - Passed a freed block (%p) by %s:%i (was freed by %s:%i)",
+			head, File, Line,
+			head->File, head->Line);
+		Proc_PrintBacktrace();
 		return;
 	}
 	if(head->Magic != MAGIC_USED) {
@@ -326,25 +386,33 @@ void Heap_Deallocate(void *Ptr)
 	}
 	
 	// Check memory block - Footer
-	foot = (void*)( (Uint)head + head->Size - sizeof(tHeapFoot) );
+	tHeapFoot *foot = Heap_ThisFoot(head);
 	if(foot->Head != head) {
 		Log_Warning("Heap", "free - Footer backlink is incorrect (%p, 0x%x)", head, foot->Head);
 		Log_Notice("Heap", "Allocated by %s:%i", head->File, head->Line);
 		return;
 	}
 	if(foot->Magic != MAGIC_FOOT) {
-		Log_Warning("Heap", "free - Footer magic is invalid (%p, %p = 0x%x)", head, &foot->Magic, foot->Magic);
+		Log_Warning("Heap", "free - Footer magic is invalid (%p, %p = 0x%x)",
+			head, &foot->Magic, foot->Magic);
 		Log_Notice("Heap", "Allocated by %s:%i", head->File, head->Line);
 		return;
 	}
 	
+	#if DEBUG_TRACE
+	Log_Debug("Heap", "free: %p freed by %s:%i (%i old)",
+		Ptr, File, Line, now()-head->AllocateTime);
+	#endif
+	
 	// Lock
 	Mutex_Acquire( &glHeap );
-	
+
+	Heap_int_WatchBlock(head, false);
+
 	// Mark as free
 	head->Magic = MAGIC_FREE;
-	//head->File = NULL;
-	//head->Line = 0;
+	head->File = File;
+	head->Line = Line;
 	head->ValidSize = 0;
 	// Merge blocks
 	Heap_Merge( head );
@@ -362,25 +430,49 @@ void Heap_Deallocate(void *Ptr)
  */
 void *Heap_Reallocate(const char *File, int Line, void *__ptr, size_t __size)
 {
-	tHeapHead	*head = (void*)( (Uint)__ptr-sizeof(tHeapHead) );
+	tHeapHead	*head = (tHeapHead*)__ptr - 1;
 	tHeapHead	*nextHead;
 	tHeapFoot	*foot;
-	Uint	newSize = (__size + sizeof(tHeapFoot)+sizeof(tHeapHead)+MIN_SIZE-1)&~(MIN_SIZE-1);
+	Uint	newSize = Heap_int_GetBlockSize(__size);
 	
 	// Check for reallocating NULL
-	if(__ptr == NULL)	return Heap_Allocate(File, Line, __size);
+	if(__ptr == NULL)
+		return Heap_Allocate(File, Line, __size);
+	
+	if( !Heap_IsHeapAddr(__ptr)) {
+		Log_Error("Heap", "%s:%i passed non-heap address %p when reallocating to %zi",
+			File, Line, __ptr, __size);
+		return NULL;
+	}
 	
 	// Check if resize is needed
-	if(newSize <= head->Size)	return __ptr;
+	// TODO: Reduce size of block if needed
+	if(newSize <= head->Size) {
+		#if DEBUG_TRACE
+		Log_Debug("Heap", "realloc maintain %p (0x%x >= 0x%x), returning to %s:%i",
+			head->Data, head->Size, newSize, File, Line);
+		#endif
+		return __ptr;
+	}
+
+	#if VALIDATE_ON_ALLOC
+	Heap_Validate();
+	#endif
+	
+	Heap_int_WatchBlock(head, false);
 	
 	// Check if next block is free
-	nextHead = (void*)( (Uint)head + head->Size );
+	nextHead = Heap_NextHead(head);
 	
 	// Extend into next block
 	if(nextHead->Magic == MAGIC_FREE && nextHead->Size+head->Size >= newSize)
 	{
+		#if DEBUG_TRACE
+		Log_Debug("Heap", "realloc expand %p (0x%x to 0x%x), returning to %s:%i",
+			head->Data, head->Size, newSize, File, Line);
+		#endif
 		Uint	size = nextHead->Size + head->Size;
-		foot = (void*)( (Uint)nextHead + nextHead->Size - sizeof(tHeapFoot) );
+		foot = Heap_ThisFoot(nextHead);
 		// Exact Fit
 		if(size == newSize) {
 			head->Size = newSize;
@@ -393,23 +485,26 @@ void *Heap_Reallocate(const char *File, int Line, void *__ptr, size_t __size)
 			return __ptr;
 		}
 		// Create a new heap block
-		nextHead = (void*)( (Uint)head + newSize );
-		nextHead->Size = size - newSize;
-		nextHead->Magic = MAGIC_FREE;
-		foot->Head = nextHead;	// Edit 2nd footer
-		head->Size = newSize;	// Edit first header
-		head->File = File;
+		// - Update old with new information
+		head->Size = newSize;
+		head->File = File;	// Kinda counts as a new allocation
 		head->Line = Line;
 		head->ValidSize = __size;
-		// Create new footer
-		foot = (void*)( (Uint)head + newSize - sizeof(tHeapFoot) );
+		// - Create new footer
+		foot = Heap_ThisFoot(head);
 		foot->Head = head;
 		foot->Magic = MAGIC_FOOT;
+		// - Create new header
+		nextHead = Heap_NextHead(head);
+		nextHead->Size = size - newSize;
+		nextHead->Magic = MAGIC_FREE;
+		// - Update old footer
+		Heap_ThisFoot(nextHead)->Head = nextHead;
 		return __ptr;
 	}
 	
 	// Extend downwards?
-	foot = (void*)( (Uint)head - sizeof(tHeapFoot) );
+	foot = Heap_PrevFoot(head);
 	nextHead = foot->Head;
 	if(nextHead->Magic == MAGIC_FREE && nextHead->Size+head->Size >= newSize)
 	{
@@ -417,43 +512,47 @@ void *Heap_Reallocate(const char *File, int Line, void *__ptr, size_t __size)
 		// Inexact fit, split things up
 		if(size > newSize)
 		{
-			// TODO
+			// TODO: Handle splitting of downwards blocks
 			Warning("[Heap   ] TODO: Space efficient realloc when new size is smaller");
 		}
 		
+		#if DEBUG_TRACE
+		Log_Debug("Heap", "realloc expand down %p (0x%x to 0x%x), returning to %s:%i",
+			head->Data, head->Size, newSize, File, Line);
+		#endif
+		
 		// Exact fit
-		if(size >= newSize)
-		{
-			Uint	oldDataSize;
-			// Set 1st (new/lower) header
-			nextHead->Magic = MAGIC_USED;
-			nextHead->Size = newSize;
-			nextHead->File = File;
-			nextHead->Line = Line;
-			nextHead->ValidSize = __size;
-			// Get 2nd (old) footer
-			foot = (void*)( (Uint)nextHead + newSize );
-			foot->Head = nextHead;
-			// Save old data size
-			oldDataSize = head->Size - sizeof(tHeapFoot) - sizeof(tHeapHead);
-			// Clear old header
-			head->Size = 0;
-			head->Magic = 0;
-			// Copy data
-			memcpy(nextHead->Data, __ptr, oldDataSize);
-			// Return
-			return nextHead->Data;
-		}
-		// On to the expensive then
+		Uint	oldDataSize;
+		// Set 1st (new/lower) header
+		nextHead->Magic = MAGIC_USED;
+		nextHead->Size = newSize;
+		nextHead->File = File;
+		nextHead->Line = Line;
+		nextHead->ValidSize = __size;
+		// Get 2nd (old) footer
+		foot = Heap_ThisFoot(nextHead);
+		foot->Head = nextHead;
+		// Save old data size
+		oldDataSize = head->Size - sizeof(tHeapFoot) - sizeof(tHeapHead);
+		// Clear old header
+		head->Size = 0;
+		head->Magic = 0;
+		// Copy data
+		memmove(nextHead->Data, __ptr, oldDataSize);
+		// Return
+		return nextHead->Data;
 	}
 	
 	// Well, darn
 	nextHead = Heap_Allocate( File, Line, __size );
+	if(!nextHead)	return NULL;
 	nextHead -= 1;
 	nextHead->File = File;
 	nextHead->Line = Line;
 	nextHead->ValidSize = __size;
 	
+	ASSERTC(head->Size, <, nextHead->Size);
+	ASSERTC(__ptr, ==, head->Data);
 	memcpy(
 		nextHead->Data,
 		__ptr,
@@ -461,7 +560,8 @@ void *Heap_Reallocate(const char *File, int Line, void *__ptr, size_t __size)
 		);
 	
 	free(__ptr);
-	
+	Heap_Validate();
+
 	return nextHead->Data;
 }
 
@@ -500,32 +600,119 @@ int Heap_IsHeapAddr(void *Ptr)
 	return 1;
 }
 
+int Heap_int_ApplyWatchpont(void *Word, bool enabled)
+{
+	#if ARCHDIR_IS_x86
+	static void	*active_wps[4];
+	unsigned int	dr;
+	for( dr = 2; dr < 4; dr ++ )
+	{
+		if( (enabled && active_wps[dr] == NULL) || active_wps[dr] == Word)
+			break;
+	}
+	if(dr == 4) {
+		return 1;
+	}
+	if( enabled )
+	{
+		active_wps[dr] = Word;
+		switch(dr)
+		{
+		//case 0:	ASM("mov %0, %%dr0" : : "r" (Word));	break;
+		//case 1:	ASM("mov %0, %%dr1" : : "r" (Word));	break;
+		case 2:	ASM("mov %0, %%dr2" : : "r" (Word));	break;
+		case 3:	ASM("mov %0, %%dr3" : : "r" (Word));	break;
+		default:	ASSERTC(dr,<,4);	return 1;
+		}
+	}
+	else
+	{
+		active_wps[dr] = NULL;
+	}
+	Uint32	dr7flag;
+	ASM("MOV %%dr7, %0" : "=r" (dr7flag));
+	dr7flag &= ~(0x2 << (dr*2));
+	dr7flag &= ~(0xF000 << (dr*4));
+	if( enabled ) {
+		dr7flag |= 0x2 << (dr*2);
+		dr7flag |= 0xD000 << (dr*4);	// 4 bytes, write
+		Debug("Heap_int_ApplyWatchpont: Watchpoint #%i %p ENABLED", dr, Word);
+	}
+	else {
+		Debug("Heap_int_ApplyWatchpont: Watchpoint #%i %p disabled", dr, Word);
+	}
+	ASM("MOV %0, %%dr7" : : "r" (dr7flag));
+	return 0;
+	#else
+	return 1;
+	#endif
+}
+
+void Heap_int_WatchBlock(tHeapHead *Head, bool Enabled)
+{
+	int rv;
+	rv = Heap_int_ApplyWatchpont( &Head->Size, Enabled );
+	//rv = Heap_int_ApplyWatchpont( &Head->Magic, Enabled );
+	rv = rv + Heap_int_ApplyWatchpont( &Heap_ThisFoot(Head)->Head, Enabled );
+	if(rv && Enabled) {
+		Warning("Can't apply watch on %p", Head);
+	}
+}
+
+int Heap_WatchBlock(void *Ptr)
+{
+	//Heap_int_ApplyWatchpont();
+	tHeapHead	*head;
+	if((Uint)Ptr < (Uint)gHeapStart)	return 0;
+	if((Uint)Ptr > (Uint)gHeapEnd)	return 0;
+	if((Uint)Ptr & (sizeof(Uint)-1))	return 0;
+	
+	head = (tHeapHead*)Ptr - 1;
+	
+	Heap_int_WatchBlock( head, true );
+	
+	return 0;
+}
+
 /**
  */
 void Heap_Validate(void)
 {
-	Heap_Dump();
+	// Call dump non-verbosely.
+	// - If a heap error is detected, it'll print
+	Heap_ValidateDump(0);
 }
 
-#if WARNINGS
 void Heap_Dump(void)
+{
+	Heap_ValidateDump(1);
+}
+
+void Heap_ValidateDump(int bVerbose)
 {
 	tHeapHead	*head, *badHead;
 	tHeapFoot	*foot = NULL;
+	static int	in_heap_dump;
 	
+	if( in_heap_dump )	return;
+
+	in_heap_dump = 1;
+
 	head = gHeapStart;
 	while( (Uint)head < (Uint)gHeapEnd )
 	{		
-		foot = (void*)( (Uint)head + head->Size - sizeof(tHeapFoot) );
-		#if VERBOSE_DUMP
-		Log_Log("Heap", "%p (0x%P): 0x%08x (%i) %4C",
-			head, MM_GetPhysAddr(head), head->Size, head->ValidSize, &head->Magic);
-		Log_Log("Heap", "%p %4C", foot->Head, &foot->Magic);
-		if(head->File) {
-			Log_Log("Heap", "%sowned by %s:%i",
-				(head->Magic==MAGIC_FREE?"was ":""), head->File, head->Line);
+		foot = Heap_ThisFoot(head);
+		
+		if( bVerbose )
+		{
+			Log_Log("Heap", "%p (%P): 0x%08x (%i) %4C",
+				head, MM_GetPhysAddr(head), head->Size, head->ValidSize, &head->Magic);
+			Log_Log("Heap", "%p %4C", foot->Head, &foot->Magic);
+			if(head->File) {
+				Log_Log("Heap", "%sowned by %s:%i",
+					(head->Magic==MAGIC_FREE?"was ":""), head->File, head->Line);
+			}
 		}
-		#endif
 		
 		// Sanity Check Header
 		if(head->Size == 0) {
@@ -551,39 +738,45 @@ void Heap_Dump(void)
 			break;
 		}
 		
-		#if VERBOSE_DUMP
-		Log_Log("Heap", "");
-		#endif
+		if( bVerbose )
+		{
+			Log_Log("Heap", "");
+		}
 		
 		// All OK? Go to next
 		head = foot->NextHead;
 	}
 	
 	// If the heap is valid, ok!
-	if( (tVAddr)head == (tVAddr)gHeapEnd )
+	if( (tVAddr)head == (tVAddr)gHeapEnd ) {
+		in_heap_dump = 0;
 		return ;
+	}
 	
 	// Check for a bad return
-	if( (tVAddr)head >= (tVAddr)gHeapEnd )
+	if( (tVAddr)head >= (tVAddr)gHeapEnd ) {
+		in_heap_dump = 0;
 		return ;
-
-	#if !VERBOSE_DUMP
-	Log_Log("Heap", "%p (%P): 0x%08lx %i %4C",
-		head, MM_GetPhysAddr(head), head->Size, head->ValidSize, &head->Magic);
-	if(foot)
-		Log_Log("Heap", "Foot %p = {Head:%p,Magic:%4C}", foot, foot->Head, &foot->Magic);
-	if(head->File) {
-		Log_Log("Heap", "%sowned by %s:%i",
-			(head->Magic==MAGIC_FREE?"was ":""), head->File, head->Line);
 	}
-	Log_Log("Heap", "");
-	#endif
-	
 	
+	// If not verbose, we need to dump the failing block
+	if( !bVerbose )
+	{
+		Log_Log("Heap", "%p (%P): 0x%08lx %i %4C",
+			head, MM_GetPhysAddr(head), head->Size, head->ValidSize, &head->Magic);
+		if(foot)
+			Log_Log("Heap", "Foot %p = {Head:%p,Magic:%4C}", foot, foot->Head, &foot->Magic);
+		if(head->File) {
+			Log_Log("Heap", "%sowned by %s:%i",
+				(head->Magic==MAGIC_FREE?"was ":""), head->File, head->Line);
+		}
+		Log_Log("Heap", "");
+	}
+		
 	badHead = head;
 	
 	// Work backwards
-	foot = (void*)( (tVAddr)gHeapEnd - sizeof(tHeapFoot) );
+	foot = Heap_PrevFoot(gHeapEnd);
 	Log_Log("Heap", "==== Going Backwards ==== (from %p)", foot);
 	head = foot->Head;
 	while( (tVAddr)head >= (tVAddr)badHead )
@@ -623,19 +816,16 @@ void Heap_Dump(void)
 		
 		if(head == badHead)	break;
 		
-		foot = (void*)( (tVAddr)head - sizeof(tHeapFoot) );
+		foot = Heap_PrevFoot(head);
 		head = foot->Head;
 		Log_Debug("Heap", "head=%p", head);
 	}
 	
-	Panic("Heap_Dump - Heap is corrupted, kernel panic!");
+	Panic("Heap_Dump - Heap is corrupted, kernel panic! (%p)", badHead);
 }
-#endif
 
-#if 1
 void Heap_Stats(void)
 {
-	tHeapHead	*head;
 	 int	nBlocks = 0;
 	 int	nFree = 0;
 	 int	totalBytes = 0;
@@ -643,10 +833,7 @@ void Heap_Stats(void)
 	 int	maxAlloc=0, minAlloc=-1;
 	 int	avgAlloc, frag, overhead;
 	
-	for(head = gHeapStart;
-		(Uint)head < (Uint)gHeapEnd;
-		head = (void*)( (Uint)head + head->Size )
-		)
+	for( tHeapHead *head = gHeapStart; head < gHeapEnd; head = Heap_NextHead(head) )
 	{	
 		nBlocks ++;
 		totalBytes += head->Size;
@@ -672,7 +859,8 @@ void Heap_Stats(void)
 				head->Data, MM_GetPhysAddr(&head->Data), head->Size);
 		else
 			Log_Debug("Heap", "%p (%P) - 0x%x (%i) Owned by %s:%i (%lli ms old)",
-				head->Data, MM_GetPhysAddr(&head->Data), head->Size, head->ValidSize, head->File, head->Line,
+				head->Data, MM_GetPhysAddr(&head->Data), head->Size,
+				head->ValidSize, head->File, head->Line,
 				now() - head->AllocateTime
 				);
 		#endif
@@ -711,10 +899,7 @@ void Heap_Stats(void)
 		
 		memset(sizeCounts, 0, nBlocks*sizeof(sizeCounts[0]));
 		
-		for(head = gHeapStart;
-			(Uint)head < (Uint)gHeapEnd;
-			head = (void*)( (Uint)head + head->Size )
-			)
+		for(tHeapHead *head = gHeapStart; head < gHeapEnd; head = (void*)( (tVAddr)head + head->Size ) )
 		{
 			for( i = 0; i < nBlocks; i ++ ) {
 				if( sizeCounts[i].Size == 0 )
@@ -726,12 +911,12 @@ void Heap_Stats(void)
 			if( i == nBlocks )	continue;
 			sizeCounts[i].Size = head->Size;
 			sizeCounts[i].Count ++;
-			#if 1
-			//Log("Heap_Stats: %i %p - 0x%x bytes (%s) (%i)", nBlocks, head,
-			//	head->Size, (head->Magic==MAGIC_FREE?"FREE":"used"), i
-			//	);
-			//Log("Heap_Stats: sizeCounts[%i] = {Size:0x%x, Count: %i}", i,
-			//	sizeCounts[i].Size, sizeCounts[i].Count);
+			#if 0
+			Log("Heap_Stats: %i %p - 0x%x bytes (%s) (%i)", nBlocks, head,
+				head->Size, (head->Magic==MAGIC_FREE?"FREE":"used"), i
+				);
+			Log("Heap_Stats: sizeCounts[%i] = {Size:0x%x, Count: %i}", i,
+				sizeCounts[i].Size, sizeCounts[i].Count);
 			#endif
 		}
 		
@@ -744,7 +929,6 @@ void Heap_Stats(void)
 	}
 	#endif
 }
-#endif
 
 // === EXPORTS ===
 EXPORT(Heap_Allocate);