#define MAP_BASE  0x10000000 //Start at 256mb mark
#define PAGE_BASE 0x10020000 //Start page map
#define MEM_BASE  0x10020400 //There are 2^20 pages, starting here

#define MICRO_PAGES 0x00100000 //Micro Pages/Mem Base (4kb/each)
#define SUPER_PAGES 0x00000400 //Super Pages/Start of each page (4mb/each)

unsigned long *MicroMap = MAP_BASE;
unsigned long *PageMap = PAGE_BASE;

const int CheckAND[] = { //So we can & to see what page is used in the longs (set of 32)
2^0, 2^1, 2^2, 2^3, 2^4, 2^5, 2^6, 2^7, 2^8, 2^9, 2^10, 2^11,
2^12, 2^13, 2^14, 2^15, 2^16, 2^17, 2^18, 2^19, 2^20, 2^21,
2^22, 2^23, 2^24, 2^25, 2^26, 2^27, 2^28, 2^29, 2^30, 2^31};

void* AllocPage(){                 //Allocate 4kb block
int i = 0, x = 0, Page = 0;       //i and x are counting vars, Page is superpage to use
void* newpage;                    //so we can return the final pointer
while(PageMap[i] == 0xFFFFFFFF){  //While the Super Page is used in this list 1-32...
  i++;                             //Keep searching
  if(i >= SUPER_PAGES)return NULL; //No more room, abort!
}                                 //We leave 'i' with set of 32 that has free superpage
for(x=0; x<32; x++){              //Now search through set of 32 for that free one
  if( !(PageMap[i] & CheckAND[x]) ){ //if this is a free of the 32 super pages....
   Page = x + i*32;                //make this our page, (rem., 'x' is within 32,
   break;                          // so make it like a two-dim array like [super][mini])
  }
}
i = 0;                            //Reset counter caus' we saved it in Page
while(MicroMap[i+Page*1024] == 0xFFFFFFFF){ //Search for free Micropage set in Superpage
  i++;                                     //Haven't found one yet
  if(i >= MICRO_PAGES/SUPER_PAGES)return NULL; //Should never get her unless weird error
}                                             //Since it said there was 1(or more) free
                                               //up there in the for loop
for(x=0; x<32; x++){                          //Find Page within set of 32 micropages
  if(! (MicroMap[i+Page*1024] & CheckAND[x]) ){ //Found one?
   newpage = (void*)(MEM_BASE+((i+Page*1024)*32+x)*4096); //Make ptr. to the actual mem
   MicroMap[i+Page*1024] |= CheckAND[x];             //Mark it as used....
   return newpage;                                //Return a page of free memory!
  }                                               //(Finally)
}
}

//---------------------------
//   global vars
uint32_t mem_total_4kb= 1024;
uint32_t mem_total_4mb= 0;
uint32_t *bitmap_4kb= (uint32_t *)0xc0001000;
uint32_t *bitmap_4mb_1= (uint32_t *)0xc0021000;
uint32_t *bitmap_4mb_2= (uint32_t *)0xc0021080;
uint32_t free_first_4kb=0x100;
uint32_t free_first_4mb=1;
uint32_t free_4kb=0;
uint32_t free_4mb=0;

#define set_bit_makro(x,bit_nr) x|= (1 << bit_nr)
#define del_bit_makro(x,bit_nr) x&= ~(1 << bit_nr)
#define get_bit_makro(x,bit_nr) x& (1 << bit_nr)

uint32_t alloc_4kb_phys()
{
   register uint32_t x,y;
   uint32_t phys_addr;

   if(free_4kb == 0)
      return 0;

   x= free_first_4kb >> 5;
   y= free_first_4kb & 0x1f;

   del_bit_makro(bitmap_4kb[x],y);

   phys_addr= free_first_4kb << 12;

   free_4kb--;
   if(free_4kb == 0)
      return phys_addr;
   /////////////////////////
   //we need a new free 4MiB page
   if((free_first_4kb >> 10) == free_first_4mb)
   {
      x= free_first_4mb >> 5;
      y= free_first_4mb & 0x1f;
      del_bit_makro(bitmap_4mb_1[x],y);
      while(1)
      {
         if(bitmap_4mb_1[x] != 0)
            break;
         x++;
      }
      asm("bsf %0,%1"
         :"=r"(y)
         :"r"(bitmap_4mb_1[x]));
      free_first_4mb= (x << 5) + y;
      free_4mb--;
   }
   //end search of new 4MiB page
   /////////////////////////
   //we need a new 4KiB page
   x= free_first_4kb >> 5;
   while(1)
   {
      if(bitmap_4kb[x] != 0)
         break;
      x++;
      /////////////////////////
      //the old 4MiB page is full => search a new one with >0 free 4KiB pages
      if((x & 0x1f) == 0)
      {
         x>>= 5;
         x--;
         y= x & 0x1f;
         x>>= 5;
         del_bit_makro(bitmap_4mb_2[x],y);
         while(1)
         {
            if(bitmap_4mb_2[x] != 0)
               break;
            x++;
         }
         asm("bsf %0,%1"
            :"=r"(y)
            :"r"(bitmap_4mb_2[x]));
         x= ((x << 5) + y) << 5;
      }
      //end search for 4MiB page with >0 4KiB pages
      /////////////////////////
   }
   asm("bsf %0,%1"
      :"=r"(y)
      :"r"(bitmap_4kb[x]));
   free_first_4kb= (x << 5) + y;
   //end search of new 4KiB page
   /////////////////////////

   return phys_addr;
}

