source: svn/newcon3bcm2_21bu/BSEAV/lib/utils/bpool.c

/***************************************************************************
 *     Copyright (c) 2007-2008, Broadcom Corporation
 *     All Rights Reserved
 *     Confidential Property of Broadcom Corporation
 *
 *  THIS SOFTWARE MAY ONLY BE USED SUBJECT TO AN EXECUTED SOFTWARE LICENSE
 *  AGREEMENT  BETWEEN THE USER AND BROADCOM.  YOU HAVE NO RIGHT TO USE OR
 *  EXPLOIT THIS MATERIAL EXCEPT SUBJECT TO THE TERMS OF SUCH AN AGREEMENT.
 *
 * $brcm_Workfile: bpool.c $
 * $brcm_Revision: 6 $
 * $brcm_Date: 6/12/08 3:07p $
 *
 * Module Description:
 *
 * Pool alloc library
 * 
 * Revision History:
 *
 * $brcm_Log: /BSEAV/lib/utils/bpool.c $
 * 
 * 6   6/12/08 3:07p vsilyaev
 * PR 43607: Renamed alloc and free to bmem_alloc and bmem_free
 * 
 * 5   4/17/07 2:01p vsilyaev
 * PR 29921: Demoted debug message
 * 
 * 4   3/27/07 7:24p vsilyaev
 * PR 29125: Relaxed test to pool alloc
 * 
 * 3   2/15/07 3:12p vsilyaev
 * PR 25701: Added code to handle parsing during resource starvation
 * 
 * 2   2/14/07 5:45p vsilyaev
 * PR 25701: Added code to dump content/status of allocator
 * 
 * 1   2/5/07 5:29p vsilyaev
 * PR 25701:Portions of stackable parsing framework
 * 
 * 
 *******************************************************************************/
#include "bstd.h"
#include "bkni.h"
#include "bpool.h"
#include "blst_selist.h"

BDBG_MODULE(bpool);

#define BDBG_MSG_TRACE(x)  /* BDBG_MSG(x) */

BDBG_OBJECT_ID(bpool_t);

struct bpool {
        struct balloc_iface alloc_iface; /* must be a first member */
        uint16_t last_free;
        uint16_t nfree_elem;
        uint16_t elem_size;
        uint16_t nelem;
        uint32_t *bitmap; /* pointer to bitmap */
        BLST_SE_ENTRY(bpool);
        balloc_iface_t alloc;
        BDBG_OBJECT(bpool_t)
        /* 
         allocated objects start here 
        */
        /* 
         object bitmap start here, and bitmap points here as well 
        */
};
#define B_POOL_BITS_IN_MAP      32

static unsigned 
b_pool_first_zero(uint32_t word)
{
        unsigned i;
        for(i=0;i<B_POOL_BITS_IN_MAP;i++) {
                if (!(word&(1<<i))) {
                        return i;
                }
        }
        BDBG_ASSERT(0);
        return 0;
}

void *
bpool_alloc(bpool_t pool, size_t size)
{
        uint16_t i;
        BSTD_UNUSED(size);

        BDBG_OBJECT_ASSERT(pool, bpool_t);
        BDBG_ASSERT(size<=pool->elem_size);

        do {
                for(;pool->nfree_elem>0;) {
                        for(i=pool->last_free;i<(pool->nelem/B_POOL_BITS_IN_MAP);i++) {
                                if(pool->bitmap[i]!=(~0u)) {
                                        unsigned bit;

                                        pool->last_free = i;
                                        bit = b_pool_first_zero(pool->bitmap[i]);
                                        pool->bitmap[i] |= (1<<bit);
                                        BDBG_ASSERT(pool->nfree_elem>0);
                                        pool->nfree_elem--;
                                        BDBG_MSG_TRACE(("bpool_alloc: %#lx %#lx offset %u [%x:%u]", (unsigned long)pool, (unsigned long)((uint8_t *)pool + sizeof(*pool) + pool->elem_size*(i*B_POOL_BITS_IN_MAP + bit)), i, (unsigned)pool->bitmap[i], bit));
                                        return (void *)((uint8_t *)pool + sizeof(*pool) + pool->elem_size*(i*B_POOL_BITS_IN_MAP + bit));
                                }
                        }
                        if(pool->last_free==0) {
                                break; /* there is no empty space left */
                        }
                        pool->last_free=0; /* rescan entire bitmap */
                }
                BDBG_MSG(("bpool_alloc:%#lx exhausted", (unsigned long)pool));
        } while(NULL!=(pool=BLST_SE_NEXT(pool)));
        return NULL;
}

