/*
 * mem-phys.c
 *
 *  Created on: Aug 27, 2011
 *      Author: Tiberiu
 */
#include <memory-add.h>

uint32* BlockMap;
uint32 TotalBlocks;
uint32 TotalMemory;
uint32 UsedBlocks;

inline void ConvertIndexToBlock (uint32 index, uint32* address, uint32* offset)
{
	*address = (index >> 5);
	*offset = index & 0x1f;
}

inline uint32 ConvertBlockToIndex (uint32 address, uint32 offset)
{
	return (address<<5) | offset;
}

void MemPhSetBlock (uint32 Block, uint8 value)
{
	uint32 addr, off;
	ConvertIndexToBlock(Block, &addr, &off);

	if (value) {
		if ((BlockMap[addr] & (1<<off)) == 0) UsedBlocks++;
		BlockMap[addr] |= 1<<off;
	}
	
	else {
		if (BlockMap[addr] & (1<<off)) UsedBlocks--;
		BlockMap[addr] &= ~(1<<off);
	}
}

uint32 MemPhGetBlock (uint32 Block)
{
	uint32 addr, off;
	ConvertIndexToBlock(Block, &addr, &off);

	return BlockMap[addr] & (1<<off);
}

uint32 MemPhAllocateBlock()
{
	uint32 addr, pos;

	for (addr = 0; addr < TotalBlocks >> 5; addr++)
		if (BlockMap[addr] != 0xffffffff)
		{
			for (pos = 0; (BlockMap[addr] & (1<<pos)) != 0; pos++) ;

			uint32 index = ConvertBlockToIndex(addr, pos);
			MemPhSetBlock(index, 1);

			// Return address
			return (index<<12);
		}

	return 0xffffffff;
}

void MemPhFreeBlock(uint32 addr)
{
	uint32 Block = addr >> 12;
	if (!Block) return;

	MemPhSetBlock(Block, 0);
}

void MemPhInitialize(uint32 SystemMemoryKb)
{
	TotalBlocks = SystemMemoryKb >> 2;
	TotalMemory = SystemMemoryKb;

	BlockMap = (uint32*) kmalloc(sizeof(uint32) * (1 + (TotalBlocks>>5)));
	memset(BlockMap, 0,  sizeof(uint32) * (1 + (TotalBlocks>>5)));
	Log("Mem", "%#Started physical memory manager ok!, found %ukb\n", ColorLightGreen, SystemMemoryKb);
}

void MemPhReserveBlocks (uint32 address, uint32 length)
{
	address >>= 12;
	length = (length>>12) + ((length & 0xfff) > 0);
	uint32 end = address + length;

	for (; address < end ; address++)
		MemPhSetBlock(address, 1);
}