Usermode/Libraries/libc.so_src/heap.c

   1 /*\r
   2 AcessOS Basic LibC\r
   3 heap.c - Heap Manager\r
   4 */\r
   5 #include <acess/sys.h>\r
   6 #include <stdlib.h>\r
   7 #include <string.h>\r
   8 #include "lib.h"\r
   9 \r
  10 // === Constants ===\r
  11 #define MAGIC   0xACE55051      //AcessOS1\r
  12 #define MAGIC_FREE      (~MAGIC)\r
  13 #define BLOCK_SIZE      16      //Minimum\r
  14 #define HEAP_INIT_SIZE  0x10000\r
  15 \r
  16 typedef unsigned int Uint;\r
  17 \r
  18 // === TYPES ===\r
  19 typedef struct {\r
  20         uint32_t        magic;\r
  21         size_t  size;\r
  22 }       heap_head;\r
  23 typedef struct {\r
  24         heap_head       *header;\r
  25         uint32_t        magic;\r
  26 }       heap_foot;\r
  27 \r
  28 // === LOCAL VARIABLES ===\r
  29 static void     *_heap_start = NULL;\r
  30 static void     *_heap_end = NULL;\r
  31 \r
  32 // === PROTOTYPES ===\r
  33 EXPORT void     *malloc(size_t bytes);\r
  34 EXPORT void     *calloc(size_t bytes, size_t count);\r
  35 EXPORT void     free(void *mem);\r
  36 EXPORT void     *realloc(void *mem, size_t bytes);\r
  37 EXPORT void     *sbrk(int increment);\r
  38 LOCAL void      *extendHeap(int bytes);\r
  39 static void     *FindHeapBase();\r
  40 LOCAL uint      brk(uintptr_t newpos);\r
  41 \r
  42 //Code\r
  43 \r
  44 /**\r
  45  \fn EXPORT void *malloc(size_t bytes)\r
  46  \brief Allocates memory from the heap space\r
  47  \param bytes   Integer - Size of buffer to return\r
  48  \return Pointer to buffer\r
  49 */\r
  50 EXPORT void *malloc(size_t bytes)\r
  51 {\r
  52         size_t  bestSize;\r
  53         size_t  closestMatch = 0;\r
  54         void    *bestMatchAddr = 0;\r
  55         heap_head       *curBlock;\r
  56         \r
  57 //      _SysDebug("&_heap_start = %p, _heap_start = %p", &_heap_start, _heap_start);\r
  58         // Initialise Heap\r
  59         if(_heap_start == NULL)\r
  60         {\r
  61                 _heap_start = sbrk(0);\r
  62                 _heap_end = _heap_start;\r
  63                 extendHeap(HEAP_INIT_SIZE);\r
  64         }\r
  65         \r
  66         curBlock = _heap_start;\r
  67 //      _SysDebug("_heap_start = %p", _heap_start);\r
  68         \r
  69         bestSize = bytes + sizeof(heap_head) + sizeof(heap_foot) + BLOCK_SIZE - 1;\r
  70         bestSize = (bestSize/BLOCK_SIZE)*BLOCK_SIZE;    //Round up to block size\r
  71         \r
  72         while( (uintptr_t)curBlock < (uintptr_t)_heap_end)\r
  73         {\r
  74                 //_SysDebug(" malloc: curBlock = 0x%x, curBlock->magic = 0x%x\n", curBlock, curBlock->magic);\r
  75                 if(curBlock->magic == MAGIC_FREE)\r
  76                 {\r
  77                         if(curBlock->size == bestSize)\r
  78                                 break;\r
  79                         if(bestSize < curBlock->size && (curBlock->size < closestMatch || closestMatch == 0)) {\r
  80                                 closestMatch = curBlock->size;\r
  81                                 bestMatchAddr = curBlock;\r
  82                         }\r
  83                 }\r
  84                 else if(curBlock->magic != MAGIC)\r
  85                 {\r
  86                         //Corrupt Heap\r
  87                         _SysDebug("malloc: Corrupt Heap\n");\r
  88                         return NULL;\r
  89                 }\r
  90                 curBlock = (heap_head*)((uintptr_t)curBlock + curBlock->size);\r
  91         }\r
  92         \r
  93         if((uintptr_t)curBlock < (uintptr_t)_heap_start) {\r
  94                 _SysDebug("malloc: Heap underrun for some reason\n");\r
  95                 return NULL;\r
  96         }\r
  97         \r
  98         //Found a perfect match\r
  99         if((uintptr_t)curBlock < (uintptr_t)_heap_end) {\r
 100                 curBlock->magic = MAGIC;\r
 101                 return (void*)((uintptr_t)curBlock + sizeof(heap_head));\r
 102         }\r
 103         \r
 104         //Out of Heap Space\r
 105         if(!closestMatch) {\r
 106                 curBlock = extendHeap(bestSize);        //Allocate more\r
 107                 if(curBlock == NULL) {\r
 108                         _SysDebug("malloc: Out of Heap Space\n");\r
 109                         return NULL;\r
 110                 }\r
 111                 curBlock->magic = MAGIC;\r
 112                 return (void*)((uintptr_t)curBlock + sizeof(heap_head));\r
 113         }\r
 114         \r
 115         //Split Block?\r
 116         if(closestMatch - bestSize > BLOCK_SIZE) {\r
 117                 heap_foot       *foot;\r
 118                 curBlock = (heap_head*)bestMatchAddr;\r
 119                 curBlock->magic = MAGIC;\r
 120                 curBlock->size = bestSize;\r
 121                 foot = (heap_foot*)(bestMatchAddr + bestSize - sizeof(heap_foot));\r
 122                 foot->header = curBlock;\r
 123                 foot->magic = MAGIC;\r
 124 \r
 125                 curBlock = (heap_head*)(bestMatchAddr + bestSize);\r
 126                 curBlock->magic = MAGIC_FREE;\r
 127                 curBlock->size = closestMatch - bestSize;\r
 128                 \r
 129                 foot = (heap_foot*)(bestMatchAddr + closestMatch - sizeof(heap_foot));\r
 130                 foot->header = curBlock;\r
 131                 \r
 132                 ((heap_head*)bestMatchAddr)->magic = MAGIC;     //mark as used\r
 133                 return (void*)(bestMatchAddr + sizeof(heap_head));\r
 134         }\r
 135         \r
 136         //Don't Split the block\r
 137         ((heap_head*)bestMatchAddr)->magic = MAGIC;\r
 138         return (void*)(bestMatchAddr+sizeof(heap_head));\r
 139 }\r
 140 \r
 141 /**\r
 142  * \fn EXPORT void *calloc(size_t bytes, size_t count)\r
 143  * \brief Allocate and zero a block of memory\r
 144  * \param __nmemb       Number of memeber elements\r
 145  * \param __size        Size of one element\r
 146  */\r
 147 EXPORT void *calloc(size_t __nmemb, size_t __size)\r
 148 {\r
 149         void    *ret = malloc(__size*__nmemb);\r
 150         if(!ret)        return NULL;\r
 151         memset(ret, 0, __size*__nmemb);\r
 152         return ret;\r
 153 }\r
 154 \r
 155 /**\r
 156  \fn EXPORT void free(void *mem)\r
 157  \brief Free previously allocated memory\r
 158  \param mem     Pointer - Memory to free\r
 159 */\r
 160 EXPORT void free(void *mem)\r
 161 {\r
 162         heap_head       *head = (void*)((intptr_t)mem-sizeof(heap_head));\r
 163         \r
 164         // Sanity please!\r
 165         if(!mem)        return;\r
 166         \r
 167         if(head->magic != MAGIC)        //Valid Heap Address\r
 168                 return;\r
 169         \r
 170         head->magic = MAGIC_FREE;\r
 171         \r
 172         //Unify Right\r
 173         if((intptr_t)head + head->size < (intptr_t)_heap_end)\r
 174         {\r
 175                 heap_head       *nextHead = (heap_head*)((intptr_t)head + head->size);\r
 176                 if(nextHead->magic == MAGIC_FREE) {     //Is the next block free\r
 177                         head->size += nextHead->size;   //Amalgamate\r
 178                         nextHead->magic = 0;    //For Security\r
 179                 }\r
 180         }\r
 181         //Unify Left\r
 182         if((intptr_t)head - sizeof(heap_foot) > (intptr_t)_heap_start)\r
 183         {\r
 184                 heap_head       *prevHead;\r
 185                 heap_foot       *prevFoot = (heap_foot *)((intptr_t)head - sizeof(heap_foot));\r
 186                 if(prevFoot->magic == MAGIC) {\r
 187                         prevHead = prevFoot->header;\r
 188                         if(prevHead->magic == MAGIC_FREE) {\r
 189                                 prevHead->size += head->size;   //Amalgamate\r
 190                                 head->magic = 0;        //For Security\r
 191                         }\r
 192                 }\r
 193         }\r
 194 }\r
 195 \r
 196 /**\r
 197  \fn EXPORT void *realloc(void *oldPos, size_t bytes)\r
 198  \brief Reallocate a block of memory\r
 199  \param bytes   Integer - Size of new buffer\r
 200  \param oldPos  Pointer - Old Buffer\r
 201  \return Pointer to new buffer\r
 202 */\r
 203 EXPORT void *realloc(void *oldPos, size_t bytes)\r
 204 {\r
 205         void *ret;\r
 206         heap_head       *head;\r
 207         \r
 208         if(oldPos == NULL) {\r
 209                 return malloc(bytes);\r
 210         }\r
 211         \r
 212         //Check for free space after block\r
 213         head = (heap_head*)((uintptr_t)oldPos-sizeof(heap_head));\r
 214         \r
 215         //Hack to used free's amagamating algorithym and malloc's splitting\r
 216         free(oldPos);\r
 217         \r
 218         //Allocate new memory\r
 219         ret = malloc(bytes);\r
 220         if(ret == NULL)\r
 221                 return NULL;\r
 222         \r
 223         //Copy Old Data\r
 224         if(ret != oldPos) {\r
 225                 memcpy(ret, oldPos, head->size-sizeof(heap_head)-sizeof(heap_foot));\r
 226         }\r
 227         \r
 228         //Return\r
 229         return ret;\r
 230 }\r
 231 \r
 232 /**\r
 233  * \fn LOCAL void *extendHeap(int bytes)\r
 234  * \brief Create a new block at the end of the heap area\r
 235  * \param bytes Integer - Size reqired\r
 236  * \return Pointer to last free block\r
 237  */\r
 238 \r
 239 LOCAL void *extendHeap(int bytes)\r
 240 {\r
 241         heap_head       *head = _heap_end;\r
 242         heap_foot       *foot;\r
 243         \r
 244         //Expand Memory Space  (Use foot as a temp pointer)\r
 245         foot = sbrk(bytes);\r
 246         if(foot == (void*)-1)\r
 247                 return NULL;\r
 248         \r
 249         //Create New Block\r
 250         // Header\r
 251         head->magic = MAGIC_FREE;       //Unallocated\r
 252         head->size = bytes;\r
 253         // Footer\r
 254         foot = _heap_end + bytes - sizeof(heap_foot);\r
 255         foot->header = head;\r
 256         foot->magic = MAGIC;\r
 257         \r
 258         //Combine with previous block if nessasary\r
 259         if(_heap_end != _heap_start && ((heap_foot*)((uintptr_t)_heap_end-sizeof(heap_foot)))->magic == MAGIC) {\r
 260                 heap_head       *tmpHead = ((heap_foot*)((uintptr_t)_heap_end-sizeof(heap_foot)))->header;\r
 261                 if(tmpHead->magic == MAGIC_FREE) {\r
 262                         tmpHead->size += bytes;\r
 263                         foot->header = tmpHead;\r
 264                         head = tmpHead;\r
 265                 }\r
 266         }\r
 267         \r
 268         _heap_end = (void*) ((uintptr_t)foot+sizeof(heap_foot));\r
 269         return head;\r
 270 }\r
 271 \r
 272 /**\r
 273  \fn EXPORT void *sbrk(int increment)\r
 274  \brief Increases the program's memory space\r
 275  \param count   Integer - Size of heap increase\r
 276  \return Pointer to start of new region\r
 277 */\r
 278 EXPORT void *sbrk(int increment)\r
 279 {\r
 280         static uintptr_t oldEnd = 0;\r
 281         static uintptr_t curEnd = 0;\r
 282 \r
 283         //_SysDebug("sbrk: (increment=%i)", increment);\r
 284 \r
 285         if (curEnd == 0) {\r
 286                 oldEnd = curEnd = (uintptr_t)FindHeapBase();\r
 287                 //_SysAllocate(curEnd); // Allocate the first page\r
 288         }\r
 289 \r
 290         //_SysDebug(" sbrk: oldEnd = 0x%x", oldEnd);\r
 291         if (increment == 0)     return (void *) curEnd;\r
 292 \r
 293         oldEnd = curEnd;\r
 294 \r
 295         // Single Page\r
 296         if( (curEnd & 0xFFF) && (curEnd & 0xFFF) + increment < 0x1000 )\r
 297         {\r
 298                 //if( curEnd & 0xFFF == 0 )\r
 299                 //{\r
 300                 //      if( !_SysAllocate(curEnd) )\r
 301                 //      {\r
 302                 //              _SysDebug("sbrk - Error allocating memory");\r
 303                 //              return (void*)-1;\r
 304                 //      }\r
 305                 //}\r
 306                 curEnd += increment;\r
 307                 //_SysDebug("sbrk: RETURN %p (single page, no alloc)", (void *) oldEnd);\r
 308                 return (void *)oldEnd;\r
 309         }\r
 310 \r
 311         increment -= curEnd & 0xFFF;\r
 312         curEnd += 0xFFF;        curEnd &= ~0xFFF;\r
 313         while( increment > 0 )\r
 314         {\r
 315                 if( !_SysAllocate(curEnd) )\r
 316                 {\r
 317                         // Error?\r
 318                         _SysDebug("sbrk - Error allocating memory");\r
 319                         return (void*)-1;\r
 320                 }\r
 321                 increment -= 0x1000;\r
 322                 curEnd += 0x1000;\r
 323         }\r
 324 \r
 325         //_SysDebug("sbrk: RETURN %p", (void *) oldEnd);\r
 326         return (void *) oldEnd;\r
 327 }\r
 328 \r
 329 /**\r
 330  * \fn EXPORT int IsHeap(void *ptr)\r
 331  */\r
 332 EXPORT int IsHeap(void *ptr)\r
 333 {\r
 334         #if 0\r
 335         heap_head       *head;\r
 336         heap_foot       *foot;\r
 337         #endif\r
 338         if( (uintptr_t)ptr < (uintptr_t)_heap_start )   return 0;\r
 339         if( (uintptr_t)ptr > (uintptr_t)_heap_end )     return 0;\r
 340         \r
 341         #if 0\r
 342         head = (void*)((Uint)ptr - 4);\r
 343         if( head->magic != MAGIC )      return 0;\r
 344         foot = (void*)( (Uint)ptr + head->size - sizeof(heap_foot) );\r
 345         if( foot->magic != MAGIC )      return 0;\r
 346         #endif\r
 347         return 1;\r
 348 }\r
 349 \r
 350 // === STATIC FUNCTIONS ===\r
 351 /**\r
 352  * Does the job of brk(0)\r
 353  */\r
 354 static void *FindHeapBase()\r
 355 {\r
 356         #if 0\r
 357         #define MAX             0xC0000000      // Address\r
 358         #define THRESHOLD       512     // Pages\r
 359         uint    addr;\r
 360         uint    stretch = 0;\r
 361         uint64_t        tmp;\r
 362         \r
 363         // Scan address space\r
 364         for(addr = 0;\r
 365                 addr < MAX;\r
 366                 addr += 0x1000\r
 367                 )\r
 368         {\r
 369                 tmp = _SysGetPhys(addr);\r
 370                 if( tmp != 0 ) {\r
 371                         stretch = 0;\r
 372                 } else {\r
 373                         stretch ++;\r
 374                         if(stretch > THRESHOLD)\r
 375                         {\r
 376                                 return (void*)( addr - stretch*0x1000 );\r
 377                         }\r
 378                 }\r
 379                 //__asm__ __volatile__ (\r
 380                 //      "push %%ebx;mov %%edx,%%ebx;int $0xAC;pop %%ebx"\r
 381                 //      ::"a"(256),"d"("%x"),"c"(addr));\r
 382         }\r
 383         \r
 384         return NULL;\r
 385         #else\r
 386         return (void*)0x00900000;\r
 387         #endif\r
 388 }\r
 389 \r
 390 LOCAL uint brk(uintptr_t newpos)\r
 391 {\r
 392         static uintptr_t        curpos;\r
 393         uint    pages;\r
 394         uint    ret = curpos;\r
 395          int    delta;\r
 396         \r
 397         _SysDebug("brk: (newpos=0x%x)", newpos);\r
 398         \r
 399         // Find initial position\r
 400         if(curpos == 0) curpos = (uintptr_t)FindHeapBase();\r
 401         \r
 402         // Get Current Position\r
 403         if(newpos == 0) return curpos;\r
 404         \r
 405         if(newpos < curpos)     return newpos;\r
 406         \r
 407         delta = newpos - curpos;\r
 408         _SysDebug(" brk: delta = 0x%x", delta);\r
 409         \r
 410         // Do we need to add pages\r
 411         if(curpos & 0xFFF && (curpos & 0xFFF) + delta < 0x1000)\r
 412                 return curpos += delta;\r
 413         \r
 414         // Page align current position\r
 415         if(curpos & 0xFFF)      delta -= 0x1000 - (curpos & 0xFFF);\r
 416         curpos = (curpos + 0xFFF) & ~0xFFF;\r
 417         \r
 418         // Allocate Pages\r
 419         pages = (delta + 0xFFF) >> 12;\r
 420         while(pages--)\r
 421         {\r
 422                 _SysAllocate(curpos);\r
 423                 curpos += 0x1000;\r
 424                 delta -= 0x1000;\r
 425         }\r
 426         \r
 427         // Bring the current position to exactly what we want\r
 428         curpos -= ((delta + 0xFFF) & ~0xFFF) - delta;\r
 429         \r
 430         return ret;     // Return old curpos\r
 431 }\r