/*********************************************************************
 * (c) 2010 CTA Systems Inc. All rights reserved. Glory To God       *
 *                                                                   *
 *          Physical Memory Manager                                  *
 *          =======================                                  *
 ************************************************************ cta os */

// +==============================================+
// |    HEADERS                                   |
// +===================================== cta os =+
#include "mmngr_ph.h"


// +==============================================+
// |    DEFINITIONS                               |
// +===================================== cta os =+
#define PMMNGR_BLOCK_SIZE 4096      // block size (4k)
#define PMMNGR_BLOCK_ALIGN PMMNGR_BLOCK_SIZE // block alignment


// +==============================================+
// |    DATA DECLARATIONS                         |
// +===================================== cta os =+
static unsigned _mmngr_memory_size=0;     // size of physical memory
static unsigned _mmngr_used_blocks=0;     // number of blocks currently in use
static unsigned _mmngr_max_blocks=0;      // maximum number of available memory blocks
static unsigned _mmngr_index = 0;

static mstack* _mmngr_memory_stack= 0;    // memory stack


// +==============================================+
// |    LOCAL FUNCTIONS                           |
// +===================================== cta os =+
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 int mstack_test (const mstack *block)
{
      unsigned i;
      for (i = 0; i < _mmngr_index; i++)
            if (_mmngr_memory_stack[i].low == block->low && _mmngr_memory_stack[i].high == block->high)
                  return (int) i;
               
      return -1;
}

inline int mstack_qsort_cmp (mstack a, mstack b) 
{
      return (a.high == b.high) ? (int)a.low - (int)b.low : (int)a.high - (int)b.high;
}

void mstack_qsort(int beg, int end)
{

      mstack piv; mstack tmp;
 
      int  l,r,p;
 
      while (beg<end)    // This while loop will avoid the second recursive call
      {
            l = beg; p = (beg+end)/2; r = end;
 
            piv.low = _mmngr_memory_stack[p].low;
            piv.high = _mmngr_memory_stack[p].high;
 
            while (1)
            {
                  while ((l<=r) && (mstack_qsort_cmp(_mmngr_memory_stack[l],piv) <= 0)) l++;
                  while ((l<=r) && (mstack_qsort_cmp(_mmngr_memory_stack[r],piv) > 0)) r--;
 
                  if (l>r) break;
 
                  tmp.low = _mmngr_memory_stack[l].low;
                  tmp.high = _mmngr_memory_stack[l].high;
            
                  _mmngr_memory_stack[l].low = _mmngr_memory_stack[r].low;
                  _mmngr_memory_stack[l].high = _mmngr_memory_stack[r].high;
            
                  _mmngr_memory_stack[r].low = tmp.low;
                  _mmngr_memory_stack[r].high = tmp.high;

                  if (p==r) p=l;
 
                  l++; r--;
            }
 
            _mmngr_memory_stack[p].low = _mmngr_memory_stack[r].low;
            _mmngr_memory_stack[p].high = _mmngr_memory_stack[r].high;
            
            _mmngr_memory_stack[r].low = piv.low;
            _mmngr_memory_stack[r].high = piv.high;
            r--;
 
            // Recursion on the shorter side & loop (with new indexes) on the longer
            if ((r-beg)<(end-l)) {
                  mstack_qsort(beg, r);
                  beg=l;
            }
            else {
                  mstack_qsort(l, end);
                  end=r;
            }
      }   
}


// +==============================================+
// |    DEBUGGING FUNCTIONS                       |
// +===================================== cta os =+
/*void print_stack()
{
      int i;
      for (i = 0; i < _mmngr_index; i++) \n\r (" %u", _mmngr_memory_stack[i].low);
}
extern char getch();*/

// +==============================================+
// |    INITIALISATION FUNCTIONS                  |
// +===================================== cta os =+
void pmmngr_init (unsigned memSize, unsigned 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 (unsigned base, unsigned 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 (unsigned base, unsigned 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;

            }
}


// +==============================================+
// |    MEMORY MANAGING FUNCTIONS                 |
// +===================================== cta os =+
unsigned char pmmngr_test_block (unsigned block)
{
      mstack temp;
      temp.low = block & 0xFFFF;
      temp.high = (block>>16) & 0xFF;
      
      return (mstack_test(&temp) == -1)? 0 : 1;
}

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

void pmmngr_free_blocks (unsigned base, unsigned size)
{
      mstack start, end, i;
      
      // 4k align
      base /= PMMNGR_BLOCK_SIZE;
      size /= PMMNGR_BLOCK_SIZE;
      
      // Calculate blocks
      start.low = base & 0xFFFF;
      start.high = (base >> 16) & 0xFF;
      end.low = (base + size) & 0xFFFF;
      end.high = ((base + size)>>16) & 0xFF;
      
      for (i.low = start.low, i.high = start.high;    // i = start
           i.low < end.low || i.high < end.high;)    // i != end
           {
                  // only push if block is used
                  if (mstack_test(&i) == -1) mstack_push(&i);
           
                  // increment i.high
                  if (i.low == 0xFFFF) {
                        i.low = 0; i.high++;
                        }
                  else i.low++;
           }
}

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;
      unsigned temp = block.low | (block.high<<16);
      address = (void *)(temp * PMMNGR_BLOCK_SIZE);
      
      return address;
}

void* pmmngr_alloc_blocks (unsigned blocks)
{
      // Less than 2 blocks requested
      if (blocks == 0) return 0;
      if (blocks == 1) return pmmngr_alloc_block();
      
      // Sort the stack for the next step
      mstack_qsort(0, (int)_mmngr_index);
      
      int i = (int) _mmngr_index-1;        // i = counter
      int l = 1;                           // l = number of consecutive blocks
      unsigned temp;                       // temp = temporary storage
      unsigned prev = _mmngr_memory_stack[i].low | (_mmngr_memory_stack[i].high<<16); --i;
      
      // Search consecutive blocks 
      for (i = 0; i < _mmngr_index; i++) {
            temp = _mmngr_memory_stack[i].low | (_mmngr_memory_stack[i].high<<16);

            if (temp == prev+1) l++;
            else l = 1;
            
            if (l == blocks) {
                  pmmngr_deinit_region (temp * PMMNGR_BLOCK_SIZE, blocks * PMMNGR_BLOCK_SIZE);
                  return (void*) ((temp-l+1) * PMMNGR_BLOCK_SIZE);
                  }
                  
            prev = temp;
            }

       return 0;                    // Could not find so many free blocks            
}




// +==============================================+
// |    GET DATA FUNCTIONS                        |
// +===================================== cta os =+
unsigned pmmngr_get_memory_size () {
      return _mmngr_memory_size;
}

unsigned pmmngr_get_block_count () {
      return _mmngr_max_blocks;
}

unsigned pmmngr_get_use_block_count () {
      return _mmngr_used_blocks;
}

unsigned pmmngr_get_free_block_count () {
      return _mmngr_index;
}

unsigned pmmngr_get_block_size () {
      return PMMNGR_BLOCK_SIZE;
}


// +==============================================+
// |    PAGING RELATED FUNCTIONS                  |
// +===================================== cta os =+
void pmmngr_paging_enable (unsigned char b) {
      unsigned temp;

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

unsigned char pmmngr_is_paging () {
      unsigned temp = read_cr0();
      return ((temp&0x80000000)>0);
}