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