71e3d6dc0c7cb754ed015aa66fc24d0bc0d7a568
[tpg/acess2.git] / Usermode / Libraries / libc.so_src / heap.c
1 /*\r
2  * Acess2 C Library\r
3  * - By John Hodge (thePowersGang)\r
4  *\r
5  * heap.c\r
6  * - Dynamic Memory (malloc/free)\r
7  */\r
8 #include <acess/sys.h>\r
9 #include <stdlib.h>\r
10 #include <string.h>\r
11 #include <assert.h>\r
12 #include <stdbool.h>\r
13 #include "lib.h"\r
14 \r
15 #if 0\r
16 # define DEBUGS(s...)   _SysDebug(s)\r
17 #else\r
18 # define DEBUGS(s...)   do{}while(0)\r
19 #endif\r
20 \r
21 // === Constants ===\r
22 #define MAGIC   0xACE55051      //AcessOS1\r
23 #define MAGIC_FREE      (~MAGIC)\r
24 #define BLOCK_SIZE      16      //Minimum\r
25 #define HEAP_INIT_SIZE  0x10000\r
26 #define DEBUG_HEAP      1\r
27 \r
28 typedef unsigned int Uint;\r
29 \r
30 // === TYPES ===\r
31 typedef struct {\r
32         uint32_t        magic;\r
33         size_t  size;\r
34         // - \r
35         #if DEBUG_HEAP\r
36         void    *Caller;\r
37         size_t  RealSize;\r
38         #endif\r
39         char    data[];\r
40 }       heap_head;\r
41 typedef struct {\r
42         heap_head       *header;\r
43         uint32_t        magic;\r
44 }       heap_foot;\r
45 \r
46 // === HELPERS ===\r
47 static inline heap_head *NEXT_HEAD(heap_head *ptr)\r
48 {\r
49         return (void*)((char*)ptr + ptr->size);\r
50 }\r
51 static inline heap_foot *THIS_FOOT(heap_head *ptr)\r
52 {\r
53         return (void*)((char*)ptr + ptr->size - sizeof(heap_foot));\r
54 }\r
55 static inline heap_foot *PREV_FOOT(heap_head *ptr)\r
56 {\r
57         return (void*)((char*)ptr - sizeof(heap_foot));\r
58 }\r
59 static inline size_t CALC_BLOCK_SIZE(size_t bytes)\r
60 {\r
61         size_t ret;\r
62         ret = bytes + sizeof(heap_head) + sizeof(heap_foot) + BLOCK_SIZE - 1;\r
63         ret = (ret/BLOCK_SIZE)*BLOCK_SIZE;      //Round up to block size\r
64         return ret;\r
65 }\r
66 \r
67 // === LOCAL VARIABLES ===\r
68 static heap_head        *_heap_start = NULL;\r
69 static heap_head        *_heap_end = NULL;\r
70 static const heap_head  _heap_zero_allocation;\r
71 \r
72 // === PROTOTYPES ===\r
73 EXPORT void     *malloc(size_t bytes);\r
74 void    *_malloc(size_t bytes, void *owner);\r
75 EXPORT void     *calloc(size_t bytes, size_t count);\r
76 bool    _libc_free(void *mem);\r
77 EXPORT void     free(void *mem);\r
78 EXPORT void     *realloc(void *mem, size_t bytes);\r
79 EXPORT void     *sbrk(int increment);\r
80 LOCAL void      *extendHeap(int bytes);\r
81 static void     *FindHeapBase();\r
82 LOCAL uint      brk(uintptr_t newpos);\r
83 EXPORT int      Heap_Validate(int bDump);\r
84 \r
85 //Code\r
86 \r
87 /**\r
88  \fn EXPORT void *malloc(size_t bytes)\r
89  \brief Allocates memory from the heap space\r
90  \param bytes   Integer - Size of buffer to return\r
91  \return Pointer to buffer\r
92 */\r
93 EXPORT void *malloc(size_t bytes)\r
94 {\r
95         return _malloc(bytes, __builtin_return_address(0));\r
96 }\r
97 \r
98 void *_malloc(size_t bytes, void *owner)\r
99 {\r
100         size_t  closestMatch = 0;\r
101         void    *bestMatchAddr = 0;\r
102 \r
103         // Initialise Heap\r
104         if(_heap_start == NULL)\r
105         {\r
106                 _heap_start = sbrk(0);\r
107                 _heap_end = _heap_start;\r
108                 extendHeap(HEAP_INIT_SIZE);\r
109         }\r
110 \r
111         // Zero bytes, return a random area (in .rodata)\r
112         if( bytes == 0 )\r
113                 return (void*)_heap_zero_allocation.data;       \r
114 \r
115         // Calculate the required block size\r
116         size_t bestSize = CALC_BLOCK_SIZE(bytes);\r
117         \r
118         heap_head       *curBlock;\r
119         for(curBlock = _heap_start; curBlock < _heap_end; curBlock = NEXT_HEAD(curBlock))\r
120         {\r
121                 //_SysDebug(" malloc: curBlock = 0x%x, curBlock->magic = 0x%x\n", curBlock, curBlock->magic);\r
122                 if(curBlock->magic == MAGIC_FREE)\r
123                 {\r
124                         // Perfect match, nice\r
125                         if(curBlock->size == bestSize)\r
126                                 break;\r
127                         // Check if this is a tighter match than the last free block\r
128                         if(bestSize < curBlock->size && (curBlock->size < closestMatch || closestMatch == 0)) {\r
129                                 closestMatch = curBlock->size;\r
130                                 bestMatchAddr = curBlock;\r
131                         }\r
132                 }\r
133                 else if(curBlock->magic != MAGIC)\r
134                 {\r
135                         //Corrupt Heap\r
136                         Heap_Validate(1);\r
137                         _SysDebug("malloc: Corrupt Heap when called by %p\n", owner);\r
138                         exit(128);\r
139                         return NULL;\r
140                 }\r
141         }\r
142         \r
143         if(curBlock < _heap_start) {\r
144                 Heap_Validate(1);\r
145                 _SysDebug("malloc: Heap underrun for some reason\n");\r
146                 exit(128);\r
147                 return NULL;\r
148         }\r
149         \r
150         //Found a perfect match\r
151         if(curBlock < _heap_end) {\r
152                 curBlock->magic = MAGIC;\r
153                 curBlock->RealSize = bytes;\r
154                 curBlock->Caller = owner;\r
155                 return curBlock->data;\r
156         }\r
157         \r
158         //Out of Heap Space\r
159         if(!closestMatch) {\r
160                 curBlock = extendHeap(bestSize);        //Allocate more\r
161                 if(curBlock == NULL) {\r
162                         _SysDebug("malloc: Out of Heap Space\n");\r
163                         return NULL;\r
164                 }\r
165                 curBlock->magic = MAGIC;\r
166                 curBlock->RealSize = bytes;\r
167                 curBlock->Caller = owner;\r
168                 DEBUGS("malloc(0x%x) = %p (extend) 0x%x", bytes, curBlock->data, bestSize);\r
169                 return curBlock->data;\r
170         }\r
171         \r
172         heap_head *besthead = (void*)bestMatchAddr;\r
173         \r
174         // Do we need to split this block?\r
175         if(closestMatch - bestSize > BLOCK_SIZE) {\r
176                 heap_foot       *foot;\r
177                 \r
178                 // Block we're going to return\r
179                 curBlock = (heap_head*)bestMatchAddr;\r
180                 curBlock->magic = MAGIC;\r
181                 curBlock->size = bestSize;\r
182                 foot = THIS_FOOT(curBlock);\r
183                 foot->header = curBlock;\r
184                 foot->magic = MAGIC;\r
185 \r
186                 // Remaining parts of the block\r
187                 curBlock = (heap_head*)(bestMatchAddr + bestSize);\r
188                 curBlock->magic = MAGIC_FREE;\r
189                 curBlock->size = closestMatch - bestSize;\r
190                 foot = THIS_FOOT(curBlock);\r
191                 foot->header = curBlock;\r
192         \r
193                 // Mark return as used  \r
194                 DEBUGS("malloc(0x%x) = %p (split) 0x%x", bytes, besthead->data, bestSize);\r
195         }\r
196         else {\r
197                 // No need to split\r
198                 DEBUGS("malloc(0x%x) = %p (reuse) 0x%x", bytes, besthead->data, besthead->size);\r
199         }\r
200         \r
201         besthead->magic = MAGIC;\r
202         besthead->RealSize = bytes;\r
203         besthead->Caller = owner;\r
204         return besthead->data;\r
205 }\r
206 \r
207 /**\r
208  * \fn EXPORT void *calloc(size_t bytes, size_t count)\r
209  * \brief Allocate and zero a block of memory\r
210  * \param __nmemb       Number of memeber elements\r
211  * \param __size        Size of one element\r
212  */\r
213 EXPORT void *calloc(size_t __nmemb, size_t __size)\r
214 {\r
215         void    *ret = _malloc(__size*__nmemb, __builtin_return_address(0));\r
216         if(!ret)        return NULL;\r
217         memset(ret, 0, __size*__nmemb);\r
218         return ret;\r
219 }\r
220 \r
221 /**\r
222  \fn EXPORT void free(void *mem)\r
223  \brief Free previously allocated memory\r
224  \param mem     Pointer - Memory to free\r
225 */\r
226 EXPORT void free(void *mem)\r
227 {\r
228         if( !_libc_free(mem) ) {\r
229                 Heap_Validate(1);\r
230                 exit(0);\r
231         }\r
232 }\r
233 \r
234 // Exported for libc++\r
235 EXPORT bool _libc_free(void *mem)\r
236 {\r
237         heap_head       *head = (heap_head*)mem - 1;\r
238 \r
239         // Free of NULL or the zero allocation does nothing\r
240         if(!mem || mem == _heap_zero_allocation.data)\r
241                 return true;\r
242         \r
243         // Sanity check the head address\r
244         if(head->magic != MAGIC) {\r
245                 if( head->magic != MAGIC_FREE ) {\r
246                         _SysDebug("Double free of %p by %p", mem, __builtin_return_address(0));\r
247                 }\r
248                 else {\r
249                         _SysDebug("Free of invalid pointer %p by ", mem, __builtin_return_address(0));\r
250                 }\r
251                 return false;\r
252         }\r
253         \r
254         head->magic = MAGIC_FREE;\r
255         DEBUGS("free(%p) : 0x%x bytes", mem, head->size);\r
256         \r
257         // Unify Right\r
258         if( NEXT_HEAD(head) < _heap_end )\r
259         {\r
260                 heap_head       *nextHead = NEXT_HEAD(head);\r
261                 // Is the next block free?\r
262                 if(nextHead->magic == MAGIC_FREE)\r
263                 {\r
264                         head->size += nextHead->size;   // Amalgamate\r
265                         THIS_FOOT(head)->header = head;\r
266                         nextHead->magic = 0;    //For Security\r
267                 }\r
268         }\r
269         \r
270         // Unify Left\r
271         if( head > _heap_start )\r
272         {\r
273                 heap_foot *prevFoot = PREV_FOOT(head);\r
274                 if( prevFoot->magic != MAGIC )\r
275                 {\r
276                         _SysDebug("Heap corruption, previous foot magic invalid");\r
277                         Heap_Validate(1);\r
278                         return false;\r
279                 }\r
280                 \r
281                 heap_head *prevHead = prevFoot->header;\r
282                 if(prevHead->magic == MAGIC_FREE)\r
283                 {\r
284                         // Amalgamate\r
285                         prevHead->size += head->size;\r
286                         THIS_FOOT(prevHead)->header = prevHead;\r
287                         // For Security\r
288                         head->magic = 0;\r
289                         prevFoot->magic = 0;\r
290                         prevFoot->header = NULL;\r
291                 }\r
292         }\r
293         \r
294         return true;\r
295 }\r
296 \r
297 /**\r
298  \fn EXPORT void *realloc(void *oldPos, size_t bytes)\r
299  \brief Reallocate a block of memory\r
300  \param bytes   Integer - Size of new buffer\r
301  \param oldPos  Pointer - Old Buffer\r
302  \return Pointer to new buffer\r
303 */\r
304 EXPORT void *realloc(void *oldPos, size_t bytes)\r
305 {\r
306         if(oldPos == NULL || oldPos == _heap_zero_allocation.data) {\r
307                 return _malloc(bytes, __builtin_return_address(0));\r
308         }\r
309 \r
310         size_t  reqd_size = CALC_BLOCK_SIZE(bytes);     \r
311 \r
312         // Check for free space within the block\r
313         // - oldPos points to just after the header, so -1 gives the header\r
314         heap_head *head = (heap_head*)oldPos - 1;\r
315         if( head->size >= reqd_size ) {\r
316                 head->RealSize = bytes;\r
317                 return oldPos;\r
318         }\r
319         \r
320         // Check for free space after the block\r
321         heap_head *nexthead = NEXT_HEAD(head);\r
322         assert( nexthead <= _heap_end );\r
323         if( nexthead != _heap_end && nexthead->magic == MAGIC_FREE && head->size + nexthead->size >= reqd_size )\r
324         {\r
325                 // Split next block\r
326                 if( head->size + nexthead->size > reqd_size )\r
327                 {\r
328                         size_t newblocksize = nexthead->size - (reqd_size - head->size);\r
329                         head->size = reqd_size;\r
330                         \r
331                         nexthead = NEXT_HEAD(head);\r
332                         nexthead->size = newblocksize;\r
333                         nexthead->magic = MAGIC_FREE;\r
334                         THIS_FOOT(nexthead)->header = nexthead;\r
335                 }\r
336                 else\r
337                 {\r
338                         head->size = reqd_size;\r
339                 }\r
340                 THIS_FOOT(head)->magic = MAGIC;\r
341                 THIS_FOOT(head)->header = head;\r
342                 head->RealSize = bytes;\r
343                 return oldPos;\r
344         }\r
345         \r
346         // TODO: Should I check for free before too? What about before AND after?\r
347         \r
348         // Allocate new memory\r
349         void *ret = _malloc(bytes, __builtin_return_address(0));\r
350         if(ret == NULL)\r
351                 return NULL;\r
352         heap_head *newhead = (heap_head*)ret - 1;\r
353         \r
354         // Copy Old Data\r
355         assert( head->size < newhead->size );\r
356         size_t copy_size = head->size-sizeof(heap_head)-sizeof(heap_foot);\r
357         memcpy(ret, oldPos, copy_size);\r
358         free(oldPos);\r
359         \r
360         //Return\r
361         return ret;\r
362 }\r
363 \r
364 /**\r
365  * \fn LOCAL void *extendHeap(int bytes)\r
366  * \brief Create a new block at the end of the heap area\r
367  * \param bytes Integer - Size reqired\r
368  * \return Pointer to last free block\r
369  */\r
370 \r
371 LOCAL void *extendHeap(int bytes)\r
372 {\r
373         heap_head       *head = _heap_end;\r
374         heap_foot       *foot;\r
375         \r
376         //Expand Memory Space  (Use foot as a temp pointer)\r
377         foot = sbrk(bytes);\r
378         if(foot == (void*)-1)\r
379                 return NULL;\r
380         \r
381         //Create New Block\r
382         // Header\r
383         head->magic = MAGIC_FREE;       //Unallocated\r
384         head->size = bytes;\r
385         // Footer\r
386         foot = THIS_FOOT(head);\r
387         foot->header = head;\r
388         foot->magic = MAGIC;\r
389         \r
390         //Combine with previous block if nessasary\r
391         if(_heap_end != _heap_start && ((heap_foot*)((uintptr_t)_heap_end-sizeof(heap_foot)))->magic == MAGIC) {\r
392                 heap_head       *tmpHead = ((heap_foot*)((uintptr_t)_heap_end-sizeof(heap_foot)))->header;\r
393                 if(tmpHead->magic == MAGIC_FREE) {\r
394                         tmpHead->size += bytes;\r
395                         foot->header = tmpHead;\r
396                         head = tmpHead;\r
397                 }\r
398         }\r
399         \r
400         _heap_end = (void*) ((uintptr_t)foot+sizeof(heap_foot));\r
401         return head;\r
402 }\r
403 \r
404 /**\r
405  \fn EXPORT void *sbrk(int increment)\r
406  \brief Increases the program's memory space\r
407  \param count   Integer - Size of heap increase\r
408  \return Pointer to start of new region\r
409 */\r
410 EXPORT void *sbrk(int increment)\r
411 {\r
412         static uintptr_t oldEnd = 0;\r
413         static uintptr_t curEnd = 0;\r
414 \r
415         //_SysDebug("sbrk: (increment=%i)", increment);\r
416 \r
417         if (curEnd == 0) {\r
418                 oldEnd = curEnd = (uintptr_t)FindHeapBase();\r
419                 //_SysAllocate(curEnd); // Allocate the first page\r
420         }\r
421 \r
422         //_SysDebug(" sbrk: oldEnd = 0x%x", oldEnd);\r
423         if (increment == 0)     return (void *) curEnd;\r
424 \r
425         oldEnd = curEnd;\r
426 \r
427         // Single Page\r
428         if( (curEnd & 0xFFF) && (curEnd & 0xFFF) + increment < 0x1000 )\r
429         {\r
430                 //if( curEnd & 0xFFF == 0 )\r
431                 //{\r
432                 //      if( !_SysAllocate(curEnd) )\r
433                 //      {\r
434                 //              _SysDebug("sbrk - Error allocating memory");\r
435                 //              return (void*)-1;\r
436                 //      }\r
437                 //}\r
438                 curEnd += increment;\r
439                 //_SysDebug("sbrk: RETURN %p (single page, no alloc)", (void *) oldEnd);\r
440                 return (void *)oldEnd;\r
441         }\r
442 \r
443         increment -= curEnd & 0xFFF;\r
444         curEnd += 0xFFF;        curEnd &= ~0xFFF;\r
445         while( increment > 0 )\r
446         {\r
447                 if( !_SysAllocate(curEnd) )\r
448                 {\r
449                         // Error?\r
450                         _SysDebug("sbrk - Error allocating memory");\r
451                         return (void*)-1;\r
452                 }\r
453                 increment -= 0x1000;\r
454                 curEnd += 0x1000;\r
455         }\r
456 \r
457         //_SysDebug("sbrk: RETURN %p", (void *) oldEnd);\r
458         return (void *) oldEnd;\r
459 }\r
460 \r
461 /**\r
462  * \fn EXPORT int IsHeap(void *ptr)\r
463  */\r
464 EXPORT int IsHeap(void *ptr)\r
465 {\r
466         if( ptr == _heap_zero_allocation.data )\r
467                 return 1;\r
468         #if 0\r
469         heap_head       *head;\r
470         heap_foot       *foot;\r
471         #endif\r
472         if( (uintptr_t)ptr < (uintptr_t)_heap_start )   return 0;\r
473         if( (uintptr_t)ptr > (uintptr_t)_heap_end )     return 0;\r
474         \r
475         #if 0\r
476         head = (void*)((Uint)ptr - 4);\r
477         if( head->magic != MAGIC )      return 0;\r
478         foot = (void*)( (Uint)ptr + head->size - sizeof(heap_foot) );\r
479         if( foot->magic != MAGIC )      return 0;\r
480         #endif\r
481         return 1;\r
482 }\r
483 \r
484 // === STATIC FUNCTIONS ===\r
485 /**\r
486  * Does the job of brk(0)\r
487  */\r
488 static void *FindHeapBase()\r
489 {\r
490         #if 0\r
491         #define MAX             0xC0000000      // Address\r
492         #define THRESHOLD       512     // Pages\r
493         uint    addr;\r
494         uint    stretch = 0;\r
495         uint64_t        tmp;\r
496         \r
497         // Scan address space\r
498         for(addr = 0;\r
499                 addr < MAX;\r
500                 addr += 0x1000\r
501                 )\r
502         {\r
503                 tmp = _SysGetPhys(addr);\r
504                 if( tmp != 0 ) {\r
505                         stretch = 0;\r
506                 } else {\r
507                         stretch ++;\r
508                         if(stretch > THRESHOLD)\r
509                         {\r
510                                 return (void*)( addr - stretch*0x1000 );\r
511                         }\r
512                 }\r
513                 //__asm__ __volatile__ (\r
514                 //      "push %%ebx;mov %%edx,%%ebx;int $0xAC;pop %%ebx"\r
515                 //      ::"a"(256),"d"("%x"),"c"(addr));\r
516         }\r
517         \r
518         return NULL;\r
519         #else\r
520         return (void*)0x00900000;\r
521         #endif\r
522 }\r
523 \r
524 LOCAL uint brk(uintptr_t newpos)\r
525 {\r
526         static uintptr_t        curpos;\r
527         uint    pages;\r
528         uint    ret = curpos;\r
529          int    delta;\r
530         \r
531         _SysDebug("brk: (newpos=0x%x)", newpos);\r
532         \r
533         // Find initial position\r
534         if(curpos == 0) curpos = (uintptr_t)FindHeapBase();\r
535         \r
536         // Get Current Position\r
537         if(newpos == 0) return curpos;\r
538         \r
539         if(newpos < curpos)     return newpos;\r
540         \r
541         delta = newpos - curpos;\r
542         _SysDebug(" brk: delta = 0x%x", delta);\r
543         \r
544         // Do we need to add pages\r
545         if(curpos & 0xFFF && (curpos & 0xFFF) + delta < 0x1000)\r
546                 return curpos += delta;\r
547         \r
548         // Page align current position\r
549         if(curpos & 0xFFF)      delta -= 0x1000 - (curpos & 0xFFF);\r
550         curpos = (curpos + 0xFFF) & ~0xFFF;\r
551         \r
552         // Allocate Pages\r
553         pages = (delta + 0xFFF) >> 12;\r
554         while(pages--)\r
555         {\r
556                 _SysAllocate(curpos);\r
557                 curpos += 0x1000;\r
558                 delta -= 0x1000;\r
559         }\r
560         \r
561         // Bring the current position to exactly what we want\r
562         curpos -= ((delta + 0xFFF) & ~0xFFF) - delta;\r
563         \r
564         return ret;     // Return old curpos\r
565 }\r
566 \r
567 int Heap_Validate(int bDump)\r
568 {\r
569          int    rv = 0;\r
570         heap_head *cur = _heap_start;\r
571         while( cur < (heap_head*)_heap_end && rv == 0 )\r
572         {\r
573                 if( cur->magic == MAGIC ) {\r
574                         if( bDump ) {\r
575                                 _SysDebug("Used block %p[0x%x] - ptr=%p,Owner=%p,Size=0x%x",\r
576                                         cur, cur->size, cur->data,\r
577                                         cur->Caller, cur->RealSize\r
578                                         );\r
579                         }\r
580                 }\r
581                 else if( cur->magic == MAGIC_FREE ) {\r
582                         if( bDump ) {\r
583                                 _SysDebug("Free block %p[0x%x]", cur, cur->size, cur->data);\r
584                         }\r
585                 }\r
586                 else {\r
587                         _SysDebug("Block %p bad magic (0x%x)", cur, cur->magic);\r
588                         rv = 1;\r
589                         break ;\r
590                 }\r
591                 heap_foot *foot = THIS_FOOT(cur);\r
592                 if( foot->magic != MAGIC ) {\r
593                         _SysDebug("- %p: Foot magic clobbered (0x%x!=0x%x)", cur, foot->magic, MAGIC);\r
594                         rv = 1;\r
595                 }\r
596                 if( foot->header != cur ) {\r
597                         _SysDebug("- %p: Head pointer clobbered (%p)", cur, foot->header);\r
598                         rv = 1;\r
599                 }\r
600         \r
601                 if( rv && !bDump ) {\r
602                         _SysDebug("%s block %p[0x%x] - ptr=%p,Owner=%p,Size=0x%x",\r
603                                 (cur->magic == MAGIC ? "Used":"Free"),\r
604                                 cur, cur->size, cur->data,\r
605                                 cur->Caller, cur->RealSize\r
606                                 );\r
607                 }       \r
608         \r
609                 cur = NEXT_HEAD(cur);\r
610         }\r
611         if( rv && !bDump ) {\r
612                 _SysDebug("- Caller: %p", __builtin_return_address(0));\r
613                 exit(1);\r
614         }\r
615         return 0;\r
616 }\r
617 \r

UCC git Repository :: git.ucc.asn.au