#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

#define MEM_BITMAP_SIZE 32768
//#define MEM_BITMAP_SIZE 327

// memory map bit array. Each bit represents a memory block
//uint32_t _mmngr_memory_map[MEM_BITMAP_SIZE];
uint32_t *_mmngr_memory_map;

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< MEM_BITMAP_SIZE ; 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<MEM_BITMAP_SIZE;i++)
    {
	_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* pmmngr_alloc_block () 
{

    int frame = mmap_first_free ();
 
    if (frame == -1)
    {
	scr_put_string_nl("OUT OF MEM!");
	return 0;	//out of memory
    }

//    scr_put_string_nl("MEM ALLOC OK!");
//    scr_put_hex32(frame);
 //   scr_put_string_nl("");
 
    mmap_set (frame);
 
    uint32_t addr = frame * PMMNGR_BLOCK_SIZE;
    mem_free_blocks--;
 
    return (void*)addr;
}

void pmmngr_free_block (void* p) {
 
    uint32_t addr = (uint32_t*)p;
    int frame = addr / PMMNGR_BLOCK_SIZE;
 
    if(mmap_test(frame))
    {
	mmap_unset (frame);
	mem_free_blocks++;
    }
 
}

/*
void mem_test_3()
{
	uint32_t *addr;

	addr=pmmngr_alloc_block();
	scr_put_string("alloc 1: ");
	scr_put_hex32(addr);
	scr_put_string_nl("");

	addr=pmmngr_alloc_block();
	scr_put_string("alloc 2: ");
	scr_put_hex32(addr);
	scr_put_string_nl("");
}

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

    int cnt=-1;
    uint32_t *addr;
    uint32_t *lastaddr;

    do{
	lastaddr=addr;
	addr=pmmngr_alloc_block();
	cnt++;
    }while(addr!=0);


    scr_put_string("no mem after : ");
    scr_put_hex32(cnt);
    scr_put_string_nl("");

    pmmngr_free_block(lastaddr);


    cnt=-1;
    do{
	lastaddr=addr;
	addr=pmmngr_alloc_block();
	cnt++;
    }while(addr!=0);


    scr_put_string("no mem after : ");
    scr_put_hex32(cnt);
    scr_put_string_nl("");
}

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)
{

    _mmngr_memory_map=0x9000;
    mem_free_blocks=0;
    pmmngr_init ();

    // count available memory
    uint32_t avail_mem=0;

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


#ifdef MEM_PRINT_MEMORYMAP
	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("");
#endif


	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("mem: 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("mem: Total of free (4KB) blocks: ");
    scr_put_hex(mem_free_blocks>>16);
    scr_put_hex(mem_free_blocks&0xffff);
    scr_put_string_nl("");


}