#include "kernel.h"

//! 8 blocks per byte
#define PMMNGR_BLOCKS_PER_BYTE 8
 
//! block size (4k)
#define PMMNGR_BLOCK_SIZE	4096
 
//! block alignment
#define PMMNGR_BLOCK_ALIGN	PMMNGR_BLOCK_SIZE

// memory map bit array. Each bit represents a memory block
static uint32_t	_mmngr_memory_map[129*1024];
static uint32_t mem_free_blocks;

// bit funcs!

void mmap_set(int bit) 
{
      _mmngr_memory_map[bit / 32] |= (1 << (bit % 32));
}

void mmap_unset(int bit)
{
      _mmngr_memory_map[bit / 32] &= ~ (1 << (bit % 32));
}

int  mmap_test(int bit)
{
     return _mmngr_memory_map[bit / 32] &  (1 << (bit % 32));
}

///

int mmap_first_free () {
 
    //! find the first free bit
    uint32_t i;
    int j;


    for (i=0; i< 128 ; i++)
	if (_mmngr_memory_map[i] != 0xffffffff)
	    for (j=0; j<32; j++) {		//! test each bit in the dword
 
		int bit = 1 << j;
		if (! (_mmngr_memory_map[i] & bit) )
		    return i*4*8+j;
	    }
 
    return -1;
}

void pmmngr_init () {
 
    //! By default, all of memory is in use
    //    memset (_mmngr_memory_map, 0xf, 128 );
    int i;
    for(i=0;i++;i<128*1024)
    {
	_mmngr_memory_map[i]=0xffffffff;
    }
}

void pmmngr_init_region (uint32_t  base, uint32_t size)
{

 
    uint32_t align = base / PMMNGR_BLOCK_SIZE;
    uint32_t blocks = size / PMMNGR_BLOCK_SIZE;

    for (; blocks>0; blocks--) 
    {
	// hack to lock first ~4MB of memory
	if(align<1000)
	{
	    align++;
	    continue;
	}


	mmap_unset (align++);
	mem_free_blocks++;
	//_mmngr_used_blocks--;
    }

    //mmap_set (0);	//first block is always set. This insures allocs cant be 0
}

void mem_test(int start, int end, int steps)
{
    int *p=start;
    for(;p<end;p+=steps)
    {
	scr_put_string("testing memory at ");
	scr_put_hex32(p);

	//
	*p=p;
	volatile x=*p;
	if(x==p) scr_put_string(" OK");
	//

	scr_put_string_nl("");
    }
}

void mem_init(uint16_t *memmap)
{
    mem_free_blocks=0;

    // count available memory
    uint32_t avail_mem=0;

    //print memory map;
    while(1)
    {
	if(memmap[8]==0)break;

	scr_put_hex(memmap[8]);
	scr_put_string(" : ");


	scr_put_hex(memmap[1]);
	scr_put_string(" ");
	scr_put_hex(memmap[0]);
	scr_put_string(" ");

	scr_put_string(" - ");
	scr_put_hex(memmap[5]);
	scr_put_string(" ");
	scr_put_hex(memmap[4]);

	scr_put_string_nl("");

	if(memmap[8]==1)
	{
	    avail_mem+=memmap[4]+(memmap[5]<<16); 
	    pmmngr_init_region(memmap[0]+(memmap[1]<<16),memmap[4]+((memmap[5])<<16));
	}

	memmap+=12;
    }

    scr_put_string("Total Available Mem: ");
    scr_put_hex(avail_mem>>16);
    scr_put_string(" ");
    scr_put_hex(avail_mem&0xffff);
    scr_put_string_nl(" byte");

    scr_put_string("Total of free (4KB) blocks: ");
    scr_put_hex(mem_free_blocks>>16);
    scr_put_hex(mem_free_blocks&0xffff);
    scr_put_string_nl("");

    //crossing 512MB boundary! 
    // (do not do this in real, coz it probably is reserved)
    //mem_test(0x1fee1000,0x2000f000,0x800);

}