First version
[3rdparty/ote_partner/tlk.git] / lib / heap / heap.c
1 /*
2  * Copyright (c) 2008-2009 Travis Geiselbrecht
3  * Copyright (c) 2009 Corey Tabaka
4  *
5  * Permission is hereby granted, free of charge, to any person obtaining
6  * a copy of this software and associated documentation files
7  * (the "Software"), to deal in the Software without restriction,
8  * including without limitation the rights to use, copy, modify, merge,
9  * publish, distribute, sublicense, and/or sell copies of the Software,
10  * and to permit persons to whom the Software is furnished to do so,
11  * subject to the following conditions:
12  *
13  * The above copyright notice and this permission notice shall be
14  * included in all copies or substantial portions of the Software.
15  *
16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
19  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
20  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
21  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
22  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
23  */
24 #include <debug.h>
25 #include <assert.h>
26 #include <err.h>
27 #include <list.h>
28 #include <rand.h>
29 #include <string.h>
30 #include <kernel/thread.h>
31 #include <lib/heap.h>
32
33 #define LOCAL_TRACE 0
34
35 #define DEBUG_HEAP 0
36 #define ALLOC_FILL 0x99
37 #define FREE_FILL 0x77
38 #define PADDING_FILL 0x55
39 #define PADDING_SIZE 64
40
41 #define ROUNDUP(a, b) (((a) + ((b)-1)) & ~((b)-1))
42
43 #define HEAP_MAGIC 'HEAP'
44
45 #if WITH_STATIC_HEAP
46
47 #if !defined(HEAP_START) || !defined(HEAP_LEN)
48 #error WITH_STATIC_HEAP set but no HEAP_START or HEAP_LEN defined
49 #endif
50
51 #else
52 // end of the binary
53 extern int _end;
54
55 // end of memory
56 extern int _heap_end;
57
58 #define HEAP_START ((unsigned long)&_end)
59 #define HEAP_LEN ((size_t)_heap_end - (size_t)&_end)
60 #endif
61
62 struct free_heap_chunk {
63         struct list_node node;
64         size_t len;
65 };
66
67 struct heap {
68         void *base;
69         size_t len;
70         struct list_node free_list;
71 };
72
73 // heap static vars
74 static struct heap theheap;
75
76 // structure placed at the beginning every allocation
77 struct alloc_struct_begin {
78         unsigned int magic;
79         void *ptr;
80         size_t size;
81 #if DEBUG_HEAP
82         void *padding_start;
83         size_t padding_size;
84 #endif
85 };
86
87 static void dump_free_chunk(struct free_heap_chunk *chunk)
88 {
89         dprintf(INFO, "\t\tbase %p, end 0x%lx, len 0x%zx\n", chunk, (vaddr_t)chunk + chunk->len, chunk->len);
90 }
91
92 static void heap_dump(void)
93 {
94         dprintf(INFO, "Heap dump:\n");
95         dprintf(INFO, "\tbase %p, len 0x%zx\n", theheap.base, theheap.len);
96         dprintf(INFO, "\tfree list:\n");
97
98         struct free_heap_chunk *chunk;
99         list_for_every_entry(&theheap.free_list, chunk, struct free_heap_chunk, node) {
100                 dump_free_chunk(chunk);
101         }
102 }
103
104 static void heap_test(void)
105 {
106         void *ptr[16];
107
108         ptr[0] = heap_alloc(8, 0);
109         ptr[1] = heap_alloc(32, 0);
110         ptr[2] = heap_alloc(7, 0);
111         ptr[3] = heap_alloc(0, 0);
112         ptr[4] = heap_alloc(98713, 0);
113         ptr[5] = heap_alloc(16, 0);
114
115         heap_free(ptr[5]);
116         heap_free(ptr[1]);
117         heap_free(ptr[3]);
118         heap_free(ptr[0]);
119         heap_free(ptr[4]);
120         heap_free(ptr[2]);
121
122         heap_dump();
123
124         int i;
125         for (i=0; i < 16; i++)
126                 ptr[i] = 0;
127
128         for (i=0; i < 32768; i++) {
129                 unsigned int index = (unsigned int)rand() % 16;
130
131                 if ((i % (16*1024)) == 0)
132                         printf("pass %d\n", i);
133
134 //              printf("index 0x%x\n", index);
135                 if (ptr[index]) {
136 //                      printf("freeing ptr[0x%x] = %p\n", index, ptr[index]);
137                         heap_free(ptr[index]);
138                         ptr[index] = 0;
139                 }
140                 unsigned int align = 1 << ((unsigned int)rand() % 8);
141                 ptr[index] = heap_alloc((unsigned int)rand() % 32768, align);
142 //              printf("ptr[0x%x] = %p, align 0x%x\n", index, ptr[index], align);
143
144                 DEBUG_ASSERT(((addr_t)ptr[index] % align) == 0);
145 //              heap_dump();
146         }
147
148         for (i=0; i < 16; i++) {
149                 if (ptr[i])
150                         heap_free(ptr[i]);
151         }
152
153         heap_dump();
154 }
155
156 // try to insert this free chunk into the free list, consuming the chunk by merging it with
157 // nearby ones if possible. Returns base of whatever chunk it became in the list.
158 static struct free_heap_chunk *heap_insert_free_chunk(struct free_heap_chunk *chunk)
159 {
160 #if DEBUGLEVEL > INFO
161         vaddr_t chunk_end = (vaddr_t)chunk + chunk->len;
162 #endif
163
164 //      dprintf("%s: chunk ptr %p, size 0x%lx, chunk_end 0x%x\n", __FUNCTION__, chunk, chunk->len, chunk_end);
165
166         struct free_heap_chunk *next_chunk;
167         struct free_heap_chunk *last_chunk;
168
169         // walk through the list, finding the node to insert before
170         list_for_every_entry(&theheap.free_list, next_chunk, struct free_heap_chunk, node) {
171                 if (chunk < next_chunk) {
172                         DEBUG_ASSERT(chunk_end <= (vaddr_t)next_chunk);
173
174                         list_add_before(&next_chunk->node, &chunk->node);
175
176                         goto try_merge;
177                 }
178         }
179
180         // walked off the end of the list, add it at the tail
181         list_add_tail(&theheap.free_list, &chunk->node);
182
183         // try to merge with the previous chunk
184 try_merge:
185         last_chunk = list_prev_type(&theheap.free_list, &chunk->node, struct free_heap_chunk, node);
186         if (last_chunk) {
187                 if ((vaddr_t)last_chunk + last_chunk->len == (vaddr_t)chunk) {
188                         // easy, just extend the previous chunk
189                         last_chunk->len += chunk->len;
190
191                         // remove ourself from the list
192                         list_delete(&chunk->node);
193
194                         // set the chunk pointer to the newly extended chunk, in case
195                         // it needs to merge with the next chunk below
196                         chunk = last_chunk;
197                 }
198         }
199
200         // try to merge with the next chunk
201         if (next_chunk) {
202                 if ((vaddr_t)chunk + chunk->len == (vaddr_t)next_chunk) {
203                         // extend our chunk
204                         chunk->len += next_chunk->len;
205
206                         // remove them from the list
207                         list_delete(&next_chunk->node);
208                 }
209         }
210
211         return chunk;
212 }
213
214 struct free_heap_chunk *heap_create_free_chunk(void *ptr, size_t len)
215 {
216         DEBUG_ASSERT((len % sizeof(void *)) == 0); // size must be aligned on pointer boundary
217
218 #if DEBUG_HEAP
219         memset(ptr, FREE_FILL, len);
220 #endif
221
222         struct free_heap_chunk *chunk = (struct free_heap_chunk *)ptr;
223         chunk->len = len;
224
225         return chunk;
226 }
227
228 void *heap_alloc(size_t size, unsigned int alignment)
229 {
230         void *ptr;
231 #if DEBUG_HEAP
232         size_t original_size = size;
233 #endif
234
235         LTRACEF("size %zd, align %d\n", size, alignment);
236
237         // alignment must be power of 2
238         if (alignment & (alignment - 1))
239                 return NULL;
240
241         // we always put a size field + base pointer + magic in front of the allocation
242         size += sizeof(struct alloc_struct_begin);
243 #if DEBUG_HEAP
244         size += PADDING_SIZE;
245 #endif
246
247         // make sure we allocate at least the size of a struct free_heap_chunk so that
248         // when we free it, we can create a struct free_heap_chunk struct and stick it
249         // in the spot
250         if (size < sizeof(struct free_heap_chunk))
251                 size = sizeof(struct free_heap_chunk);
252
253         // round up size to a multiple of native pointer size
254         size = ROUNDUP(size, sizeof(void *));
255
256         // deal with nonzero alignments
257         if (alignment > 0) {
258                 if (alignment < 16)
259                         alignment = 16;
260
261                 // add alignment for worst case fit
262                 size += alignment;
263         }
264
265         // critical section
266         enter_critical_section();
267
268         // walk through the list
269         ptr = NULL;
270         struct free_heap_chunk *chunk;
271         list_for_every_entry(&theheap.free_list, chunk, struct free_heap_chunk, node) {
272                 DEBUG_ASSERT((chunk->len % sizeof(void *)) == 0); // len should always be a multiple of pointer size
273
274                 // is it big enough to service our allocation?
275                 if (chunk->len >= size) {
276                         ptr = chunk;
277
278                         // remove it from the list
279                         struct list_node *next_node = list_next(&theheap.free_list, &chunk->node);
280                         list_delete(&chunk->node);
281
282                         if (chunk->len > size + sizeof(struct free_heap_chunk)) {
283                                 // there's enough space in this chunk to create a new one after the allocation
284                                 struct free_heap_chunk *newchunk = heap_create_free_chunk((uint8_t *)ptr + size, chunk->len - size);
285
286                                 // truncate this chunk
287                                 chunk->len -= chunk->len - size;
288
289                                 // add the new one where chunk used to be
290                                 if (next_node)
291                                         list_add_before(next_node, &newchunk->node);
292                                 else
293                                         list_add_tail(&theheap.free_list, &newchunk->node);
294                         }
295
296                         // the allocated size is actually the length of this chunk, not the size requested
297                         DEBUG_ASSERT(chunk->len >= size);
298                         size = chunk->len;
299
300 #if DEBUG_HEAP
301                         memset(ptr, ALLOC_FILL, size);
302 #endif
303
304                         ptr = (void *)((addr_t)ptr + sizeof(struct alloc_struct_begin));
305
306                         // align the output if requested
307                         if (alignment > 0) {
308                                 ptr = (void *)ROUNDUP((addr_t)ptr, alignment);
309                         }
310
311                         struct alloc_struct_begin *as = (struct alloc_struct_begin *)ptr;
312                         as--;
313                         as->magic = HEAP_MAGIC;
314                         as->ptr = (void *)chunk;
315                         as->size = size;
316 #if DEBUG_HEAP
317                         as->padding_start = ((uint8_t *)ptr + original_size);
318                         as->padding_size = (((addr_t)chunk + size) - ((addr_t)ptr + original_size));
319 //                      printf("padding start %p, size %u, chunk %p, size %u\n", as->padding_start, as->padding_size, chunk, size);
320
321                         memset(as->padding_start, PADDING_FILL, as->padding_size);
322 #endif
323
324                         break;
325                 }
326         }
327
328         LTRACEF("returning ptr %p\n", ptr);
329
330 //      heap_dump();
331
332         exit_critical_section();
333
334         return ptr;
335 }
336
337 void heap_free(void *ptr)
338 {
339         if (ptr == 0)
340                 return;
341
342         LTRACEF("ptr %p\n", ptr);
343
344         // check for the old allocation structure
345         struct alloc_struct_begin *as = (struct alloc_struct_begin *)ptr;
346         as--;
347
348         if (as->magic != HEAP_MAGIC) {
349                 dprintf(CRITICAL, "ERROR: invalid/corrupt heap memory ptr!!\n");
350                 return;
351         }
352
353 #if DEBUG_HEAP
354         {
355                 uint i;
356                 uint8_t *pad = (uint8_t *)as->padding_start;
357
358                 for (i = 0; i < as->padding_size; i++) {
359                         if (pad[i] != PADDING_FILL) {
360                                 printf("free at %p scribbled outside the lines:\n", ptr);
361                                 hexdump(pad, as->padding_size);
362                                 panic("die\n");
363                         }
364                 }
365         }
366 #endif
367
368         LTRACEF("allocation was %zd bytes long at ptr %p\n", as->size, as->ptr);
369
370         // looks good, create a free chunk and add it to the pool
371         enter_critical_section();
372         heap_insert_free_chunk(heap_create_free_chunk(as->ptr, as->size));
373         exit_critical_section();
374
375 //      heap_dump();
376 }
377
378 void heap_init(void)
379 {
380         LTRACE_ENTRY;
381
382         // set the heap range
383         theheap.base = (void *)HEAP_START;
384         theheap.len = HEAP_LEN;
385
386         LTRACEF("base %p size %zd bytes\n", theheap.base, theheap.len);
387
388         // initialize the free list
389         list_initialize(&theheap.free_list);
390
391         // create an initial free chunk
392         heap_insert_free_chunk(heap_create_free_chunk(theheap.base, theheap.len));
393
394         // dump heap info
395 //      heap_dump();
396
397 //      dprintf(INFO, "running heap tests\n");
398 //      heap_test();
399 }
400
401 #if DEBUGLEVEL > 1
402 #if WITH_LIB_CONSOLE
403
404 #include <lib/console.h>
405
406 static int cmd_heap(int argc, const cmd_args *argv);
407
408 STATIC_COMMAND_START
409 STATIC_COMMAND("heap", "heap debug commands", &cmd_heap)
410 STATIC_COMMAND_END(heap);
411
412 static int cmd_heap(int argc, const cmd_args *argv)
413 {
414         if (argc < 2) {
415                 printf("not enough arguments\n");
416                 return -1;
417         }
418
419         if (strcmp(argv[1].str, "info") == 0) {
420                 heap_dump();
421         } else {
422                 printf("unrecognized command\n");
423                 return -1;
424         }
425
426         return 0;
427 }
428
429 #endif
430 #endif
431