--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kerneltest/e32test/nkernsa/tlsf.cpp	Mon Oct 19 15:55:17 2009 +0100
@@ -0,0 +1,527 @@
+// Copyright (c) 2005-2009 Nokia Corporation and/or its subsidiary(-ies).
+// All rights reserved.
+// This component and the accompanying materials are made available
+// under the terms of the License "Eclipse Public License v1.0"
+// which accompanies this distribution, and is available
+// at the URL "http://www.eclipse.org/legal/epl-v10.html".
+//
+// Initial Contributors:
+// Nokia Corporation - initial contribution.
+//
+// Contributors:
+//
+// Description:
+// x86pc\nkern\tlsf.cpp
+// 
+//
+
+#include <nktest/nkutils.h>
+
+extern "C" {
+extern TLinAddr RomHeaderAddress;
+extern TLinAddr SuperPageAddress;
+}
+
+#define	BLOCK_STATUS_FREE	1u	// bit set if SBlock free
+#define	BLOCK_STATUS_FINAL	2u	// bit set if this is last physical SBlock
+#define BLOCK_SIZE_MASK		0xfffffffcu
+
+struct SBlock
+	{
+	TUint32 size;					// bits 2-31 give size, bits 0,1 are status bits
+	SBlock* predecessor;		// previous SBlock in physical address order
+	};
+
+struct SFreeBlock
+	{
+	SBlock b;
+	SFreeBlock* next;			// next SBlock in same bucket, circular list
+	SFreeBlock* prev;			// previous SBlock in same bucket, circular list
+	};
+
+struct STlsfAllocator
+	{
+public:
+	static STlsfAllocator* Create(TAny* aBase, TUint32 aTotalSize);
+	TAny* Alloc(TUint32 aSize);
+	void Free(TAny* aCell);
+	TAny* ReAlloc(TAny* aCell, TUint32 aSize);
+
+	void Init(TUint32 aTotalSize);
+	void InsertFree(SFreeBlock* aB);
+	TUint32 MapSize(TUint32 aSize, TUint32* p_sli, TInt round_up);
+	SFreeBlock* FindFree(TUint32 aSize);
+	void RemoveFree(SFreeBlock* aB);
+	TUint32 ConsistencyCheck();
+public:
+	TUint32	imin_size;			// minimum SBlock size
+	TUint32 itotal_size;		// total size of allocated and free blocks
+	TUint32 il2min;				// log2 min size
+	TUint32 infl;				// number of first level lists
+	TUint32 insl;				// number of second level lists
+	TUint32 il2nsl;
+	SFreeBlock** isll;			// pointer to first second level list pointer for minimum size
+	TUint32 iflb;				// first level bitmap
+	SBlock* iff;				// first free address
+	TUint32 islb[1];			// second level bitmaps, one for each first level list
+								// second level lists follow, nsl pointers for each first level list
+	};
+
+STlsfAllocator* TheAllocator;
+NFastMutex AllocMutex;
+
+STlsfAllocator* STlsfAllocator::Create(void* aBase, TUint32 aTotalSize)
+	{
+	STlsfAllocator* p = (STlsfAllocator*)aBase;
+	p->Init(aTotalSize);
+	return p;
+	}
+
+void STlsfAllocator::Init(TUint32 total_size)
+	{
+	TUint32 a;
+	imin_size = 16;
+	il2min = 4;
+	insl = 16;
+	il2nsl = 4;
+	itotal_size = total_size;
+	infl = __e32_find_ms1_32(total_size) - il2min + 1;
+	iflb = 0;
+	a = (TUint32)&islb[infl];
+	a = (a+63)&~63;
+	isll = (SFreeBlock**)a;
+	a += insl * infl * sizeof(TUint32*);
+	iff = (SBlock*)a;
+	a -= (TUint32)this;
+	memset(islb, 0, total_size - sizeof(STlsfAllocator) + sizeof(TUint32));
+	iff->size = (total_size - a) | BLOCK_STATUS_FINAL;
+	iff->predecessor = 0;
+	Free(iff+1);
+	}
+
+// size already rounded up to multiple of min_size
+TUint32 STlsfAllocator::MapSize(TUint32 size, TUint32* p_sli, TInt round_up)
+	{
+	TUint32 sli = size >> il2min;
+	TUint32 fli = __e32_find_ms1_32(sli);
+	sli -= (1u<<fli);
+	if (fli > il2nsl)
+		{
+		TUint32 sls = (fli - il2nsl);
+		TUint32 sz2 = sli;
+		sli >>= sls;
+		if (round_up && ((sli << sls) < sz2) )
+			{
+			if (++sli == insl)
+				sli=0, ++fli;
+			}
+		}
+	*p_sli = sli;
+	return fli;
+	}
+
+SFreeBlock* STlsfAllocator::FindFree(TUint32 size)
+	{
+	TUint32 sli;
+	TUint32 fli = MapSize(size, &sli, 1);
+	TUint32 sli2;
+	TUint32 fli2;
+	SFreeBlock** sll;
+	SFreeBlock* b;
+	TUint32 act_sz;
+
+	if ((iflb >> fli) & 1)
+		{
+		if ((islb[fli]>>sli)==0)
+			++fli, sli=0;
+		}
+	else
+		sli = 0;
+	if (fli >= infl)
+		return 0;
+	fli2 = __e32_find_ls1_32(iflb >> fli);
+	if ((TInt)fli2 < 0)
+		return 0;
+	fli2 += fli;
+	sli2 = __e32_find_ls1_32(islb[fli2] >> sli) + sli;	// must find a 1
+	sll = &isll[(fli2 << il2nsl) | sli2];
+	b = *sll;
+	if (b->next == b)
+		{
+		// list now empty
+		*sll = 0;
+		if ( (islb[fli2] &= ~(1u<<sli2)) == 0 )
+			iflb &= ~(1u<<fli2);
+		}
+	else
+		{
+		*sll = b->next;
+		b->next->prev = b->prev;
+		b->prev->next = b->next;
+		}
+	act_sz = b->b.size & BLOCK_SIZE_MASK;
+	if (act_sz > size)
+		{
+		// free the extra
+		SBlock* nfb = (SBlock*)((TUint32)b + size);
+		nfb->size = (act_sz - size) | (b->b.size & BLOCK_STATUS_FINAL);
+		nfb->predecessor = &b->b;
+		if (!(b->b.size & BLOCK_STATUS_FINAL))
+			{
+			// if this isn't final SBlock, update predecessor of following SBlock
+			SBlock* nb = (SBlock*)((TUint32)b + act_sz);
+			nb->predecessor = nfb;
+			}
+		InsertFree((SFreeBlock*)nfb);
+		b->b.size = 0;	// allocated SBlock can't be final
+		}
+	b->b.size &= BLOCK_STATUS_FINAL;
+	b->b.size |= size;
+	return b;
+	}
+
+void STlsfAllocator::InsertFree(SFreeBlock* b)
+	{
+	TUint32 size = b->b.size & BLOCK_SIZE_MASK;
+	TUint32 sli;
+	TUint32 fli = MapSize(size, &sli, 0);
+	SFreeBlock** sll = &isll[(fli << il2nsl) | sli];
+	if (*sll)
+		{
+		SFreeBlock* first = *sll;
+		b->next = first;
+		b->prev = first->prev;
+		first->prev->next = b;
+		first->prev = b;
+		}
+	else
+		{
+		b->next = b;
+		b->prev = b;
+		islb[fli] |= (1u<<sli);
+		iflb |= (1u<<fli);
+		}
+	*sll = b;
+	b->b.size |= BLOCK_STATUS_FREE;
+	}
+
+void STlsfAllocator::RemoveFree(SFreeBlock* b)
+	{
+	TUint32 size = b->b.size & BLOCK_SIZE_MASK;
+	TUint32 sli;
+	TUint32 fli = MapSize(size, &sli, 0);
+	SFreeBlock** sll = &isll[(fli << il2nsl) | sli];
+	if (b->next != b)
+		{
+		if (*sll == b)
+			*sll = b->next;
+		b->next->prev = b->prev;
+		b->prev->next = b->next;
+		return;
+		}
+	*sll = 0;
+	if ( (islb[fli] &= ~(1u<<sli)) == 0)
+		iflb &= ~(1u<<fli);
+	}
+
+TAny* STlsfAllocator::Alloc(TUint32 size)
+	{
+	SFreeBlock* b;
+	TUint32 msm = imin_size - 1;
+
+	size = (size + sizeof(SBlock) + msm) &~ msm;
+
+	b = FindFree(size);
+	if (b)
+		return &b->next;
+	return 0;
+	}
+
+void STlsfAllocator::Free(TAny* cell)
+	{
+	SBlock* b = ((SBlock*)cell) - 1;
+//	SFreeBlock* fb = (SFreeBlock*)b;
+	TUint32 size;
+	if (!cell)
+		return;
+	size = b->size & BLOCK_SIZE_MASK;
+	if (!(b->size & BLOCK_STATUS_FINAL))
+		{
+		// not last SBlock
+		SBlock* nb = (SBlock*)((TUint32)b + size);
+		TUint32 nbs = nb->size;
+		if (nbs & BLOCK_STATUS_FREE)
+			{
+			// following SBlock is free
+			RemoveFree((SFreeBlock*)nb);
+			b->size = size + (nbs &~ BLOCK_STATUS_FREE);	// keeps final flag from following SBlock
+			size = b->size & BLOCK_SIZE_MASK;
+			if (!(nbs & BLOCK_STATUS_FINAL))
+				{
+				nb = (SBlock*)((TUint32)b + size);
+				nb->predecessor = b;
+				}
+			}
+		}
+	if (b->predecessor && b->predecessor->size & BLOCK_STATUS_FREE)
+		{
+		// predecessor SBlock is free
+		TUint32 psz = b->predecessor->size & BLOCK_SIZE_MASK;
+		RemoveFree((SFreeBlock*)b->predecessor);
+		psz += b->size; // keeps final flag if necessary
+		b->predecessor->size = psz;
+		b = b->predecessor;
+		if (!(psz & BLOCK_STATUS_FINAL))
+			{
+			// need to adjust prev pointer of following SBlock
+			SBlock* nb = (SBlock*)((TUint32)b + psz);
+			nb->predecessor = b;
+			}
+		}
+	InsertFree((SFreeBlock*)b);
+	}
+
+TAny* STlsfAllocator::ReAlloc(TAny* cell, TUint32 newsize)
+	{
+	SBlock* b;
+	TUint32 size;
+	TUint32 msm;
+	SBlock* nb;
+	TAny* newcell;
+
+	if (!cell)
+		return (newsize>0) ? Alloc(newsize) : 0;
+	if (newsize == 0)
+		{
+		Free(cell);
+		return 0;
+		}
+	b = ((SBlock*)cell) - 1;
+	size = b->size & BLOCK_SIZE_MASK;
+	msm = imin_size - 1;
+	newsize = (newsize + sizeof(SBlock) + msm) &~ msm;
+	if (newsize > size)
+		{
+		nb = (SBlock*)((TUint32)b + size);
+		if (nb->size & BLOCK_STATUS_FREE)
+			{
+			// following SBlock is free
+			TUint32 nbs = nb->size;
+			if (nbs + size >= newsize)
+				{
+				// we can expand in place - grab the entire free SBlock for now
+				// it will be split if necessary in the next section of code
+				RemoveFree((SFreeBlock*)nb);
+				b->size = size + (nbs &~ BLOCK_STATUS_FREE);	// keeps final flag from following SBlock
+				size = b->size & BLOCK_SIZE_MASK;
+				if (!(nbs & BLOCK_STATUS_FINAL))
+					{
+					SBlock* nnb = (SBlock*)((TUint32)b + size);
+					nnb->predecessor = b;
+					}
+				}
+			}
+		}
+	if (newsize == size)
+		return cell;
+	if (newsize < size)
+		{
+		// shrinking - split SBlock
+		TUint32 final = b->size & BLOCK_STATUS_FINAL;
+		SBlock* nfb = (SBlock*)((TUint32)b + newsize);
+		nfb->size = (size - newsize) | final;
+		nfb->predecessor = b;
+		if (!final)
+			{
+			// if this isn't final SBlock, update predecessor of following SBlock
+			nb = (SBlock*)((TUint32)b + size);
+			nb->predecessor = nfb;
+			}
+		b->size = newsize;	// original SBlock can't be final
+		Free((SBlock*)nfb + 1);
+		return cell;
+		}
+
+	// must move SBlock to expand
+	newcell = Alloc(newsize);
+	if (newcell)
+		{
+		memcpy(newcell, cell, size);
+		Free(cell);
+		}
+	return newcell;
+	}
+
+TUint32 STlsfAllocator::ConsistencyCheck()
+	{
+	TUint32 a;
+	TUint32 szs = 0;
+	TUint32 size;
+	TUint32 total_user_size;
+	TUint32 total_block_size = 0;
+	TUint32 block_count = 0;
+	TUint32 flb = 0;
+	TUint32 slb[32];
+	TUint32 sli;
+	TUint32 fli;
+	TUint32 total_free = 0;
+	SBlock* b = iff;
+	SBlock* pb = 0;
+
+	memset(slb, 0, sizeof(slb));
+	__NK_ASSERT_DEBUG(imin_size == 16);
+	__NK_ASSERT_DEBUG(insl == 16);
+	__NK_ASSERT_DEBUG(il2min == 4);
+	__NK_ASSERT_DEBUG(il2nsl == 4);
+	__NK_ASSERT_DEBUG(infl == __e32_find_ms1_32(itotal_size) - il2min + 1);
+	a = (TUint32)&islb[infl];
+	a = (a+63)&~63;
+	__NK_ASSERT_DEBUG(isll == (SFreeBlock**)a);
+	a += insl * infl * sizeof(TUint32*);
+	__NK_ASSERT_DEBUG(iff == (SBlock*)a);
+	total_user_size = itotal_size - (a - (TUint32)this);
+
+	do	{
+		szs = b->size;
+		size = szs & BLOCK_SIZE_MASK;
+		__NK_ASSERT_DEBUG(b->predecessor == pb);
+		__NK_ASSERT_DEBUG(size > 0);
+		__NK_ASSERT_DEBUG(size <= total_user_size);
+		__NK_ASSERT_DEBUG(size == ((size >> il2min) << il2min));
+		total_block_size += size;
+		++block_count;
+		pb = b;
+		b = (SBlock*)((TUint32)b + size);
+		} while(!(szs & BLOCK_STATUS_FINAL));
+	__NK_ASSERT_DEBUG((TUint32)b == (TUint32)this + itotal_size);
+	__NK_ASSERT_DEBUG(total_block_size == total_user_size);
+
+	b = iff;
+	do	{
+		szs = b->size;
+		size = szs & BLOCK_SIZE_MASK;
+		if (szs & BLOCK_STATUS_FREE)
+			{
+			SFreeBlock* fb = (SFreeBlock*)b;
+			SFreeBlock* pfb = fb;
+			SFreeBlock* lh;
+			TUint32 lhi;
+			TInt lh_found = 0;
+			TInt c = (TInt)block_count;
+			TUint32 fli = __e32_find_ms1_32(size) - il2min;
+			TUint32 sli = (size >> il2min) - (1u << fli);
+			TUint32 sli2;
+			TUint32 fli2 = MapSize(size, &sli2, 0);
+			(void)sli2, (void)fli2;
+			if (fli > il2nsl)
+				sli >>= (fli - il2nsl);
+			__NK_ASSERT_DEBUG(fli == fli2);
+			__NK_ASSERT_DEBUG(sli == sli2);
+			flb |= (1u << fli);
+			slb[fli] |= (1u << sli);
+			lhi = (fli << il2nsl) | sli;
+			lh = isll[lhi];
+			do	{
+				if (fb == lh)
+					lh_found = 1;
+				pfb = fb;
+				fb = fb->next;
+				__NK_ASSERT_DEBUG(fb->prev == pfb);
+				__NK_ASSERT_DEBUG(fb->b.size & BLOCK_STATUS_FREE);
+				} while ((fb != (SFreeBlock*)b) && --c>=0);
+			__NK_ASSERT_DEBUG(fb == (SFreeBlock*)b);
+			__NK_ASSERT_DEBUG(lh_found);
+			total_free += size;
+			}
+		b = (SBlock*)((TUint32)b + size);
+		} while(!(szs & BLOCK_STATUS_FINAL));
+
+	__NK_ASSERT_DEBUG(flb == iflb);
+	for (fli=0; fli<infl; ++fli)
+		{
+		__NK_ASSERT_DEBUG(slb[fli] == islb[fli]);
+		if (!(flb & (1u<<fli)))
+			__NK_ASSERT_DEBUG(slb[fli]==0);
+		for (sli=0; sli<insl; ++sli)
+			{
+			TUint32 lhi = (fli << il2nsl) | sli;
+			(void)lhi;
+			if (!(slb[fli] & (1u<<sli)))
+				__NK_ASSERT_DEBUG(!isll[lhi]);
+			}
+		}
+	return total_free;
+	}
+
+#define __E32CMN_H__
+
+#undef EXPORT_C
+#define EXPORT_C /* */
+
+class TVersion
+	{
+public:
+	TInt8 iMajor;
+	TInt8 iMinor;
+	TInt16 iBuild;
+	};
+
+class TDesC;
+
+#include <e32rom.h>
+#include "kernboot.h"
+
+extern "C" {
+void SetupMemoryAllocator()
+	{
+	const SSuperPageBase& sp = *(const SSuperPageBase*)SuperPageAddress;
+	const TRomHeader& romHdr = *(const TRomHeader*)RomHeaderAddress;
+	const TRomEntry* primaryEntry = (const TRomEntry*)sp.iPrimaryEntry;
+	const TRomImageHeader* primaryImageHeader = (const TRomImageHeader*)primaryEntry->iAddressLin;
+	TLinAddr stack = romHdr.iKernDataAddress + round_to_page(romHdr.iTotalSvDataSize);
+	TLinAddr heapbase = stack + round_to_page(primaryImageHeader->iStackSize);
+	TLinAddr heapsize = sp.iInitialHeapSize;
+
+	KPrintf("Heap base %08x size %08x", heapbase, heapsize);
+	TheAllocator = STlsfAllocator::Create((TAny*)heapbase, heapsize);
+	}
+
+extern TBool InitialThreadDefined;
+
+TAny* malloc(TUint32 aSize)
+	{
+//	__KTRACE_OPT(KMMU,DEBUGPRINT("malloc(%d)",aSize));
+	if (InitialThreadDefined)
+		NKern::FMWait(&AllocMutex);
+	TAny* p = TheAllocator->Alloc(aSize);
+	if (InitialThreadDefined)
+		NKern::FMSignal(&AllocMutex);
+	__KTRACE_OPT(KMMU,DEBUGPRINT("malloc(%d)->%08x",aSize,p));
+	return p;
+	}
+
+void free(TAny* aCell)
+	{
+	__KTRACE_OPT(KMMU,DEBUGPRINT("free(%08x)",aCell));
+#ifdef _DEBUG
+	if (aCell)
+		{
+		SBlock* b = (SBlock*)aCell;
+		TUint32 size = b[-1].size & BLOCK_SIZE_MASK;
+		memset(aCell, 0xde, size - sizeof(SBlock));
+		}
+#endif
+	NKern::FMWait(&AllocMutex);
+	TheAllocator->Free(aCell);
+	NKern::FMSignal(&AllocMutex);
+	}
+
+TAny* realloc(TAny* aCell, TUint32 aSize)
+	{
+	NKern::FMWait(&AllocMutex);
+	TAny* p = TheAllocator->ReAlloc(aCell, aSize);
+	NKern::FMSignal(&AllocMutex);
+	return p;
+	}
+}
+