static bool 
b_pool_test_one(bpool_t pool, void *ptr)
{
        if ((uint8_t *)ptr >= (uint8_t *)pool + sizeof(*pool) && (uint8_t *)ptr <= (uint8_t *)pool->bitmap - pool->elem_size) {
                return true;
        } else {
                return false;
        }
}

bool
bpool_test_block(bpool_t pool, void *ptr)
{
        BDBG_OBJECT_ASSERT(pool, bpool_t);
        do {
                BDBG_OBJECT_ASSERT(pool, bpool_t);
                if(b_pool_test_one(pool, ptr)) {
                        return true;
                }
        } while(NULL!=(pool=BLST_SE_NEXT(pool)));
        return false;
}

void
bpool_join(bpool_t parent, bpool_t child)
{
        BDBG_OBJECT_ASSERT(parent, bpool_t);
        BDBG_OBJECT_ASSERT(child, bpool_t);
        BDBG_ASSERT(parent->elem_size == child->elem_size);
        BDBG_ASSERT(parent != child);

        BLST_SE_INSERT_AFTER(parent, child);
        return;
}

bool
bpool_is_empty(bpool_t pool)
{
        BDBG_OBJECT_ASSERT(pool, bpool_t);
        /* this function doesn't query child pools */
        return pool->nfree_elem == pool->nelem;
}

bpool_t 
bpool_last_child(bpool_t pool)
{
        BDBG_OBJECT_ASSERT(pool, bpool_t);
        /* adjust to the child right the way */
        while(NULL!=(pool=BLST_SE_NEXT(pool))) {
                if (BLST_SE_NEXT(pool)==NULL) {
                        return pool;
                }
        }
        return NULL;
}

void 
bpool_detach(bpool_t parent, bpool_t child)
{
        bpool_t pool;

        BDBG_OBJECT_ASSERT(parent, bpool_t);
        BDBG_OBJECT_ASSERT(child, bpool_t);
        BDBG_ASSERT(parent->elem_size == child->elem_size);
        BDBG_ASSERT(parent != child);
        BDBG_ASSERT(bpool_is_empty(child)); /* can't delete busy child */

        pool = parent;
        do {
                if(child==BLST_SE_NEXT(pool)) {
                        BLST_SE_REMOVE_BEFORE(pool, child);
                        return;
                }
        } while(NULL!=(pool=BLST_SE_NEXT(pool)));
        BDBG_ASSERT(0); /* child doesn't belong to a parent */
        return;
}

void 
bpool_free(bpool_t pool, void *ptr)
{
        unsigned offset;
        unsigned bit;

        BDBG_OBJECT_ASSERT(pool, bpool_t);
        BDBG_ASSERT(ptr);
        BDBG_ASSERT(bpool_test_block(pool, ptr));
        do {
                if (!b_pool_test_one(pool,ptr)) {
                        continue; /* try next pool */
                }
                offset = (uint8_t *)ptr - ((uint8_t *)pool + sizeof(*pool));
                BDBG_ASSERT((offset % pool->elem_size) == 0);
                offset = offset/pool->elem_size;
                bit = offset % B_POOL_BITS_IN_MAP;
                offset = offset/B_POOL_BITS_IN_MAP;
                BDBG_MSG_TRACE(("bpool_free: %#lx %#lx offset %u [%#x:%u]", (unsigned long)pool, (unsigned long)ptr, offset, (unsigned)pool->bitmap[offset], bit));
                BDBG_ASSERT(pool->bitmap[offset] & (1<<bit)); /* block shall have been allocated */
                pool->bitmap[offset] &= ~((1<<bit)); /* clear alloc bit */
                BDBG_ASSERT(pool->nfree_elem<pool->nelem);
                pool->nfree_elem++;
                return;
        } while(NULL!=(pool=BLST_SE_NEXT(pool)));
        BDBG_ASSERT(0); /* block wasn't allocated */
        return;
}

static void *
b_pool_alloc(balloc_iface_t alloc, size_t size)
{
        return bpool_alloc((bpool_t)alloc, size);
}

static void
b_pool_free(balloc_iface_t alloc, void *ptr)
{
        bpool_free((bpool_t)alloc, ptr);
}

