efimm/Heap.c

   1 #include <efi.h>
   2 #include <efiapi.h>
   3 #include <HashVector.h>
   4
   5 #define LARGE_HEAP_SIZE (1048576)
   6
   7 #define CEILX(V,D) ((int)(((V)+(D)-1)/(D)))
   8 #define CEIL(V,D) (CEILX(V,D)*(D))
   9
  10 extern EFI_BOOT_SERVICES *boot_services;
  11
  12 static void *real_malloc(size_t size) {
  13     size_t pages = CEILX( size, EFI_PAGE_SIZE );
  14     EFI_PHYSICAL_ADDRESS p = 0;
  15     if ( boot_services->AllocatePages(
  16              AllocateAnyPages, EfiLoaderData, pages, &p ) != EFI_SUCCESS ) {
  17         return 0;
  18     }
  19     return (void*) p;
  20 }
  21
  22 static void real_free(void *p,size_t size) {
  23     size_t pages = CEILX( size, EFI_PAGE_SIZE );
  24     (void) boot_services->FreePages( (EFI_PHYSICAL_ADDRESS) p, pages );
  25 }
  26
  27 static void memclear(char *p,size_t size) {
  28     char *end = p + size;
  29     while ( p < end ) {
  30         *(p++) = 0;
  31     }
  32 }
  33
  34 typedef struct ChunkHead {
  35     size_t size;
  36     struct ChunkHead *data;
  37 } ChunkHead;
  38
  39 static ChunkHead *free_chunks;
  40
  41 void* malloc(size_t size) {
  42     // Adjust allocation size to include a preceding size count
  43     size_t asize = CEIL( size + sizeof( size_t ), sizeof( ChunkHead ) );
  44     if ( asize >= LARGE_HEAP_SIZE ) {
  45         // Managed without subdivision
  46         ChunkHead *p = real_malloc( asize );
  47         p->size = asize;
  48         return (void*) &p->data;
  49     }
  50     // Cut out a chunk by subdivision adjusted to an even number of
  51     // ChunkHead;
  52     ChunkHead *this = free_chunks;
  53     ChunkHead *prev = 0;
  54     for ( ; this ; prev = this, this = this->data ) {
  55         if ( this->size < asize ) {
  56             continue;
  57         }
  58         // Cut out an allocation chunk here,
  59         size_t rest = this->size - asize;
  60         ChunkHead *next = this->data;
  61         if ( rest >= sizeof( ChunkHead ) ) {
  62             next = (ChunkHead *) (((char*)this) + asize);
  63             next->data = this->data;
  64             next->size = rest;
  65         }
  66         if ( prev ) {
  67             prev->data = next;
  68         } else {
  69             free_chunks = next;
  70         }
  71         memclear( (char*) this, sizeof( ChunkHead ) );
  72         this->size = asize;
  73         return (void*) ((char*)this) + sizeof( size_t );
  74     }
  75     // No subdivision available; add a new subdivision page and try
  76     // again. Note that this contains its own ChunkHead.
  77     this = (ChunkHead *) real_malloc( LARGE_HEAP_SIZE );
  78     memclear( (char*) this, LARGE_HEAP_SIZE );
  79     this->size = LARGE_HEAP_SIZE;
  80     this->data = free_chunks;
  81     free_chunks = this;
  82     return malloc( size );
  83 }
  84
  85 void free(void *p) {
  86     if ( p == 0 ) {
  87         return;
  88     }
  89     ChunkHead *chp = (ChunkHead *) ( ((char*) p) - sizeof( size_t ) );
  90     if ( chp->size >= LARGE_HEAP_SIZE ) {
  91         real_free( p, chp->size );
  92         return;
  93     }
  94     // find nearest ChunkHead in the free_chunks list.
  95     ChunkHead *this = free_chunks;
  96     ChunkHead *prev = 0;
  97     for ( ; this; prev = this, this = this->data ) {
  98         if ( (void*) this >= p ) {
  99             break;
 100         }
 101     }
 102     this = chp;
 103     if ( prev == 0 ) {
 104         this->data = free_chunks;
 105         free_chunks = this;
 106     } else {
 107         this->data = prev->data;
 108     }
 109     if ( ((char*)prev->data) + prev->size == (char*)this ) {
 110         // collapse into single area
 111         prev->size += this->size;
 112         this = prev;
 113     }
 114     // Maybe this emptied a subdivision block, except the last?
 115     if ( this->data && ((char*)this) + this->size == (char*)(this->data) ) {
 116         this->size += this->data->size;
 117     }
 118     // Yes. That should also be freed
 119     if ( this->size == LARGE_HEAP_SIZE || this->data ) {
 120         this = free_chunks;
 121         prev = 0;
 122         for ( ; this; prev = this, this = this->data ) {
 123             if ( this == chp ) {
 124                 if ( prev ) {
 125                     prev->data = this->data;
 126                 } else {
 127                     free_chunks = this->data;
 128                 }
 129                 real_free( this, this->size );
 130                 return;
 131             }
 132         }
 133     }
 134 }