/******************************************************
 *          Physical Memory Manager                   *
 ******************************************************/
#include <system.h>
#include <string.h>
#include "mmngr_ph.h"

#define PMMNGR_BLOCK_SIZE 4096      // block size (4k)
#define PMMNGR_BLOCK_ALIGN PMMNGR_BLOCK_SIZE // block alignment

struct memory_stack_entry{
      word low;
      byte high;
} __attribute__ ((__packed__));
typedef struct memory_stack_entry mstack;

static uint32_t _mmngr_memory_size=0;     // size of physical memory
static uint32_t _mmngr_used_blocks=0;     // number of blocks currently in use
static uint32_t _mmngr_max_blocks=0;      // maximum number of available memory blocks
static uint32_t _mmngr_index = 0;

static mstack* _mmngr_memory_stack= 0;    // memory stack

inline mstack mstack_pop ()
{
      mstack temp;
      temp.low = _mmngr_memory_stack[--_mmngr_index].low;
      temp.high = _mmngr_memory_stack[_mmngr_index].high;
      _mmngr_used_blocks++;
      
      _mmngr_memory_stack[_mmngr_index].low = 0xFFFF;
      _mmngr_memory_stack[_mmngr_index].high = 0xFF;
      
      return temp;
}


inline void mstack_push (const mstack *block)
{
      if (block->low == 0 &&  block-> high == 0) return;
      
      _mmngr_memory_stack[_mmngr_index].low = block->low;
      _mmngr_memory_stack[_mmngr_index].high = block->high;

      _mmngr_index++;
      _mmngr_used_blocks--;
}


inline byte mstack_test (const mstack *block)
{
      uint32_t i;
      for (i = 0; i < _mmngr_index; i++)
            if (_mmngr_memory_stack[i].low == block->low && _mmngr_memory_stack[i].high == block->high) return 1;
               
      return 0;
}

byte pmmngr_test_block (uint32_t block)
{
      mstack temp;
      temp.low = block & 0xFFFF;
      temp.high = (block>>16) & 0xFF;
      
      return mstack_test(&temp);
}

void pmmngr_free_block(void* address)
{
      // Calculate block
      mstack block;
      uint32_t temp = (uint32_t)address / PMMNGR_BLOCK_SIZE;
      block.low = temp & 0xFFFF;
      block.high = (temp>>16) & 0xFF;
      
      // Push it
      mstack_push (&block);
}


void* pmmngr_alloc_block()
{
      if (_mmngr_index == 0) return 0;// Out of memory
      // pop a block
      mstack block = mstack_pop();
      
      // Calculate and return address;
      void* address;
      uint32_t temp = block.low | (block.high<<16);
      address = (void *)(temp * PMMNGR_BLOCK_SIZE);
      
      return address;
}


void pmmngr_init (size_t memSize, uint32_t stack) {

      _mmngr_memory_size = memSize;
      _mmngr_memory_stack = (mstack*) stack;
      _mmngr_max_blocks = (_mmngr_memory_size*1024) / PMMNGR_BLOCK_SIZE;
      _mmngr_used_blocks = _mmngr_max_blocks;
      _mmngr_index = 0;

      // By default, all of memory is in use
}

void pmmngr_init_region (physical_addr base, size_t size) {

      mstack block;


      unsigned int count = size / PMMNGR_BLOCK_SIZE;
      unsigned int start = base / PMMNGR_BLOCK_SIZE;

      for (; count!=0; count--) {
            block.low = (start + count) & 0xFFFF;
            block.high = ((start + count) << 16) & 0xFF;
            mstack_push(&block);
            
      }
      
}

void pmmngr_deinit_region (physical_addr base, size_t size) {
      unsigned int start = base / PMMNGR_BLOCK_SIZE;
      unsigned int count = size / PMMNGR_BLOCK_SIZE;
      int temp;

      unsigned int i;
      unsigned int j;
      
      // Find free blocks in the area and zero them
      for (i = 0; i < _mmngr_index; i++) {
            temp = (_mmngr_memory_stack[i].high << 16) | _mmngr_memory_stack[i].low;
            if (temp >=start && temp < start+count)
            {     _mmngr_memory_stack[i].high = 0;
                  _mmngr_memory_stack[i].low = 0;
            }
      }

      // Eliminate zero blocks
      for (i = 0; i<_mmngr_index; i++)
            if (_mmngr_memory_stack[i].high == 0 && _mmngr_memory_stack[i].low == 0)
            {
                  // Find next non-zero
                  for (j = i; _mmngr_memory_stack[j].high == 0 && _mmngr_memory_stack[j].low == 0; j++)
                        if (j == _mmngr_index-1) {
                              j = 0; break; }
                  
                  if (j == 0) {
                        _mmngr_index = i;
                        break;
                        }
                  
                  _mmngr_memory_stack[i].high = _mmngr_memory_stack[j].high;
                  _mmngr_memory_stack[i].low = _mmngr_memory_stack[j].low;
                  _mmngr_memory_stack[j].high = 0;
                  _mmngr_memory_stack[j].low = 0;

            }
}


size_t pmmngr_get_memory_size () {
      return _mmngr_memory_size;
}

uint32_t pmmngr_get_block_count () {
      return _mmngr_max_blocks;
}

uint32_t pmmngr_get_use_block_count () {
      return _mmngr_used_blocks;
}

uint32_t pmmngr_get_free_block_count () {
      return _mmngr_index;
}

uint32_t pmmngr_get_block_size () {
      return PMMNGR_BLOCK_SIZE;
}

void pmmngr_paging_enable (byte b) {
      uint32_t temp;

      temp = read_cr0();
      // Enable
      if (b) temp |= 0x80000000;
      else temp &= ~0x80000000;
      
      write_cr0(temp);
}


byte pmmngr_is_paging () {
      uint32_t temp = read_cr0();
      return ((temp&0x80000000)>0);
}

mstack* pmmngr_get_stack_addr()
{
      return _mmngr_memory_stack;
}