// 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_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;

	TUint32 total_user_size;

	memset(slb, 0, sizeof(slb));

	__NK_ASSERT_ALWAYS(imin_size == 16);

	__NK_ASSERT_ALWAYS(insl == 16);

	__NK_ASSERT_ALWAYS(il2min == 4);

	__NK_ASSERT_ALWAYS(il2nsl == 4);

	__NK_ASSERT_ALWAYS(infl == __e32_find_ms1_32(itotal_size) - il2min + 1);

	a = (TUint32)&islb[infl];

	a = (a+63)&~63;

	__NK_ASSERT_ALWAYS(isll == (SFreeBlock**)a);

	a += insl * infl * sizeof(TUint32*);

	__NK_ASSERT_ALWAYS(iff == (SBlock*)a);

	total_user_size = itotal_size - (a - (TUint32)this);

	do	{

		szs = b->size;

		size = szs & BLOCK_SIZE_MASK;

		__NK_ASSERT_ALWAYS(b->predecessor == pb);

		__NK_ASSERT_ALWAYS(size > 0);

		__NK_ASSERT_ALWAYS(size <= total_user_size);

		__NK_ASSERT_ALWAYS(size == ((size >> il2min) << il2min));

		total_block_size += size;

		++block_count;

		pb = b;

		b = (SBlock*)((TUint32)b + size);

		} while(!(szs & BLOCK_STATUS_FINAL));

	__NK_ASSERT_ALWAYS((TUint32)b == (TUint32)this + itotal_size);

	__NK_ASSERT_ALWAYS(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;

			TUint32 lhi;

			SFreeBlock* lh;

			SFreeBlock* pfb = fb;

			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_ALWAYS(fli == fli2);

			__NK_ASSERT_ALWAYS(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_ALWAYS(fb->prev == pfb);

				__NK_ASSERT_ALWAYS(fb->b.size & BLOCK_STATUS_FREE);

				} while ((fb != (SFreeBlock*)b) && --c>=0);

			__NK_ASSERT_ALWAYS(fb == (SFreeBlock*)b);

			__NK_ASSERT_ALWAYS(lh_found);

			total_free += size;

			}

		b = (SBlock*)((TUint32)b + size);

		} while(!(szs & BLOCK_STATUS_FINAL));

	__NK_ASSERT_ALWAYS(flb == iflb);

	for (fli=0; fli<infl; ++fli)

		{

		__NK_ASSERT_ALWAYS(slb[fli] == islb[fli]);

		if (!(flb & (1u<<fli)))

			__NK_ASSERT_ALWAYS(slb[fli]==0);

		for (sli=0; sli<insl; ++sli)

			{

			TUint32 lhi = (fli << il2nsl) | sli;

			if (!(slb[fli] & (1u<<sli)))

				__NK_ASSERT_ALWAYS(!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;

	}