1 | /* |
2 | Copyright (c) 2012, Broadcom Europe Ltd |
3 | All rights reserved. |
4 | |
5 | Redistribution and use in source and binary forms, with or without |
6 | modification, are permitted provided that the following conditions are met: |
7 | * Redistributions of source code must retain the above copyright |
8 | notice, this list of conditions and the following disclaimer. |
9 | * Redistributions in binary form must reproduce the above copyright |
10 | notice, this list of conditions and the following disclaimer in the |
11 | documentation and/or other materials provided with the distribution. |
12 | * Neither the name of the copyright holder nor the |
13 | names of its contributors may be used to endorse or promote products |
14 | derived from this software without specific prior written permission. |
15 | |
16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND |
17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY |
20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
26 | */ |
27 | |
28 | #define VCOS_LOG_CATEGORY (&vcos_blockpool_log) |
29 | |
30 | #include <stddef.h> |
31 | #include <string.h> |
32 | #include "interface/vcos/vcos.h" |
33 | #include "interface/vcos/generic/vcos_generic_blockpool.h" |
34 | |
35 | #define VCOS_BLOCKPOOL_FOURCC(a,b,c,d) ((a) | (b << 8) | (c << 16) | (d << 24)) |
36 | #define VCOS_BLOCKPOOL_MAGIC VCOS_BLOCKPOOL_FOURCC('v', 'b', 'p', 'l') |
37 | #define VCOS_BLOCKPOOL_SUBPOOL_MAGIC VCOS_BLOCKPOOL_FOURCC('v', 's', 'p', 'l') |
38 | |
39 | #define VCOS_BLOCKPOOL_SUBPOOL_FLAG_NONE (0) |
40 | #define VCOS_BLOCKPOOL_SUBPOOL_FLAG_OWNS_MEM (1 << 0) |
41 | #define VCOS_BLOCKPOOL_SUBPOOL_FLAG_EXTENSION (1 << 1) |
42 | |
43 | /* Uncomment to enable really verbose debug messages */ |
44 | /* #define VCOS_BLOCKPOOL_DEBUGGING */ |
45 | /* Whether to overwrite freed blocks with 0xBD */ |
46 | #ifdef VCOS_BLOCKPOOL_DEBUGGING |
47 | #define VCOS_BLOCKPOOL_OVERWRITE_ON_FREE 1 |
48 | #define VCOS_BLOCKPOOL_DEBUG_MEMSET_MAX_SIZE (UINT32_MAX) |
49 | #else |
50 | #define VCOS_BLOCKPOOL_OVERWRITE_ON_FREE 0 |
51 | #define VCOS_BLOCKPOOL_DEBUG_MEMSET_MAX_SIZE (2 * 1024 * 1024) |
52 | #endif |
53 | |
54 | #ifdef VCOS_BLOCKPOOL_DEBUGGING |
55 | #define VCOS_BLOCKPOOL_ASSERT vcos_demand |
56 | #define VCOS_BLOCKPOOL_TRACE_LEVEL VCOS_LOG_TRACE |
57 | #define VCOS_BLOCKPOOL_DEBUG_LOG(s, ...) vcos_log_trace("%s: " s, VCOS_FUNCTION, __VA_ARGS__) |
58 | #undef VCOS_BLOCKPOOL_OVERWRITE_ON_FREE |
59 | #define VCOS_BLOCKPOOL_OVERWRITE_ON_FREE 1 |
60 | #else |
61 | #define VCOS_BLOCKPOOL_ASSERT vcos_demand |
62 | #define VCOS_BLOCKPOOL_TRACE_LEVEL VCOS_LOG_ERROR |
63 | #define VCOS_BLOCKPOOL_DEBUG_LOG(s, ...) |
64 | #endif |
65 | |
66 | #define ASSERT_POOL(p) \ |
67 | VCOS_BLOCKPOOL_ASSERT((p) && (p)->magic == VCOS_BLOCKPOOL_MAGIC); |
68 | |
69 | #define ASSERT_SUBPOOL(p) \ |
70 | VCOS_BLOCKPOOL_ASSERT((p) && (p)->magic == VCOS_BLOCKPOOL_SUBPOOL_MAGIC && \ |
71 | p->start >= p->mem); |
72 | |
73 | #if defined(VCOS_LOGGING_ENABLED) |
74 | static VCOS_LOG_CAT_T vcos_blockpool_log = |
75 | VCOS_LOG_INIT("vcos_blockpool" , VCOS_BLOCKPOOL_TRACE_LEVEL); |
76 | #endif |
77 | |
78 | static void vcos_generic_blockpool_subpool_init( |
79 | VCOS_BLOCKPOOL_T *pool, VCOS_BLOCKPOOL_SUBPOOL_T *subpool, |
80 | void *mem, size_t pool_size, VCOS_UNSIGNED num_blocks, int align, |
81 | uint32_t flags) |
82 | { |
83 | VCOS_BLOCKPOOL_HEADER_T *block; |
84 | VCOS_BLOCKPOOL_HEADER_T *end; |
85 | |
86 | vcos_unused(flags); |
87 | |
88 | vcos_log_trace( |
89 | "%s: pool %p subpool %p mem %p pool_size %d " \ |
90 | "num_blocks %d align %d flags %x" , |
91 | VCOS_FUNCTION, |
92 | pool, subpool, mem, (uint32_t) pool_size, |
93 | num_blocks, align, flags); |
94 | |
95 | subpool->magic = VCOS_BLOCKPOOL_SUBPOOL_MAGIC; |
96 | subpool->mem = mem; |
97 | |
98 | /* The block data pointers must be aligned according to align and the |
99 | * block header pre-preceeds the first block data. |
100 | * For large alignments there may be wasted space between subpool->mem |
101 | * and the first block header. |
102 | */ |
103 | subpool->start = (char *) subpool->mem + sizeof(VCOS_BLOCKPOOL_HEADER_T); |
104 | subpool->start = (void*) |
105 | VCOS_BLOCKPOOL_ROUND_UP((unsigned long) subpool->start, align); |
106 | subpool->start = (char *) subpool->start - sizeof(VCOS_BLOCKPOOL_HEADER_T); |
107 | |
108 | vcos_assert(subpool->start >= subpool->mem); |
109 | |
110 | vcos_log_trace("%s: mem %p subpool->start %p" \ |
111 | " pool->block_size %d pool->block_data_size %d" , |
112 | VCOS_FUNCTION, mem, subpool->start, |
113 | (int) pool->block_size, (int) pool->block_data_size); |
114 | |
115 | subpool->num_blocks = num_blocks; |
116 | subpool->available_blocks = num_blocks; |
117 | subpool->free_list = NULL; |
118 | subpool->owner = pool; |
119 | |
120 | /* Initialise to a predictable bit pattern unless the pool is so big |
121 | * that the delay would be noticeable. */ |
122 | if (pool_size < VCOS_BLOCKPOOL_DEBUG_MEMSET_MAX_SIZE) |
123 | memset(subpool->mem, 0xBC, pool_size); /* For debugging */ |
124 | |
125 | block = (VCOS_BLOCKPOOL_HEADER_T*) subpool->start; |
126 | end = (VCOS_BLOCKPOOL_HEADER_T*) |
127 | ((char *) subpool->start + (pool->block_size * num_blocks)); |
128 | subpool->end = end; |
129 | |
130 | /* Initialise the free list for this subpool */ |
131 | while (block < end) |
132 | { |
133 | block->owner.next = subpool->free_list; |
134 | subpool->free_list = block; |
135 | block = (VCOS_BLOCKPOOL_HEADER_T*)((char*) block + pool->block_size); |
136 | } |
137 | |
138 | } |
139 | |
140 | VCOS_STATUS_T vcos_generic_blockpool_init(VCOS_BLOCKPOOL_T *pool, |
141 | VCOS_UNSIGNED num_blocks, VCOS_UNSIGNED block_size, |
142 | void *start, VCOS_UNSIGNED pool_size, VCOS_UNSIGNED align, |
143 | VCOS_UNSIGNED flags, const char *name) |
144 | { |
145 | VCOS_STATUS_T status = VCOS_SUCCESS; |
146 | |
147 | vcos_unused(name); |
148 | vcos_unused(flags); |
149 | |
150 | vcos_log_trace( |
151 | "%s: pool %p num_blocks %d block_size %d start %p pool_size %d name %p" , |
152 | VCOS_FUNCTION, pool, num_blocks, block_size, start, pool_size, name); |
153 | |
154 | vcos_demand(pool); |
155 | vcos_demand(start); |
156 | vcos_assert((block_size > 0)); |
157 | vcos_assert(num_blocks > 0); |
158 | |
159 | if (! align) |
160 | align = VCOS_BLOCKPOOL_ALIGN_DEFAULT; |
161 | |
162 | if (align & 0x3) |
163 | { |
164 | vcos_log_error("%s: invalid alignment %d" , VCOS_FUNCTION, align); |
165 | return VCOS_EINVAL; |
166 | } |
167 | |
168 | if (VCOS_BLOCKPOOL_SIZE(num_blocks, block_size, align) > pool_size) |
169 | { |
170 | vcos_log_error("%s: Pool is too small" \ |
171 | " num_blocks %d block_size %d align %d" |
172 | " pool_size %d required size %d" , VCOS_FUNCTION, |
173 | num_blocks, block_size, align, |
174 | pool_size, (int) VCOS_BLOCKPOOL_SIZE(num_blocks, block_size, align)); |
175 | return VCOS_ENOMEM; |
176 | } |
177 | |
178 | status = vcos_mutex_create(&pool->mutex, "vcos blockpool mutex" ); |
179 | if (status != VCOS_SUCCESS) |
180 | return status; |
181 | |
182 | pool->block_data_size = block_size; |
183 | |
184 | /* TODO - create flag that if set forces the header to be in its own cache |
185 | * line */ |
186 | pool->block_size = VCOS_BLOCKPOOL_ROUND_UP(pool->block_data_size + |
187 | (align >= 4096 ? 32 : 0) + |
188 | sizeof(VCOS_BLOCKPOOL_HEADER_T), align); |
189 | |
190 | pool->magic = VCOS_BLOCKPOOL_MAGIC; |
191 | pool->num_subpools = 1; |
192 | pool->num_extension_blocks = 0; |
193 | pool->align = align; |
194 | memset(pool->subpools, 0, sizeof(pool->subpools)); |
195 | |
196 | vcos_generic_blockpool_subpool_init(pool, &pool->subpools[0], start, |
197 | pool_size, num_blocks, align, VCOS_BLOCKPOOL_SUBPOOL_FLAG_NONE); |
198 | |
199 | return status; |
200 | } |
201 | |
202 | VCOS_STATUS_T vcos_generic_blockpool_create_on_heap(VCOS_BLOCKPOOL_T *pool, |
203 | VCOS_UNSIGNED num_blocks, VCOS_UNSIGNED block_size, VCOS_UNSIGNED align, |
204 | VCOS_UNSIGNED flags, const char *name) |
205 | { |
206 | VCOS_STATUS_T status = VCOS_SUCCESS; |
207 | size_t size = VCOS_BLOCKPOOL_SIZE(num_blocks, block_size, align); |
208 | void* mem = vcos_malloc(size, name); |
209 | |
210 | vcos_log_trace("%s: num_blocks %d block_size %d name %s" , |
211 | VCOS_FUNCTION, num_blocks, block_size, name); |
212 | |
213 | if (! mem) |
214 | return VCOS_ENOMEM; |
215 | |
216 | status = vcos_generic_blockpool_init(pool, num_blocks, |
217 | block_size, mem, size, align, flags, name); |
218 | |
219 | if (status != VCOS_SUCCESS) |
220 | goto fail; |
221 | |
222 | pool->subpools[0].flags |= VCOS_BLOCKPOOL_SUBPOOL_FLAG_OWNS_MEM; |
223 | return status; |
224 | |
225 | fail: |
226 | vcos_free(mem); |
227 | return status; |
228 | } |
229 | |
230 | VCOS_STATUS_T vcos_generic_blockpool_extend(VCOS_BLOCKPOOL_T *pool, |
231 | VCOS_UNSIGNED num_extensions, VCOS_UNSIGNED num_blocks) |
232 | { |
233 | VCOS_UNSIGNED i; |
234 | ASSERT_POOL(pool); |
235 | |
236 | vcos_log_trace("%s: pool %p num_extensions %d num_blocks %d" , |
237 | VCOS_FUNCTION, pool, num_extensions, num_blocks); |
238 | |
239 | /* Extend may only be called once */ |
240 | if (pool->num_subpools > 1) |
241 | return VCOS_EACCESS; |
242 | |
243 | if (num_extensions < 1 || |
244 | num_extensions > VCOS_BLOCKPOOL_MAX_SUBPOOLS - 1) |
245 | return VCOS_EINVAL; |
246 | |
247 | if (num_blocks < 1) |
248 | return VCOS_EINVAL; |
249 | |
250 | pool->num_subpools += num_extensions; |
251 | pool->num_extension_blocks = num_blocks; |
252 | |
253 | /* Mark these subpools as valid but unallocated */ |
254 | for (i = 1; i < pool->num_subpools; ++i) |
255 | { |
256 | pool->subpools[i].magic = VCOS_BLOCKPOOL_SUBPOOL_MAGIC; |
257 | pool->subpools[i].start = NULL; |
258 | pool->subpools[i].mem = NULL; |
259 | } |
260 | |
261 | return VCOS_SUCCESS; |
262 | } |
263 | |
264 | void *vcos_generic_blockpool_alloc(VCOS_BLOCKPOOL_T *pool) |
265 | { |
266 | VCOS_UNSIGNED i; |
267 | void* ret = NULL; |
268 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool = NULL; |
269 | |
270 | ASSERT_POOL(pool); |
271 | vcos_mutex_lock(&pool->mutex); |
272 | |
273 | /* Starting with the main pool try and find a free block */ |
274 | for (i = 0; i < pool->num_subpools; ++i) |
275 | { |
276 | if (pool->subpools[i].start && pool->subpools[i].available_blocks > 0) |
277 | { |
278 | subpool = &pool->subpools[i]; |
279 | break; /* Found a subpool with free blocks */ |
280 | } |
281 | } |
282 | |
283 | if (! subpool) |
284 | { |
285 | /* All current subpools are full, try to allocate a new one */ |
286 | for (i = 1; i < pool->num_subpools; ++i) |
287 | { |
288 | if (! pool->subpools[i].start) |
289 | { |
290 | VCOS_BLOCKPOOL_SUBPOOL_T *s = &pool->subpools[i]; |
291 | size_t size = VCOS_BLOCKPOOL_SIZE(pool->num_extension_blocks, |
292 | pool->block_data_size, pool->align); |
293 | void *mem = vcos_malloc(size, pool->name); |
294 | if (mem) |
295 | { |
296 | vcos_log_trace("%s: Allocated subpool %d" , VCOS_FUNCTION, i); |
297 | vcos_generic_blockpool_subpool_init(pool, s, mem, size, |
298 | pool->num_extension_blocks, |
299 | pool->align, |
300 | VCOS_BLOCKPOOL_SUBPOOL_FLAG_OWNS_MEM | |
301 | VCOS_BLOCKPOOL_SUBPOOL_FLAG_EXTENSION); |
302 | subpool = s; |
303 | break; /* Created a subpool */ |
304 | } |
305 | else |
306 | { |
307 | vcos_log_warn("%s: Failed to allocate subpool" , VCOS_FUNCTION); |
308 | } |
309 | } |
310 | } |
311 | } |
312 | |
313 | if (subpool) |
314 | { |
315 | /* Remove from free list */ |
316 | VCOS_BLOCKPOOL_HEADER_T* nb = subpool->free_list; |
317 | |
318 | vcos_assert(subpool->free_list); |
319 | subpool->free_list = nb->owner.next; |
320 | |
321 | /* Owner is pool so free can be called without passing pool |
322 | * as a parameter */ |
323 | nb->owner.subpool = subpool; |
324 | |
325 | ret = nb + 1; /* Return pointer to block data */ |
326 | --(subpool->available_blocks); |
327 | } |
328 | vcos_mutex_unlock(&pool->mutex); |
329 | VCOS_BLOCKPOOL_DEBUG_LOG("pool %p subpool %p ret %p" , pool, subpool, ret); |
330 | |
331 | if (ret) |
332 | { |
333 | vcos_assert(ret > subpool->start); |
334 | vcos_assert(ret < subpool->end); |
335 | } |
336 | return ret; |
337 | } |
338 | |
339 | void *vcos_generic_blockpool_calloc(VCOS_BLOCKPOOL_T *pool) |
340 | { |
341 | void* ret = vcos_generic_blockpool_alloc(pool); |
342 | if (ret) |
343 | memset(ret, 0, pool->block_data_size); |
344 | return ret; |
345 | } |
346 | |
347 | void vcos_generic_blockpool_free(void *block) |
348 | { |
349 | VCOS_BLOCKPOOL_DEBUG_LOG("block %p" , block); |
350 | if (block) |
351 | { |
352 | VCOS_BLOCKPOOL_HEADER_T* hdr = (VCOS_BLOCKPOOL_HEADER_T*) block - 1; |
353 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool = hdr->owner.subpool; |
354 | VCOS_BLOCKPOOL_T *pool = NULL; |
355 | |
356 | ASSERT_SUBPOOL(subpool); |
357 | pool = subpool->owner; |
358 | ASSERT_POOL(pool); |
359 | |
360 | vcos_mutex_lock(&pool->mutex); |
361 | vcos_assert((unsigned) subpool->available_blocks < subpool->num_blocks); |
362 | |
363 | /* Change ownership of block to be the free list */ |
364 | hdr->owner.next = subpool->free_list; |
365 | subpool->free_list = hdr; |
366 | ++(subpool->available_blocks); |
367 | |
368 | if (VCOS_BLOCKPOOL_OVERWRITE_ON_FREE) |
369 | memset(block, 0xBD, pool->block_data_size); /* For debugging */ |
370 | |
371 | if ( (subpool->flags & VCOS_BLOCKPOOL_SUBPOOL_FLAG_EXTENSION) && |
372 | subpool->available_blocks == subpool->num_blocks) |
373 | { |
374 | VCOS_BLOCKPOOL_DEBUG_LOG("%s: freeing subpool %p mem %p" , VCOS_FUNCTION, |
375 | subpool, subpool->mem); |
376 | /* Free the sub-pool if it was dynamically allocated */ |
377 | vcos_free(subpool->mem); |
378 | subpool->mem = NULL; |
379 | subpool->start = NULL; |
380 | } |
381 | vcos_mutex_unlock(&pool->mutex); |
382 | } |
383 | } |
384 | |
385 | VCOS_UNSIGNED vcos_generic_blockpool_available_count(VCOS_BLOCKPOOL_T *pool) |
386 | { |
387 | VCOS_UNSIGNED ret = 0; |
388 | VCOS_UNSIGNED i; |
389 | |
390 | ASSERT_POOL(pool); |
391 | vcos_mutex_lock(&pool->mutex); |
392 | for (i = 0; i < pool->num_subpools; ++i) |
393 | { |
394 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool = &pool->subpools[i]; |
395 | ASSERT_SUBPOOL(subpool); |
396 | |
397 | /* Assume the malloc of sub pool would succeed */ |
398 | if (subpool->start) |
399 | ret += subpool->available_blocks; |
400 | else |
401 | ret += pool->num_extension_blocks; |
402 | } |
403 | vcos_mutex_unlock(&pool->mutex); |
404 | return ret; |
405 | } |
406 | |
407 | VCOS_UNSIGNED vcos_generic_blockpool_used_count(VCOS_BLOCKPOOL_T *pool) |
408 | { |
409 | VCOS_UNSIGNED ret = 0; |
410 | VCOS_UNSIGNED i; |
411 | |
412 | ASSERT_POOL(pool); |
413 | vcos_mutex_lock(&pool->mutex); |
414 | |
415 | for (i = 0; i < pool->num_subpools; ++i) |
416 | { |
417 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool = &pool->subpools[i]; |
418 | ASSERT_SUBPOOL(subpool); |
419 | if (subpool->start) |
420 | ret += (subpool->num_blocks - subpool->available_blocks); |
421 | } |
422 | vcos_mutex_unlock(&pool->mutex); |
423 | return ret; |
424 | } |
425 | |
426 | void vcos_generic_blockpool_delete(VCOS_BLOCKPOOL_T *pool) |
427 | { |
428 | vcos_log_trace("%s: pool %p" , VCOS_FUNCTION, pool); |
429 | |
430 | if (pool) |
431 | { |
432 | VCOS_UNSIGNED i; |
433 | |
434 | ASSERT_POOL(pool); |
435 | for (i = 0; i < pool->num_subpools; ++i) |
436 | { |
437 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool = &pool->subpools[i]; |
438 | ASSERT_SUBPOOL(subpool); |
439 | if (subpool->mem) |
440 | { |
441 | /* For debugging */ |
442 | memset(subpool->mem, |
443 | 0xBE, |
444 | VCOS_BLOCKPOOL_SIZE(subpool->num_blocks, |
445 | pool->block_data_size, pool->align)); |
446 | |
447 | if (subpool->flags & VCOS_BLOCKPOOL_SUBPOOL_FLAG_OWNS_MEM) |
448 | vcos_free(subpool->mem); |
449 | subpool->mem = NULL; |
450 | subpool->start = NULL; |
451 | } |
452 | } |
453 | vcos_mutex_delete(&pool->mutex); |
454 | memset(pool, 0xBE, sizeof(VCOS_BLOCKPOOL_T)); /* For debugging */ |
455 | } |
456 | } |
457 | |
458 | uint32_t vcos_generic_blockpool_elem_to_handle(void *block) |
459 | { |
460 | uint32_t ret = -1; |
461 | uint32_t index = -1; |
462 | VCOS_BLOCKPOOL_HEADER_T *hdr = NULL; |
463 | VCOS_BLOCKPOOL_T *pool = NULL; |
464 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool = NULL; |
465 | uint32_t subpool_id; |
466 | |
467 | vcos_assert(block); |
468 | hdr = (VCOS_BLOCKPOOL_HEADER_T*) block - 1; |
469 | subpool = hdr->owner.subpool; |
470 | ASSERT_SUBPOOL(subpool); |
471 | |
472 | pool = subpool->owner; |
473 | ASSERT_POOL(pool); |
474 | vcos_mutex_lock(&pool->mutex); |
475 | |
476 | /* The handle is the index into the array of blocks combined |
477 | * with the subpool id. |
478 | */ |
479 | index = ((size_t) hdr - (size_t) subpool->start) / pool->block_size; |
480 | vcos_assert(index < subpool->num_blocks); |
481 | |
482 | subpool_id = ((char*) subpool - (char*) &pool->subpools[0]) / |
483 | sizeof(VCOS_BLOCKPOOL_SUBPOOL_T); |
484 | |
485 | vcos_assert(subpool_id < VCOS_BLOCKPOOL_MAX_SUBPOOLS); |
486 | vcos_assert(subpool_id < pool->num_subpools); |
487 | ret = VCOS_BLOCKPOOL_HANDLE_CREATE(index, subpool_id); |
488 | |
489 | vcos_log_trace("%s: index %d subpool_id %d handle 0x%08x" , |
490 | VCOS_FUNCTION, index, subpool_id, ret); |
491 | |
492 | vcos_mutex_unlock(&pool->mutex); |
493 | return ret; |
494 | } |
495 | |
496 | void *vcos_generic_blockpool_elem_from_handle( |
497 | VCOS_BLOCKPOOL_T *pool, uint32_t handle) |
498 | { |
499 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool; |
500 | uint32_t subpool_id; |
501 | uint32_t index; |
502 | void *ret = NULL; |
503 | |
504 | |
505 | ASSERT_POOL(pool); |
506 | vcos_mutex_lock(&pool->mutex); |
507 | subpool_id = VCOS_BLOCKPOOL_HANDLE_GET_SUBPOOL(handle); |
508 | |
509 | if (subpool_id < pool->num_subpools) |
510 | { |
511 | index = VCOS_BLOCKPOOL_HANDLE_GET_INDEX(handle); |
512 | subpool = &pool->subpools[subpool_id]; |
513 | if (pool->subpools[subpool_id].magic == VCOS_BLOCKPOOL_SUBPOOL_MAGIC && |
514 | pool->subpools[subpool_id].mem && index < subpool->num_blocks) |
515 | { |
516 | VCOS_BLOCKPOOL_HEADER_T *hdr = (VCOS_BLOCKPOOL_HEADER_T*) |
517 | ((size_t) subpool->start + (index * pool->block_size)); |
518 | |
519 | if (hdr->owner.subpool == subpool) /* Check block is allocated */ |
520 | ret = hdr + 1; |
521 | } |
522 | } |
523 | vcos_mutex_unlock(&pool->mutex); |
524 | |
525 | vcos_log_trace("%s: pool %p handle 0x%08x elem %p" , VCOS_FUNCTION, pool, |
526 | handle, ret); |
527 | return ret; |
528 | } |
529 | |
530 | uint32_t vcos_generic_blockpool_is_valid_elem( |
531 | VCOS_BLOCKPOOL_T *pool, const void *block) |
532 | { |
533 | uint32_t ret = 0; |
534 | const char *pool_end; |
535 | VCOS_UNSIGNED i = 0; |
536 | |
537 | ASSERT_POOL(pool); |
538 | if (((size_t) block) & 0x3) |
539 | return 0; |
540 | |
541 | vcos_mutex_lock(&pool->mutex); |
542 | |
543 | for (i = 0; i < pool->num_subpools; ++i) |
544 | { |
545 | VCOS_BLOCKPOOL_SUBPOOL_T *subpool = &pool->subpools[i]; |
546 | ASSERT_SUBPOOL(subpool); |
547 | |
548 | if (subpool->mem && subpool->start) |
549 | { |
550 | pool_end = (const char*)subpool->start + |
551 | (subpool->num_blocks * pool->block_size); |
552 | |
553 | if ((const char*)block > (const char*)subpool->start && |
554 | (const char*)block < pool_end) |
555 | { |
556 | const VCOS_BLOCKPOOL_HEADER_T *hdr = ( |
557 | const VCOS_BLOCKPOOL_HEADER_T*) block - 1; |
558 | |
559 | /* If the block has a header where the owner points to the pool then |
560 | * it's a valid block. */ |
561 | ret = (hdr->owner.subpool == subpool && subpool->owner == pool); |
562 | break; |
563 | } |
564 | } |
565 | } |
566 | vcos_mutex_unlock(&pool->mutex); |
567 | return ret; |
568 | } |
569 | |