void *Heap::CAlloc(size_t aCount, size_t aSize)
{
void * ret = NULL;
Block * prev = NULL;
Block * curr = NULL;
uint16_t size = static_cast<uint16_t>(aCount * aSize);
VerifyOrExit(size);
size += kAlignSize - 1 - kBlockRemainderSize;
size &= ~(kAlignSize - 1);
size += kBlockRemainderSize;
prev = &BlockSuper();
curr = &BlockNext(*prev);
while (curr->GetSize() < size)
{
prev = curr;
curr = &BlockNext(*curr);
}
VerifyOrExit(curr->IsFree());
prev->SetNext(curr->GetNext());
if (curr->GetSize() > size + sizeof(Block))
{
const uint16_t newBlockSize = curr->GetSize() - size - sizeof(Block);
curr->SetSize(size);
Block &newBlock = BlockRight(*curr);
newBlock.SetSize(newBlockSize);
newBlock.SetNext(0);
if (prev->GetSize() < newBlockSize)
{
BlockInsert(*prev, newBlock);
}
else
{
BlockInsert(BlockSuper(), newBlock);
}
}
curr->SetNext(0);
memset(curr->GetPointer(), 0, size);
ret = curr->GetPointer();
exit:
return ret;
}
Block &Heap::BlockPrev(const Block &aBlock)
{
Block *prev = &BlockSuper();
while (prev->GetNext() != BlockOffset(aBlock))
{
prev = &BlockNext(*prev);
}
return *prev;
}
void Heap::Free(void *aPointer)
{
if (aPointer == NULL)
{
return;
}
Block &block = BlockOf(aPointer);
Block &right = BlockRight(block);
if (IsLeftFree(block))
{
Block *prev = &BlockSuper();
Block *left = &BlockNext(*prev);
for (const uint16_t offset = block.GetLeftNext(); left->GetNext() != offset; left = &BlockNext(*left))
{
prev = left;
}
// Remove left from free list.
prev->SetNext(left->GetNext());
left->SetNext(0);
if (right.IsFree())
{
if (right.GetSize() > left->GetSize())
{
for (const uint16_t offset = BlockOffset(right); prev->GetNext() != offset; prev = &BlockNext(*prev))
;
}
else
{
prev = &BlockPrev(right);
}
// Remove right from free list.
prev->SetNext(right.GetNext());
right.SetNext(0);
// Add size of right.
left->SetSize(left->GetSize() + right.GetSize() + sizeof(Block));
}
// Add size of current block.
left->SetSize(left->GetSize() + block.GetSize() + sizeof(Block));
BlockInsert(*prev, *left);
}
else
{
if (right.IsFree())
{
Block &prev = BlockPrev(right);
prev.SetNext(right.GetNext());
block.SetSize(block.GetSize() + right.GetSize() + sizeof(Block));
BlockInsert(prev, block);
}
else
{
BlockInsert(BlockSuper(), block);
}
}
}
/**
* This method returns whether the heap is clean.
*
*/
bool IsClean(void) {
const Block &super = BlockSuper();
const Block &first = BlockRight(super);
return super.GetNext() == BlockOffset(first) && first.GetSize() == kFirstBlockSize;
}