1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
|
#include<source/heap.h>
#include<types.h>
void k_heapBMInit(KHEAPBM *heap)
{
heap->fblock = 0;
}
int k_heapBMAddBlock(KHEAPBM *heap, uintptr_t addr, uint32_t size, uint32_t
bsize)
{
KHEAPBLOCKBM *b;
uint32_t bcnt;
uint32_t x;
uint8_t *bm;
b = (KHEAPBLOCKBM*)addr;
b->size = size - sizeof(KHEAPBLOCKBM);
b->bsize = bsize;
b->next = heap->fblock;
heap->fblock = b;
bcnt = b->size / b->bsize;
bm = (uint8_t*)&b[1];
/* clear bitmap */
for (x = 0; x < bcnt; ++x) {
bm[x] = 0;
}
/* reserve room for bitmap */
bcnt = (bcnt / bsize) * bsize < bcnt ? bcnt / bsize + 1 : bcnt / bsize;
for (x = 0; x < bcnt; ++x) {
bm[x] = 5;
}
b->lfb = bcnt - 1;
b->used = bcnt;
return 1;
}
static uint8_t k_heapBMGetNID(uint8_t a, uint8_t b);
static uint8_t k_heapBMGetNID(uint8_t a, uint8_t b)
{
uint8_t c;
for (c=a+1;c==b||c==0;++c);
return c;
}
void *k_heapBMAlloc(KHEAPBM *heap, uint32_t size)
{
KHEAPBLOCKBM *b;
uint8_t *bm;
uint32_t bcnt;
uint32_t x, y, z;
uint32_t bneed;
uint8_t nid;
/* iterate blocks */
for (b = heap->fblock; b; b = b->next)
{
/* check if block has enough room */
if (b->size - (b->used * b->bsize) >= size)
{
bcnt = b->size / b->bsize;
bneed = (size / b->bsize) * b->bsize < size ? size / b->bsize + 1 :
size / b->bsize;
bm = (uint8_t*)&b[1];
for (x = (b->lfb + 1 >= bcnt ? 0 : b->lfb + 1); x != b->lfb; ++x)
{
/* just wrap around */
if (x >= bcnt) x = 0;
if (bm[x] == 0)
{
/* count free blocks */
for (y = 0; bm[x + y] == 0 && y < bneed && (x + y) < bcnt;
++y);
/* we have enough, now allocate them */
if (y == bneed)
{
/* find ID that does not match left or right */
nid = k_heapBMGetNID(bm[x - 1], bm[x + y]);
/* allocate by setting id */
for (z = 0; z < y; ++z) bm[x + z] = nid;
/* optimization */
b->lfb = (x + bneed) - 2;
/* count used blocks NOT bytes */
b->used += y;
return (void*)(x * b->bsize + (uintptr_t)&b[1]);
}
/* x will be incremented by one ONCE more in our FOR loop
* */
x += (y - 1);
continue;
}
}
}
}
return 0;
}
void k_heapBMFree(KHEAPBM *heap, void *ptr)
{
KHEAPBLOCKBM *b;
uintptr_t ptroff;
uint32_t bi, x;
uint8_t *bm;
uint8_t id;
uint32_t max;
for (b = heap->fblock; b; b = b->next)
{
if ((uintptr_t)ptr > (uintptr_t)b && (uintptr_t)ptr < (uintptr_t)b +
sizeof(KHEAPBLOCKBM) + b->size) {
/* found block */
ptroff = (uintptr_t)ptr - (uintptr_t)&b[1]; /* get offset to get
block */
/* block offset in BM */
bi = ptroff / b->bsize;
/* .. */
bm = (uint8_t*)&b[1];
/* clear allocation */
id = bm[bi];
/* oddly.. GCC did not optimize this */
max = b->size / b->bsize;
for (x = bi; bm[x] == id && x < max; ++x) bm[x] = 0;
/* update free block count */
b->used -= x - bi;
return;
}
}
/* this error needs to be raised or reported somehow */
return;
}
KHEAPBM kheap;
void kheapinit()
{
k_heapBMInit(&kheap);
}
int kheapaddblock(uintptr_t addr,uint32_t size,uint32_t bsize)
{
return k_heapBMAddBlock(&kheap,addr,size,bsize);
}
void *kmalloc(uint32_t size)
{
return k_heapBMAlloc(&kheap,size);
}
void kfree(void *ptr)
{
k_heapBMFree(&kheap,ptr);
}
|
