| 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 | |