1 #include <stdlib.h>
2 #include <system.h>
3 #include <processor.h>
4 #include <sys_state.h>
5 #include <lwp_config.h>
6
7 #include "lwp_heap.h"
8
__lwp_heap_init(heap_cntrl * theheap,void * start_addr,u32 size,u32 pg_size)9 u32 __lwp_heap_init(heap_cntrl *theheap,void *start_addr,u32 size,u32 pg_size)
10 {
11 u32 dsize,level;
12 heap_block *block;
13
14 if(!__lwp_heap_pgsize_valid(pg_size) || size<HEAP_MIN_SIZE) return 0;
15
16 _CPU_ISR_Disable(level);
17 theheap->pg_size = pg_size;
18 dsize = (size - HEAP_OVERHEAD);
19
20 block = (heap_block*)start_addr;
21 block->back_flag = HEAP_DUMMY_FLAG;
22 block->front_flag = dsize;
23 block->next = __lwp_heap_tail(theheap);
24 block->prev = __lwp_heap_head(theheap);
25
26 theheap->start = block;
27 theheap->first = block;
28 theheap->perm_null = NULL;
29 theheap->last = block;
30
31 block = __lwp_heap_nextblock(block);
32 block->back_flag = dsize;
33 block->front_flag = HEAP_DUMMY_FLAG;
34 theheap->final = block;
35 _CPU_ISR_Restore(level);
36
37 return (dsize - HEAP_BLOCK_USED_OVERHEAD);
38 }
39
__lwp_heap_allocate(heap_cntrl * theheap,u32 size)40 void* __lwp_heap_allocate(heap_cntrl *theheap,u32 size)
41 {
42 u32 excess;
43 u32 dsize;
44 heap_block *block;
45 heap_block *next_block;
46 heap_block *tmp_block;
47 void *ptr;
48 u32 offset,level;
49
50 if(size>=(-1-HEAP_BLOCK_USED_OVERHEAD)) return NULL;
51
52 _CPU_ISR_Disable(level);
53 excess = (size % theheap->pg_size);
54 dsize = (size + theheap->pg_size + HEAP_BLOCK_USED_OVERHEAD);
55
56 if(excess)
57 dsize += (theheap->pg_size - excess);
58
59 if(dsize<sizeof(heap_block)) dsize = sizeof(heap_block);
60
61 for(block=theheap->first;;block=block->next) {
62 if(block==__lwp_heap_tail(theheap)) {
63 _CPU_ISR_Restore(level);
64 return NULL;
65 }
66 if(block->front_flag>=dsize) break;
67 }
68
69 if((block->front_flag-dsize)>(theheap->pg_size+HEAP_BLOCK_USED_OVERHEAD)) {
70 block->front_flag -= dsize;
71 next_block = __lwp_heap_nextblock(block);
72 next_block->back_flag = block->front_flag;
73
74 tmp_block = __lwp_heap_blockat(next_block,dsize);
75 tmp_block->back_flag = next_block->front_flag = __lwp_heap_buildflag(dsize,HEAP_BLOCK_USED);
76
77 ptr = __lwp_heap_startuser(next_block);
78 } else {
79 next_block = __lwp_heap_nextblock(block);
80 next_block->back_flag = __lwp_heap_buildflag(block->front_flag,HEAP_BLOCK_USED);
81
82 block->front_flag = next_block->back_flag;
83 block->next->prev = block->prev;
84 block->prev->next = block->next;
85
86 ptr = __lwp_heap_startuser(block);
87 }
88
89 offset = (theheap->pg_size - ((u32)ptr&(theheap->pg_size-1)));
90 ptr += offset;
91 *(((u32*)ptr)-1) = offset;
92 _CPU_ISR_Restore(level);
93
94 return ptr;
95 }
96
__lwp_heap_free(heap_cntrl * theheap,void * ptr)97 BOOL __lwp_heap_free(heap_cntrl *theheap,void *ptr)
98 {
99 heap_block *block;
100 heap_block *next_block;
101 heap_block *new_next;
102 heap_block *prev_block;
103 heap_block *tmp_block;
104 u32 dsize,level;
105
106 _CPU_ISR_Disable(level);
107
108 block = __lwp_heap_usrblockat(ptr);
109 if(!__lwp_heap_blockin(theheap,block) || __lwp_heap_blockfree(block)) {
110 _CPU_ISR_Restore(level);
111 return FALSE;
112 }
113
114 dsize = __lwp_heap_blocksize(block);
115 next_block = __lwp_heap_blockat(block,dsize);
116
117 if(!__lwp_heap_blockin(theheap,next_block) || (block->front_flag!=next_block->back_flag)) {
118 _CPU_ISR_Restore(level);
119 return FALSE;
120 }
121
122 if(__lwp_heap_prev_blockfree(block)) {
123 prev_block = __lwp_heap_prevblock(block);
124 if(!__lwp_heap_blockin(theheap,prev_block)) {
125 _CPU_ISR_Restore(level);
126 return FALSE;
127 }
128
129 if(__lwp_heap_blockfree(next_block)) {
130 prev_block->front_flag += next_block->front_flag+dsize;
131 tmp_block = __lwp_heap_nextblock(prev_block);
132 tmp_block->back_flag = prev_block->front_flag;
133 next_block->next->prev = next_block->prev;
134 next_block->prev->next = next_block->next;
135 } else {
136 prev_block->front_flag = next_block->back_flag = prev_block->front_flag+dsize;
137 }
138 } else if(__lwp_heap_blockfree(next_block)) {
139 block->front_flag = dsize+next_block->front_flag;
140 new_next = __lwp_heap_nextblock(block);
141 new_next->back_flag = block->front_flag;
142 block->next = next_block->next;
143 block->prev = next_block->prev;
144 next_block->prev->next = block;
145 next_block->next->prev = block;
146
147 if(theheap->first==next_block) theheap->first = block;
148 } else {
149 next_block->back_flag = block->front_flag = dsize;
150 block->prev = __lwp_heap_head(theheap);
151 block->next = theheap->first;
152 theheap->first = block;
153 block->next->prev = block;
154 }
155 _CPU_ISR_Restore(level);
156
157 return TRUE;
158 }
159
__lwp_heap_getinfo(heap_cntrl * theheap,heap_iblock * theinfo)160 u32 __lwp_heap_getinfo(heap_cntrl *theheap,heap_iblock *theinfo)
161 {
162 u32 not_done = 1;
163 heap_block *theblock = NULL;
164 heap_block *nextblock = NULL;
165
166 theinfo->free_blocks = 0;
167 theinfo->free_size = 0;
168 theinfo->used_blocks = 0;
169 theinfo->used_size = 0;
170
171 if(!__sys_state_up(__sys_state_get())) return 1;
172
173 theblock = theheap->start;
174 if(theblock->back_flag!=HEAP_DUMMY_FLAG) return 2;
175
176 while(not_done) {
177 if(__lwp_heap_blockfree(theblock)) {
178 theinfo->free_blocks++;
179 theinfo->free_size += __lwp_heap_blocksize(theblock);
180 } else {
181 theinfo->used_blocks++;
182 theinfo->used_size += __lwp_heap_blocksize(theblock);
183 }
184
185 if(theblock->front_flag!=HEAP_DUMMY_FLAG) {
186 nextblock = __lwp_heap_nextblock(theblock);
187 if(theblock->front_flag!=nextblock->back_flag) return 2;
188 }
189
190 if(theblock->front_flag==HEAP_DUMMY_FLAG)
191 not_done = 0;
192 else
193 theblock = nextblock;
194 }
195 return 0;
196 }
197