1 /* Copyright (c) 2021 Intel Corporation
2 * SPDX-License-Identifier: Apache-2.0
3 */
4 #include <zephyr/sys/__assert.h>
5 #include <zephyr/sys/util.h>
6 #include <zephyr/sys/sys_heap.h>
7 #include <zephyr/sys/multi_heap.h>
8 #include <string.h>
9
sys_multi_heap_init(struct sys_multi_heap * heap,sys_multi_heap_fn_t choice_fn)10 void sys_multi_heap_init(struct sys_multi_heap *heap, sys_multi_heap_fn_t choice_fn)
11 {
12 heap->nheaps = 0;
13 heap->choice = choice_fn;
14 }
15
sys_multi_heap_add_heap(struct sys_multi_heap * mheap,struct sys_heap * heap,void * user_data)16 void sys_multi_heap_add_heap(struct sys_multi_heap *mheap,
17 struct sys_heap *heap, void *user_data)
18 {
19 __ASSERT_NO_MSG(mheap->nheaps < ARRAY_SIZE(mheap->heaps));
20
21 mheap->heaps[mheap->nheaps].heap = heap;
22 mheap->heaps[mheap->nheaps++].user_data = user_data;
23
24 /* Now sort them in memory order, simple extraction sort */
25 for (int i = 0; i < mheap->nheaps; i++) {
26 struct sys_multi_heap_rec swap;
27 int lowest = -1;
28 uintptr_t lowest_addr = UINTPTR_MAX;
29
30 for (int j = i; j < mheap->nheaps; j++) {
31 uintptr_t haddr = (uintptr_t)mheap->heaps[j].heap->heap;
32
33 if (haddr < lowest_addr) {
34 lowest = j;
35 lowest_addr = haddr;
36 }
37 }
38 swap = mheap->heaps[i];
39 mheap->heaps[i] = mheap->heaps[lowest];
40 mheap->heaps[lowest] = swap;
41 }
42 }
43
sys_multi_heap_alloc(struct sys_multi_heap * mheap,void * cfg,size_t bytes)44 void *sys_multi_heap_alloc(struct sys_multi_heap *mheap, void *cfg, size_t bytes)
45 {
46 return mheap->choice(mheap, cfg, 0, bytes);
47 }
48
sys_multi_heap_aligned_alloc(struct sys_multi_heap * mheap,void * cfg,size_t align,size_t bytes)49 void *sys_multi_heap_aligned_alloc(struct sys_multi_heap *mheap,
50 void *cfg, size_t align, size_t bytes)
51 {
52 return mheap->choice(mheap, cfg, align, bytes);
53 }
54
sys_multi_heap_get_heap(const struct sys_multi_heap * mheap,void * addr)55 const struct sys_multi_heap_rec *sys_multi_heap_get_heap(const struct sys_multi_heap *mheap,
56 void *addr)
57 {
58 uintptr_t haddr, baddr = (uintptr_t) addr;
59 int i;
60
61 /* Search the heaps array to find the correct heap
62 *
63 * FIXME: just a linear search currently, as the list is
64 * always short for reasonable apps and this code is very
65 * quick. The array is stored in sorted order though, so a
66 * binary search based on the block address is the design
67 * goal.
68 */
69 for (i = 0; i < mheap->nheaps; i++) {
70 haddr = (uintptr_t)mheap->heaps[i].heap->heap;
71 if (baddr < haddr) {
72 break;
73 }
74 }
75
76 /* Now i stores the index of the heap after our target (even
77 * if it's invalid and our target is the last!)
78 * FIXME: return -ENOENT when a proper heap is not found
79 */
80 return &mheap->heaps[i-1];
81 }
82
83
sys_multi_heap_free(struct sys_multi_heap * mheap,void * block)84 void sys_multi_heap_free(struct sys_multi_heap *mheap, void *block)
85 {
86 const struct sys_multi_heap_rec *heap;
87
88 heap = sys_multi_heap_get_heap(mheap, block);
89
90 if (heap != NULL) {
91 sys_heap_free(heap->heap, block);
92 }
93 }
94
sys_multi_heap_aligned_realloc(struct sys_multi_heap * mheap,void * cfg,void * ptr,size_t align,size_t bytes)95 void *sys_multi_heap_aligned_realloc(struct sys_multi_heap *mheap, void *cfg,
96 void *ptr, size_t align, size_t bytes)
97 {
98 /* special realloc semantics */
99 if (ptr == NULL) {
100 return sys_multi_heap_aligned_alloc(mheap, cfg, align, bytes);
101 }
102 if (bytes == 0) {
103 sys_multi_heap_free(mheap, ptr);
104 return NULL;
105 }
106
107 const struct sys_multi_heap_rec *rec = sys_multi_heap_get_heap(mheap, ptr);
108
109 __ASSERT_NO_MSG(rec);
110
111 /* Invoke the realloc function on the same heap, to try to reuse in place */
112 void *new_ptr = sys_heap_aligned_realloc(rec->heap, ptr, align, bytes);
113
114 if (new_ptr != NULL) {
115 return new_ptr;
116 }
117
118 size_t old_size = sys_heap_usable_size(rec->heap, ptr);
119
120 /* Otherwise, allocate a new block and copy the data */
121 new_ptr = sys_multi_heap_aligned_alloc(mheap, cfg, align, bytes);
122 if (new_ptr != NULL) {
123 memcpy(new_ptr, ptr, MIN(old_size, bytes));
124 sys_multi_heap_free(mheap, ptr);
125 }
126
127 return new_ptr;
128 }
129
