1 // Copyright (c) 2005-2009 Nokia Corporation and/or its subsidiary(-ies).

     2 // All rights reserved.

     3 // This component and the accompanying materials are made available

     4 // under the terms of the License "Eclipse Public License v1.0"

     5 // which accompanies this distribution, and is available

     6 // at the URL "http://www.eclipse.org/legal/epl-v10.html".

     7 //

     8 // Initial Contributors:

     9 // Nokia Corporation - initial contribution.

    10 //

    11 // Contributors:

    12 //

    13 // Description:

    14 // x86pc\nkern\tlsf.cpp

    15 // 

    16 //

    17 

    18 #include <nktest/nkutils.h>

    19 

    20 extern "C" {

    21 extern TLinAddr RomHeaderAddress;

    22 extern TLinAddr SuperPageAddress;

    23 }

    24 

    25 #define	BLOCK_STATUS_FREE	1u	// bit set if SBlock free

    26 #define	BLOCK_STATUS_FINAL	2u	// bit set if this is last physical SBlock

    27 #define BLOCK_SIZE_MASK		0xfffffffcu

    28 

    29 struct SBlock

    30 	{

    31 	TUint32 size;					// bits 2-31 give size, bits 0,1 are status bits

    32 	SBlock* predecessor;		// previous SBlock in physical address order

    33 	};

    34 

    35 struct SFreeBlock

    36 	{

    37 	SBlock b;

    38 	SFreeBlock* next;			// next SBlock in same bucket, circular list

    39 	SFreeBlock* prev;			// previous SBlock in same bucket, circular list

    40 	};

    41 

    42 struct STlsfAllocator

    43 	{

    44 public:

    45 	static STlsfAllocator* Create(TAny* aBase, TUint32 aTotalSize);

    46 	TAny* Alloc(TUint32 aSize);

    47 	void Free(TAny* aCell);

    48 	TAny* ReAlloc(TAny* aCell, TUint32 aSize);

    49 

    50 	void Init(TUint32 aTotalSize);

    51 	void InsertFree(SFreeBlock* aB);

    52 	TUint32 MapSize(TUint32 aSize, TUint32* p_sli, TInt round_up);

    53 	SFreeBlock* FindFree(TUint32 aSize);

    54 	void RemoveFree(SFreeBlock* aB);

    55 	TUint32 ConsistencyCheck();

    56 public:

    57 	TUint32	imin_size;			// minimum SBlock size

    58 	TUint32 itotal_size;		// total size of allocated and free blocks

    59 	TUint32 il2min;				// log2 min size

    60 	TUint32 infl;				// number of first level lists

    61 	TUint32 insl;				// number of second level lists

    62 	TUint32 il2nsl;

    63 	SFreeBlock** isll;			// pointer to first second level list pointer for minimum size

    64 	TUint32 iflb;				// first level bitmap

    65 	SBlock* iff;				// first free address

    66 	TUint32 islb[1];			// second level bitmaps, one for each first level list

    67 								// second level lists follow, nsl pointers for each first level list

    68 	};

    69 

    70 STlsfAllocator* TheAllocator;

    71 NFastMutex AllocMutex;

    72 

    73 STlsfAllocator* STlsfAllocator::Create(void* aBase, TUint32 aTotalSize)

    74 	{

    75 	STlsfAllocator* p = (STlsfAllocator*)aBase;

    76 	p->Init(aTotalSize);

    77 	return p;

    78 	}

    79 

    80 void STlsfAllocator::Init(TUint32 total_size)

    81 	{

    82 	TUint32 a;

    83 	imin_size = 16;

    84 	il2min = 4;

    85 	insl = 16;

    86 	il2nsl = 4;

    87 	itotal_size = total_size;

    88 	infl = __e32_find_ms1_32(total_size) - il2min + 1;

    89 	iflb = 0;

    90 	a = (TUint32)&islb[infl];

    91 	a = (a+63)&~63;

    92 	isll = (SFreeBlock**)a;

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

    94 	iff = (SBlock*)a;

    95 	a -= (TUint32)this;

    96 	memset(islb, 0, total_size - sizeof(STlsfAllocator) + sizeof(TUint32));

    97 	iff->size = (total_size - a) | BLOCK_STATUS_FINAL;

    98 	iff->predecessor = 0;

    99 	Free(iff+1);

   100 	}

   101 

   102 // size already rounded up to multiple of min_size

   103 TUint32 STlsfAllocator::MapSize(TUint32 size, TUint32* p_sli, TInt round_up)

   104 	{

   105 	TUint32 sli = size >> il2min;

   106 	TUint32 fli = __e32_find_ms1_32(sli);

   107 	sli -= (1u<<fli);

   108 	if (fli > il2nsl)

   109 		{

   110 		TUint32 sls = (fli - il2nsl);

   111 		TUint32 sz2 = sli;

   112 		sli >>= sls;

   113 		if (round_up && ((sli << sls) < sz2) )

   114 			{

   115 			if (++sli == insl)

   116 				sli=0, ++fli;

   117 			}

   118 		}

   119 	*p_sli = sli;

   120 	return fli;

   121 	}

   122 

   123 SFreeBlock* STlsfAllocator::FindFree(TUint32 size)

   124 	{

   125 	TUint32 sli;

   126 	TUint32 fli = MapSize(size, &sli, 1);

   127 	TUint32 sli2;

   128 	TUint32 fli2;

   129 	SFreeBlock** sll;

   130 	SFreeBlock* b;

   131 	TUint32 act_sz;

   132 

   133 	if ((iflb >> fli) & 1)

   134 		{

   135 		if ((islb[fli]>>sli)==0)

   136 			++fli, sli=0;

   137 		}

   138 	else

   139 		sli = 0;

   140 	if (fli >= infl)

   141 		return 0;

   142 	fli2 = __e32_find_ls1_32(iflb >> fli);

   143 	if ((TInt)fli2 < 0)

   144 		return 0;

   145 	fli2 += fli;

   146 	sli2 = __e32_find_ls1_32(islb[fli2] >> sli) + sli;	// must find a 1

   147 	sll = &isll[(fli2 << il2nsl) | sli2];

   148 	b = *sll;

   149 	if (b->next == b)

   150 		{

   151 		// list now empty

   152 		*sll = 0;

   153 		if ( (islb[fli2] &= ~(1u<<sli2)) == 0 )

   154 			iflb &= ~(1u<<fli2);

   155 		}

   156 	else

   157 		{

   158 		*sll = b->next;

   159 		b->next->prev = b->prev;

   160 		b->prev->next = b->next;

   161 		}

   162 	act_sz = b->b.size & BLOCK_SIZE_MASK;

   163 	if (act_sz > size)

   164 		{

   165 		// free the extra

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

   167 		nfb->size = (act_sz - size) | (b->b.size & BLOCK_STATUS_FINAL);

   168 		nfb->predecessor = &b->b;

   169 		if (!(b->b.size & BLOCK_STATUS_FINAL))

   170 			{

   171 			// if this isn't final SBlock, update predecessor of following SBlock

   172 			SBlock* nb = (SBlock*)((TUint32)b + act_sz);

   173 			nb->predecessor = nfb;

   174 			}

   175 		InsertFree((SFreeBlock*)nfb);

   176 		b->b.size = 0;	// allocated SBlock can't be final

   177 		}

   178 	b->b.size &= BLOCK_STATUS_FINAL;

   179 	b->b.size |= size;

   180 	return b;

   181 	}

   182 

   183 void STlsfAllocator::InsertFree(SFreeBlock* b)

   184 	{

   185 	TUint32 size = b->b.size & BLOCK_SIZE_MASK;

   186 	TUint32 sli;

   187 	TUint32 fli = MapSize(size, &sli, 0);

   188 	SFreeBlock** sll = &isll[(fli << il2nsl) | sli];

   189 	if (*sll)

   190 		{

   191 		SFreeBlock* first = *sll;

   192 		b->next = first;

   193 		b->prev = first->prev;

   194 		first->prev->next = b;

   195 		first->prev = b;

   196 		}

   197 	else

   198 		{

   199 		b->next = b;

   200 		b->prev = b;

   201 		islb[fli] |= (1u<<sli);

   202 		iflb |= (1u<<fli);

   203 		}

   204 	*sll = b;

   205 	b->b.size |= BLOCK_STATUS_FREE;

   206 	}

   207 

   208 void STlsfAllocator::RemoveFree(SFreeBlock* b)

   209 	{

   210 	TUint32 size = b->b.size & BLOCK_SIZE_MASK;

   211 	TUint32 sli;

   212 	TUint32 fli = MapSize(size, &sli, 0);

   213 	SFreeBlock** sll = &isll[(fli << il2nsl) | sli];

   214 	if (b->next != b)

   215 		{

   216 		if (*sll == b)

   217 			*sll = b->next;

   218 		b->next->prev = b->prev;

   219 		b->prev->next = b->next;

   220 		return;

   221 		}

   222 	*sll = 0;

   223 	if ( (islb[fli] &= ~(1u<<sli)) == 0)

   224 		iflb &= ~(1u<<fli);

   225 	}

   226 

   227 TAny* STlsfAllocator::Alloc(TUint32 size)

   228 	{

   229 	SFreeBlock* b;

   230 	TUint32 msm = imin_size - 1;

   231 

   232 	size = (size + sizeof(SBlock) + msm) &~ msm;

   233 

   234 	b = FindFree(size);

   235 	if (b)

   236 		return &b->next;

   237 	return 0;

   238 	}

   239 

   240 void STlsfAllocator::Free(TAny* cell)

   241 	{

   242 	SBlock* b = ((SBlock*)cell) - 1;

   243 //	SFreeBlock* fb = (SFreeBlock*)b;

   244 	TUint32 size;

   245 	if (!cell)

   246 		return;

   247 	size = b->size & BLOCK_SIZE_MASK;

   248 	if (!(b->size & BLOCK_STATUS_FINAL))

   249 		{

   250 		// not last SBlock

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

   252 		TUint32 nbs = nb->size;

   253 		if (nbs & BLOCK_STATUS_FREE)

   254 			{

   255 			// following SBlock is free

   256 			RemoveFree((SFreeBlock*)nb);

   257 			b->size = size + (nbs &~ BLOCK_STATUS_FREE);	// keeps final flag from following SBlock

   258 			size = b->size & BLOCK_SIZE_MASK;

   259 			if (!(nbs & BLOCK_STATUS_FINAL))

   260 				{

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

   262 				nb->predecessor = b;

   263 				}

   264 			}

   265 		}

   266 	if (b->predecessor && b->predecessor->size & BLOCK_STATUS_FREE)

   267 		{

   268 		// predecessor SBlock is free

   269 		TUint32 psz = b->predecessor->size & BLOCK_SIZE_MASK;

   270 		RemoveFree((SFreeBlock*)b->predecessor);

   271 		psz += b->size; // keeps final flag if necessary

   272 		b->predecessor->size = psz;

   273 		b = b->predecessor;

   274 		if (!(psz & BLOCK_STATUS_FINAL))

   275 			{

   276 			// need to adjust prev pointer of following SBlock

   277 			SBlock* nb = (SBlock*)((TUint32)b + psz);

   278 			nb->predecessor = b;

   279 			}

   280 		}

   281 	InsertFree((SFreeBlock*)b);

   282 	}

   283 

   284 TAny* STlsfAllocator::ReAlloc(TAny* cell, TUint32 newsize)

   285 	{

   286 	SBlock* b;

   287 	TUint32 size;

   288 	TUint32 msm;

   289 	SBlock* nb;

   290 	TAny* newcell;

   291 

   292 	if (!cell)

   293 		return (newsize>0) ? Alloc(newsize) : 0;

   294 	if (newsize == 0)

   295 		{

   296 		Free(cell);

   297 		return 0;

   298 		}

   299 	b = ((SBlock*)cell) - 1;

   300 	size = b->size & BLOCK_SIZE_MASK;

   301 	msm = imin_size - 1;

   302 	newsize = (newsize + sizeof(SBlock) + msm) &~ msm;

   303 	if (newsize > size)

   304 		{

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

   306 		if (nb->size & BLOCK_STATUS_FREE)

   307 			{

   308 			// following SBlock is free

   309 			TUint32 nbs = nb->size;

   310 			if (nbs + size >= newsize)

   311 				{

   312 				// we can expand in place - grab the entire free SBlock for now

   313 				// it will be split if necessary in the next section of code

   314 				RemoveFree((SFreeBlock*)nb);

   315 				b->size = size + (nbs &~ BLOCK_STATUS_FREE);	// keeps final flag from following SBlock

   316 				size = b->size & BLOCK_SIZE_MASK;

   317 				if (!(nbs & BLOCK_STATUS_FINAL))

   318 					{

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

   320 					nnb->predecessor = b;

   321 					}

   322 				}

   323 			}

   324 		}

   325 	if (newsize == size)

   326 		return cell;

   327 	if (newsize < size)

   328 		{

   329 		// shrinking - split SBlock

   330 		TUint32 final = b->size & BLOCK_STATUS_FINAL;

   331 		SBlock* nfb = (SBlock*)((TUint32)b + newsize);

   332 		nfb->size = (size - newsize) | final;

   333 		nfb->predecessor = b;

   334 		if (!final)

   335 			{

   336 			// if this isn't final SBlock, update predecessor of following SBlock

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

   338 			nb->predecessor = nfb;

   339 			}

   340 		b->size = newsize;	// original SBlock can't be final

   341 		Free((SBlock*)nfb + 1);

   342 		return cell;

   343 		}

   344 

   345 	// must move SBlock to expand

   346 	newcell = Alloc(newsize);

   347 	if (newcell)

   348 		{

   349 		memcpy(newcell, cell, size);

   350 		Free(cell);

   351 		}

   352 	return newcell;

   353 	}

   354 

   355 TUint32 STlsfAllocator::ConsistencyCheck()

   356 	{

   357 	TUint32 a;

   358 	TUint32 szs = 0;

   359 	TUint32 size;

   360 	TUint32 total_user_size;

   361 	TUint32 total_block_size = 0;

   362 	TUint32 block_count = 0;

   363 	TUint32 flb = 0;

   364 	TUint32 slb[32];

   365 	TUint32 sli;

   366 	TUint32 fli;

   367 	TUint32 total_free = 0;

   368 	SBlock* b = iff;

   369 	SBlock* pb = 0;

   370 

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

   372 	__NK_ASSERT_DEBUG(imin_size == 16);

   373 	__NK_ASSERT_DEBUG(insl == 16);

   374 	__NK_ASSERT_DEBUG(il2min == 4);

   375 	__NK_ASSERT_DEBUG(il2nsl == 4);

   376 	__NK_ASSERT_DEBUG(infl == __e32_find_ms1_32(itotal_size) - il2min + 1);

   377 	a = (TUint32)&islb[infl];

   378 	a = (a+63)&~63;

   379 	__NK_ASSERT_DEBUG(isll == (SFreeBlock**)a);

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

   381 	__NK_ASSERT_DEBUG(iff == (SBlock*)a);

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

   383 

   384 	do	{

   385 		szs = b->size;

   386 		size = szs & BLOCK_SIZE_MASK;

   387 		__NK_ASSERT_DEBUG(b->predecessor == pb);

   388 		__NK_ASSERT_DEBUG(size > 0);

   389 		__NK_ASSERT_DEBUG(size <= total_user_size);

   390 		__NK_ASSERT_DEBUG(size == ((size >> il2min) << il2min));

   391 		total_block_size += size;

   392 		++block_count;

   393 		pb = b;

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

   395 		} while(!(szs & BLOCK_STATUS_FINAL));

   396 	__NK_ASSERT_DEBUG((TUint32)b == (TUint32)this + itotal_size);

   397 	__NK_ASSERT_DEBUG(total_block_size == total_user_size);

   398 

   399 	b = iff;

   400 	do	{

   401 		szs = b->size;

   402 		size = szs & BLOCK_SIZE_MASK;

   403 		if (szs & BLOCK_STATUS_FREE)

   404 			{

   405 			SFreeBlock* fb = (SFreeBlock*)b;

   406 			SFreeBlock* pfb = fb;

   407 			SFreeBlock* lh;

   408 			TUint32 lhi;

   409 			TInt lh_found = 0;

   410 			TInt c = (TInt)block_count;

   411 			TUint32 fli = __e32_find_ms1_32(size) - il2min;

   412 			TUint32 sli = (size >> il2min) - (1u << fli);

   413 			TUint32 sli2;

   414 			TUint32 fli2 = MapSize(size, &sli2, 0);

   415 			(void)sli2, (void)fli2;

   416 			if (fli > il2nsl)

   417 				sli >>= (fli - il2nsl);

   418 			__NK_ASSERT_DEBUG(fli == fli2);

   419 			__NK_ASSERT_DEBUG(sli == sli2);

   420 			flb |= (1u << fli);

   421 			slb[fli] |= (1u << sli);

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

   423 			lh = isll[lhi];

   424 			do	{

   425 				if (fb == lh)

   426 					lh_found = 1;

   427 				pfb = fb;

   428 				fb = fb->next;

   429 				__NK_ASSERT_DEBUG(fb->prev == pfb);

   430 				__NK_ASSERT_DEBUG(fb->b.size & BLOCK_STATUS_FREE);

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

   432 			__NK_ASSERT_DEBUG(fb == (SFreeBlock*)b);

   433 			__NK_ASSERT_DEBUG(lh_found);

   434 			total_free += size;

   435 			}

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

   437 		} while(!(szs & BLOCK_STATUS_FINAL));

   438 

   439 	__NK_ASSERT_DEBUG(flb == iflb);

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

   441 		{

   442 		__NK_ASSERT_DEBUG(slb[fli] == islb[fli]);

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

   444 			__NK_ASSERT_DEBUG(slb[fli]==0);

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

   446 			{

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

   448 			(void)lhi;

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

   450 				__NK_ASSERT_DEBUG(!isll[lhi]);

   451 			}

   452 		}

   453 	return total_free;

   454 	}

   455 

   456 #define __E32CMN_H__

   457 

   458 #undef EXPORT_C

   459 #define EXPORT_C /* */

   460 

   461 class TVersion

   462 	{

   463 public:

   464 	TInt8 iMajor;

   465 	TInt8 iMinor;

   466 	TInt16 iBuild;

   467 	};

   468 

   469 class TDesC;

   470 

   471 #include <e32rom.h>

   472 #include "kernboot.h"

   473 

   474 extern "C" {

   475 void SetupMemoryAllocator()

   476 	{

   477 	const SSuperPageBase& sp = *(const SSuperPageBase*)SuperPageAddress;

   478 	const TRomHeader& romHdr = *(const TRomHeader*)RomHeaderAddress;

   479 	const TRomEntry* primaryEntry = (const TRomEntry*)sp.iPrimaryEntry;

   480 	const TRomImageHeader* primaryImageHeader = (const TRomImageHeader*)primaryEntry->iAddressLin;

   481 	TLinAddr stack = romHdr.iKernDataAddress + round_to_page(romHdr.iTotalSvDataSize);

   482 	TLinAddr heapbase = stack + round_to_page(primaryImageHeader->iStackSize);

   483 	TLinAddr heapsize = sp.iInitialHeapSize;

   484 

   485 	KPrintf("Heap base %08x size %08x", heapbase, heapsize);

   486 	TheAllocator = STlsfAllocator::Create((TAny*)heapbase, heapsize);

   487 	}

   488 

   489 extern TBool InitialThreadDefined;

   490 

   491 TAny* malloc(TUint32 aSize)

   492 	{

   493 //	__KTRACE_OPT(KMMU,DEBUGPRINT("malloc(%d)",aSize));

   494 	if (InitialThreadDefined)

   495 		NKern::FMWait(&AllocMutex);

   496 	TAny* p = TheAllocator->Alloc(aSize);

   497 	if (InitialThreadDefined)

   498 		NKern::FMSignal(&AllocMutex);

   499 	__KTRACE_OPT(KMMU,DEBUGPRINT("malloc(%d)->%08x",aSize,p));

   500 	return p;

   501 	}

   502 

   503 void free(TAny* aCell)

   504 	{

   505 	__KTRACE_OPT(KMMU,DEBUGPRINT("free(%08x)",aCell));

   506 #ifdef _DEBUG

   507 	if (aCell)

   508 		{

   509 		SBlock* b = (SBlock*)aCell;

   510 		TUint32 size = b[-1].size & BLOCK_SIZE_MASK;

   511 		memset(aCell, 0xde, size - sizeof(SBlock));

   512 		}

   513 #endif

   514 	NKern::FMWait(&AllocMutex);

   515 	TheAllocator->Free(aCell);

   516 	NKern::FMSignal(&AllocMutex);

   517 	}

   518 

   519 TAny* realloc(TAny* aCell, TUint32 aSize)

   520 	{

   521 	NKern::FMWait(&AllocMutex);

   522 	TAny* p = TheAllocator->ReAlloc(aCell, aSize);

   523 	NKern::FMSignal(&AllocMutex);

   524 	return p;

   525 	}

   526 }

   527