bpool_t 
bpool_create(balloc_iface_t alloc, size_t nelem, size_t elem_size)
{
        bpool_t pool;
        size_t map_size;

        BDBG_ASSERT(nelem);
        BDBG_ASSERT(elem_size);
        BDBG_ASSERT(alloc);
        BDBG_ASSERT(nelem<65536);
        BDBG_ASSERT(elem_size<65536);

        BDBG_CASSERT(sizeof(uint32_t)==(B_POOL_BITS_IN_MAP/8));
        nelem = (nelem+(B_POOL_BITS_IN_MAP-1))&(~(B_POOL_BITS_IN_MAP-1));
        map_size = nelem/B_POOL_BITS_IN_MAP;

        pool = alloc->bmem_alloc(alloc, nelem*elem_size + sizeof(*pool) + map_size*sizeof(uint32_t));
        BDBG_MSG(("bpool_create: %#lx overhead %u allocated %u (bytes)", (unsigned long)pool, sizeof(*pool), nelem*elem_size + sizeof(*pool) + map_size*sizeof(uint32_t)));
        if (!pool) {
                BDBG_ERR(("alloc failed (%u bytes)", nelem*elem_size + sizeof(*pool) + map_size*sizeof(uint32_t)));
                return NULL;
        }
        BDBG_OBJECT_INIT(pool, bpool_t);
        pool->alloc_iface.bmem_alloc = b_pool_alloc;
        pool->alloc_iface.bmem_free = b_pool_free;
        pool->alloc = alloc;
        BLST_SE_INIT(pool);
        pool->bitmap = (uint32_t *)((uint8_t *)pool + sizeof(*pool) + nelem*elem_size);
        pool->nelem = nelem;
        pool->elem_size = elem_size;
        pool->last_free = 0;
        pool->nfree_elem = nelem;
        BKNI_Memset(pool->bitmap, 0, map_size*sizeof(uint32_t)); /* mark all as free */
        return pool;
}

void
bpool_destroy(bpool_t pool)
{
        balloc_iface_t alloc;
        bpool_t next;

        BDBG_MSG_TRACE(("bpool_destroy: >%#lx", pool));
        BDBG_OBJECT_ASSERT(pool, bpool_t);
        do {
                BDBG_MSG_TRACE(("bpool_destroy: +%#lx", pool));
                BDBG_OBJECT_ASSERT(pool, bpool_t);
                alloc = pool->alloc;
                next = BLST_SE_NEXT(pool);
                BDBG_OBJECT_DESTROY(pool, bpool_t);
                alloc->bmem_free(alloc, pool);
                BDBG_MSG_TRACE(("bpool_destroy: -%#lx", pool));
                pool=next;
        } while(pool);
        BDBG_MSG_TRACE(("bpool_destroy: <%#lx", pool));
        return;
}


balloc_iface_t 
bpool_alloc_iface(bpool_t pool)
{
        BDBG_OBJECT_ASSERT(pool, bpool_t);
        return &pool->alloc_iface;
}

void
bpool_get_status(bpool_t pool, bpool_status *status)
{
        BDBG_OBJECT_ASSERT(pool, bpool_t);
        BDBG_ASSERT(status);

        status->pools = 0;
        status->free = 0;
        status->allocated = 0;
        for(;pool;pool=BLST_SE_NEXT(pool)) {
                BDBG_OBJECT_ASSERT(pool, bpool_t);
                status->pools ++;
                status->free += pool->nfree_elem;
                status->allocated += pool->nelem - pool->nfree_elem;
        }
}

void
bpool_dump(bpool_t pool)
{
        uint16_t i,bit;
        BDBG_OBJECT_ASSERT(pool, bpool_t);

        for(;pool;pool=BLST_SE_NEXT(pool)) {
                BDBG_WRN(("bpool_dump: pool %#lx free:%u allocated:%u", (unsigned long)pool, pool->nfree_elem, pool->nelem-pool->nfree_elem));
                for(i=0;i<(pool->nelem/B_POOL_BITS_IN_MAP);i++) {
                        if(pool->bitmap[i]) {
                                for(bit=0;bit<B_POOL_BITS_IN_MAP;bit++) {
                                        if(pool->bitmap[i]&(1<<bit)) {
                                                BDBG_WRN(("bpool_dump: %#lx %#lx offset %u [%#x:%u]", (unsigned long)pool, (unsigned long)((uint8_t *)pool + sizeof(*pool) + pool->elem_size*(i*B_POOL_BITS_IN_MAP + bit)), i, (unsigned)pool->bitmap[i], bit));
                                        }
                                }
                        }
                }
        }
}