FreeRTOS/Source/portable/MemMang/static-allocator.c

#include "FreeRTOS.h"
#include "static-allocator.h"
#include <stdlib.h>
#include <stdio.h>
#include "task.h"

typedef struct priv_static_pool_s {
    uint8_t*  backing_store;
    uint32_t* flags;
    int       item_size;
    int       num_items;
} priv_static_pool_t;

#ifdef PORTABLE_ARCH_ARM_CM3
__attribute__( ( always_inline ) ) static inline uint8_t __CLZ(uint32_t value)
{
    uint8_t result;
    __asm__ volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
    return(result);
}
#else
__attribute__( ( always_inline ) ) static inline uint8_t __CLZ(uint32_t value)
{
    uint8_t result = 0;
    uint32_t mask = 0x1<<31;
    int i;

    for (i = 0; i < 32; i++)
    {
        if ((mask & value) != 0)
        {
            break;
        }
        result++;
        mask >>= 1;
    }
    return result;
}
#endif

#ifdef PORTABLE_ARCH_ARM_CM3
__attribute__((naked)) static void atomicClearFlag( uint32_t* address, int bitNumber )
{
   __asm__ volatile (
       "1:                         \n\t"
       "   ldrex   r2, [%0]        \n\t"
       "   bic     r2, %1          \n\t"
       "   strex   r3, r2, [%0]    \n\t"
       "   cmp     r3,#0           \n\t"
       "   bne     1b              \n\t"
       "   bx      lr              \n\t"
       : //Output Argument
       :"r"(address),"r"(bitNumber)        //Input Arguments
       :"cc", "r2", "r3");
}
#else
static void atomicClearFlag( uint32_t* address, int bitNumber )
{
    *address &= ~(bitNumber);
}
#endif

#ifdef PORTABLE_ARCH_ARM_CM3
__attribute__((naked)) static void  atomicSetFlag( uint32_t* address, int bitNumber )
{
   __asm__ volatile (
       "1:                         \n\t"
       "   ldrex   r2, [%0]        \n\t"
       "   orr     r2, %1          \n\t"
       "   strex   r3, r2, [%0]    \n\t"
       "   cmp     r3,#0           \n\t"
       "   bne     1b              \n\t"
       "   bx      lr              \n\t"
       :
       :"r"(address),"r"(bitNumber)
       :"cc", "r2", "r3");
}
#else
static void  atomicSetFlag( uint32_t* address, int bitNumber )
{
    *address |= (bitNumber);
}
#endif

#define NUM_ITEMS_TO_NUM_FLAGS(items) (((items)+31)/32)

void poolInit( static_pool_t* inPool, int inItemSize, int inNumItems, void* inBackingStore, void* inFlags)
{
    priv_static_pool_t* aPool = (priv_static_pool_t*) inPool;
    int i;
    aPool->item_size = inItemSize;
    aPool->num_items = inNumItems;
    aPool->backing_store = inBackingStore;
    aPool->flags = inFlags;
    for(i = 0; i < NUM_ITEMS_TO_NUM_FLAGS(inNumItems); i++)
    {
        aPool->flags[i] = ~((uint32_t)0x0ul);
    }
    if (inNumItems % 32)
    {
        //mark all flags as already taken that don't represent an item.
        aPool->flags[i-1] = aPool->flags[i-1] << (32 - (inNumItems % 32));
    }
}

void *poolAllocateBuffer( static_pool_t* inPool )
{
    vTaskSuspendAll();

    priv_static_pool_t* aPool = (priv_static_pool_t*) inPool;
    void *retval = NULL;
    int flagIndex;
    for(flagIndex = 0; flagIndex < NUM_ITEMS_TO_NUM_FLAGS(aPool->num_items); flagIndex++)
    {
        int8_t leadingZeros = __CLZ(aPool->flags[flagIndex]);
        if(leadingZeros != 32)
        {
            int8_t bitIndex = 31-leadingZeros;
            int8_t itemIndex = 32*flagIndex+(31-bitIndex);
            atomicClearFlag(&aPool->flags[flagIndex], 1 << bitIndex);
            retval = &aPool->backing_store[aPool->item_size*itemIndex];
            break;
        }
    }

    xTaskResumeAll();

    return retval;
}

void poolFreeBuffer( static_pool_t* inPool, void* inPointer)
{
    vTaskSuspendAll();

    priv_static_pool_t* aPool = (priv_static_pool_t*) inPool;

    int itemIndex = ((intptr_t)inPointer - (intptr_t)inPool->backing_store)/inPool->item_size;
    int flagIndex = itemIndex / 32;
    int bitIndex = 31-(itemIndex % 32);
    if(flagIndex >= NUM_ITEMS_TO_NUM_FLAGS(aPool->num_items))
    {
        printf("ERROR freeing pool %p, Pointer %p doesnt belong here\n", inPool, inPointer);
        while(1)
            ;
    }

    atomicSetFlag(&aPool->flags[flagIndex], 1 << bitIndex);

    xTaskResumeAll();
}