uint32_t dealloc_4kb_phys(uint32_t addr)
{
   register uint32_t x,y;

   if((addr & 0xfff) != 0)
      return 0;

   x= addr >> 12;
   y= x & 0x1f;
   x>>= 5;

   if(get_bit_makro(bitmap_4kb[x],y))
      return 0;

   set_bit_makro(bitmap_4kb[x],y);

   x>>= 5;
   y= x & 0x1f;
   x>>= 5;
   set_bit_makro(bitmap_4mb_2[x],y);

   free_4kb++;

   if((addr >> 12) < free_first_4kb)
      free_first_4kb= addr >> 12;
   /////////////////////////
   //check if the whole 4MiB page is free now
   x= addr >> 17;
   for(y= 0; y <= 31; y++)
      if(bitmap_4kb[x+y] == 0)
         return 1;
   /////////////////////////
   //set 4MiB page free
   x>>= 5;
   y= x & 0x1f;
   x>>= 5;

   set_bit_makro(bitmap_4mb_1[x],y);

   if(((x << 5) + y) < free_first_4mb)
      free_first_4mb= (x << 5) + y;

   free_4mb++;

   return 1;
}

uint32_t alloc_4mb_phys()
{
   register uint32_t x,y,z;
   uint32_t phys_addr;

   if(free_4mb == 0)
      return 0;

   x= free_first_4mb >> 5;
   y= free_first_4mb & 0x1f;

   del_bit_makro(bitmap_4mb_1[x],y);
   del_bit_makro(bitmap_4mb_2[x],y);

   for(z= 0; z <= 31; z++)
   {
      bitmap_4kb[(((x << 5) + y) << 5) + z]= 0xffffffff;
   }

   phys_addr= ((x << 5) + y) << 22;

   free_4mb--;
   free_4kb-= 1024;
   if(free_4mb == 0)
      return phys_addr;

   if((free_first_4kb >> 10) == free_first_4mb)
   {
      /////////////////////////
      //we need a new free 4KiB page
      x= 0;
      while(1)
      {
         if(bitmap_4mb_2[x] != 0)
            break;
         x++;
      }

      asm("bsf %0,%1"
         :"=r"(y)
         :"r"(bitmap_4mb_2[x]));
      x= ((x << 5) + y) << 5;
      while(1)
      {
         if(bitmap_4kb[x] != 0)
            break;
         x++;
      }
      asm("bsf %0,%1"
         :"=r"(y)
         :"r"(bitmap_4kb[x]));
      free_first_4kb= (x << 5) + y;
      //end search
      /////////////////////////
   }
   /////////////////////////
   //find new free 4MiB page
   x= 0;
   while(1)
   {
      if(bitmap_4mb_1[x] != 0)
         break;
      x++;
   }
   //end search
   /////////////////////////
   asm("bsf %0,%1"
      :"=r"(y)
      :"r"(bitmap_4mb_1[x]));
   free_first_4mb= (x << 5) + y;

   return phys_addr;
}

uint32_t dealloc_4mb_phys(uint32_t addr)
{
   register uint32_t x,y;

   if((addr & 0x3fffff) != 0)
      return 0;

   x= addr >> 17;
   y= (x >> 5) & 0x1f;

   if(get_bit_makro(bitmap_4mb_1[x>>10],y))
      return 0;

   for(y= 0; y <= 31; y++)
      bitmap_4kb[x+y]= 0xffffffff;

   x>>= 5;
   y= x & 0x1f;
   x>>= 5;
   set_bit_makro(bitmap_4mb_1[x],y);
   set_bit_makro(bitmap_4mb_2[x],y);

   free_4mb++;
   free_4kb+= 1024;

   x= addr >> 12;
   if(x < free_first_4kb)
      free_first_4kb= x;

   return 1;
}