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

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