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
sys_multi_heap_init(struct sys_multi_heap * heap,sys_multi_heap_fn_t choice_fn)9 void sys_multi_heap_init(struct sys_multi_heap *heap, sys_multi_heap_fn_t choice_fn)
10 {
11 heap->nheaps = 0;
12 heap->choice = choice_fn;
13 }
14
sys_multi_heap_add_heap(struct sys_multi_heap * mheap,struct sys_heap * heap,void * user_data)15 void sys_multi_heap_add_heap(struct sys_multi_heap *mheap,
16 struct sys_heap *heap, void *user_data)
17 {
18 __ASSERT_NO_MSG(mheap->nheaps < ARRAY_SIZE(mheap->heaps));
19
20 mheap->heaps[mheap->nheaps].heap = heap;
21 mheap->heaps[mheap->nheaps++].user_data = user_data;
22
23 /* Now sort them in memory order, simple extraction sort */
24 for (int i = 0; i < mheap->nheaps; i++) {
25 struct sys_multi_heap_rec swap;
26 int lowest = -1;
27 uintptr_t lowest_addr = UINTPTR_MAX;
28
29 for (int j = i; j < mheap->nheaps; j++) {
30 uintptr_t haddr = (uintptr_t)mheap->heaps[j].heap->heap;
31
32 if (haddr < lowest_addr) {
33 lowest = j;
34 lowest_addr = haddr;
35 }
36 }
37 swap = mheap->heaps[i];
38 mheap->heaps[i] = mheap->heaps[lowest];
39 mheap->heaps[lowest] = swap;
40 }
41 }
42
sys_multi_heap_alloc(struct sys_multi_heap * mheap,void * cfg,size_t bytes)43 void *sys_multi_heap_alloc(struct sys_multi_heap *mheap, void *cfg, size_t bytes)
44 {
45 return mheap->choice(mheap, cfg, 0, bytes);
46 }
47
sys_multi_heap_aligned_alloc(struct sys_multi_heap * mheap,void * cfg,size_t align,size_t bytes)48 void *sys_multi_heap_aligned_alloc(struct sys_multi_heap *mheap,
49 void *cfg, size_t align, size_t bytes)
50 {
51 return mheap->choice(mheap, cfg, align, bytes);
52 }
53
sys_multi_heap_get_heap(const struct sys_multi_heap * mheap,void * addr)54 const struct sys_multi_heap_rec *sys_multi_heap_get_heap(const struct sys_multi_heap *mheap,
55 void *addr)
56 {
57 uintptr_t haddr, baddr = (uintptr_t) addr;
58 int i;
59
60 /* Search the heaps array to find the correct heap
61 *
62 * FIXME: just a linear search currently, as the list is
63 * always short for reasonable apps and this code is very
64 * quick. The array is stored in sorted order though, so a
65 * binary search based on the block address is the design
66 * goal.
67 */
68 for (i = 0; i < mheap->nheaps; i++) {
69 haddr = (uintptr_t)mheap->heaps[i].heap->heap;
70 if (baddr < haddr) {
71 break;
72 }
73 }
74
75 /* Now i stores the index of the heap after our target (even
76 * if it's invalid and our target is the last!)
77 * FIXME: return -ENOENT when a proper heap is not found
78 */
79 return &mheap->heaps[i-1];
80 }
81
82
sys_multi_heap_free(struct sys_multi_heap * mheap,void * block)83 void sys_multi_heap_free(struct sys_multi_heap *mheap, void *block)
84 {
85 const struct sys_multi_heap_rec *heap;
86
87 heap = sys_multi_heap_get_heap(mheap, block);
88
89 if (heap != NULL) {
90 sys_heap_free(heap->heap, block);
91 }
92 }
93
