blob: 6fd8fd576be2b893a43e3da6433d1e496c4b29b3 [file] [log] [blame]
/*
* Copyright (c) 2018 Intel Corporation
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <kernel.h>
#include <string.h>
#include <misc/__assert.h>
#include <misc/mempool_base.h>
#include <misc/mempool.h>
#ifdef CONFIG_MISRA_SANE
#define LVL_ARRAY_SZ(n) (8 * sizeof(void *) / 2)
#else
#define LVL_ARRAY_SZ(n) (n)
#endif
static void *block_ptr(struct sys_mem_pool_base *p, size_t lsz, int block)
{
return (u8_t *)p->buf + lsz * block;
}
static int block_num(struct sys_mem_pool_base *p, void *block, int sz)
{
return ((u8_t *)block - (u8_t *)p->buf) / sz;
}
/* Places a 32 bit output pointer in word, and an integer bit index
* within that word as the return value
*/
static int get_bit_ptr(struct sys_mem_pool_base *p, int level, int bn,
u32_t **word)
{
u32_t *bitarray = level <= p->max_inline_level ?
&p->levels[level].bits : p->levels[level].bits_p;
*word = &bitarray[bn / 32];
return bn & 0x1f;
}
static void set_free_bit(struct sys_mem_pool_base *p, int level, int bn)
{
u32_t *word;
int bit = get_bit_ptr(p, level, bn, &word);
*word |= (1<<bit);
}
static void clear_free_bit(struct sys_mem_pool_base *p, int level, int bn)
{
u32_t *word;
int bit = get_bit_ptr(p, level, bn, &word);
*word &= ~(1<<bit);
}
/* Returns all four of the free bits for the specified blocks
* "partners" in the bottom 4 bits of the return value
*/
static int partner_bits(struct sys_mem_pool_base *p, int level, int bn)
{
u32_t *word;
int bit = get_bit_ptr(p, level, bn, &word);
return (*word >> (4*(bit / 4))) & 0xf;
}
static size_t buf_size(struct sys_mem_pool_base *p)
{
return p->n_max * p->max_sz;
}
static bool block_fits(struct sys_mem_pool_base *p, void *block, size_t bsz)
{
return ((u8_t *)block + bsz - 1 - (u8_t *)p->buf) < buf_size(p);
}
void z_sys_mem_pool_base_init(struct sys_mem_pool_base *p)
{
int i;
size_t buflen = p->n_max * p->max_sz, sz = p->max_sz;
u32_t *bits = (u32_t *)((u8_t *)p->buf + buflen);
p->max_inline_level = -1;
for (i = 0; i < p->n_levels; i++) {
int nblocks = buflen / sz;
sys_dlist_init(&p->levels[i].free_list);
if (nblocks < 32) {
p->max_inline_level = i;
} else {
p->levels[i].bits_p = bits;
bits += (nblocks + 31)/32;
}
sz = _ALIGN4(sz / 4);
}
for (i = 0; i < p->n_max; i++) {
void *block = block_ptr(p, p->max_sz, i);
sys_dlist_append(&p->levels[0].free_list, block);
set_free_bit(p, 0, i);
}
}
/* A note on synchronization:
*
* For k_mem_pools which are interrupt safe, all manipulation of the actual
* pool data happens in one of alloc_block()/free_block() or break_block().
* All of these transition between a state where the caller "holds" a block
* pointer that is marked used in the store and one where she doesn't (or else
* they will fail, e.g. if there isn't a free block). So that is the basic
* operation that needs synchronization, which we can do piecewise as needed in
* small one-block chunks to preserve latency. At most (in free_block) a
* single locked operation consists of four bit sets and dlist removals. If the
* overall allocation operation fails, we just free the block we have (putting
* a block back into the list cannot fail) and return failure.
*
* For user mode compatible sys_mem_pool pools, a semaphore is used at the API
* level since using that does not introduce latency issues like locking
* interrupts does.
*/
static inline int pool_irq_lock(struct sys_mem_pool_base *p)
{
if (p->flags & SYS_MEM_POOL_KERNEL) {
return irq_lock();
} else {
return 0;
}
}
static inline void pool_irq_unlock(struct sys_mem_pool_base *p, int key)
{
if (p->flags & SYS_MEM_POOL_KERNEL) {
irq_unlock(key);
}
}
static void *block_alloc(struct sys_mem_pool_base *p, int l, size_t lsz)
{
sys_dnode_t *block;
block = sys_dlist_get(&p->levels[l].free_list);
if (block != NULL) {
clear_free_bit(p, l, block_num(p, block, lsz));
}
return block;
}
/* Called with lock held */
static unsigned int bfree_recombine(struct sys_mem_pool_base *p, int level,
size_t *lsizes, int bn, unsigned int key)
{
while (level >= 0) {
int i, lsz = lsizes[level];
void *block = block_ptr(p, lsz, bn);
__ASSERT(block_fits(p, block, lsz), "");
/* Put it back */
set_free_bit(p, level, bn);
sys_dlist_append(&p->levels[level].free_list, block);
/* Relax the lock (might result in it being taken, which is OK!) */
pool_irq_unlock(p, key);
key = pool_irq_lock(p);
/* Check if we can recombine its superblock, and repeat */
if (level == 0 || partner_bits(p, level, bn) != 0xf) {
return key;
}
for (i = 0; i < 4; i++) {
int b = (bn & ~3) + i;
if (block_fits(p, block_ptr(p, lsz, b), lsz)) {
clear_free_bit(p, level, b);
sys_dlist_remove(block_ptr(p, lsz, b));
}
}
/* Free the larger block */
level = level - 1;
bn = bn / 4;
}
__ASSERT(0, "out of levels");
return -1;
}
static void block_free(struct sys_mem_pool_base *p, int level,
size_t *lsizes, int bn)
{
unsigned int key = pool_irq_lock(p);
key = bfree_recombine(p, level, lsizes, bn, key);
pool_irq_unlock(p, key);
}
/* Takes a block of a given level, splits it into four blocks of the
* next smaller level, puts three into the free list as in
* block_free() but without the need to check adjacent bits or
* recombine, and returns the remaining smaller block.
*/
static void *block_break(struct sys_mem_pool_base *p, void *block, int l,
size_t *lsizes)
{
int i, bn;
bn = block_num(p, block, lsizes[l]);
for (i = 1; i < 4; i++) {
int lbn = 4*bn + i;
int lsz = lsizes[l + 1];
void *block2 = (lsz * i) + (char *)block;
set_free_bit(p, l + 1, lbn);
if (block_fits(p, block2, lsz)) {
sys_dlist_append(&p->levels[l + 1].free_list, block2);
}
}
return block;
}
int z_sys_mem_pool_block_alloc(struct sys_mem_pool_base *p, size_t size,
u32_t *level_p, u32_t *block_p, void **data_p)
{
int i, from_l, alloc_l = -1;
unsigned int key;
void *data = NULL;
size_t lsizes[LVL_ARRAY_SZ(p->n_levels)];
/* Walk down through levels, finding the one from which we
* want to allocate and the smallest one with a free entry
* from which we can split an allocation if needed. Along the
* way, we populate an array of sizes for each level so we
* don't need to waste RAM storing it.
*/
lsizes[0] = _ALIGN4(p->max_sz);
for (i = 0; i < p->n_levels; i++) {
if (i > 0) {
lsizes[i] = _ALIGN4(lsizes[i-1] / 4);
}
if (lsizes[i] < size) {
break;
}
alloc_l = i;
}
if (alloc_l < 0) {
*data_p = NULL;
return -ENOMEM;
}
/* Now walk back down the levels (i.e. toward bigger sizes)
* looking for an available block. Start at the smallest
* enclosing block found above (note that because that loop
* was done without synchronization, it may no longer be
* available!) as a useful optimization. Note that the
* removal of the block from the list and the re-addition of
* its the three unused children needs to be performed
* atomically, otherwise we open up a situation where we can
* "steal" the top level block of the whole heap, causing a
* spurious -ENOMEM.
*/
key = pool_irq_lock(p);
for (i = alloc_l; i >= 0; i--) {
data = block_alloc(p, i, lsizes[i]);
/* Found one. Iteratively break it down to the size
* we need. Note that we relax the lock to allow a
* pending interrupt to fire so we don't hurt latency
* by locking the full loop.
*/
if (data != NULL) {
for (from_l = i; from_l < alloc_l; from_l++) {
pool_irq_unlock(p, key);
key = pool_irq_lock(p);
data = block_break(p, data, from_l, lsizes);
}
break;
}
}
pool_irq_unlock(p, key);
*data_p = data;
if (data == NULL) {
return -ENOMEM;
}
*level_p = alloc_l;
*block_p = block_num(p, data, lsizes[alloc_l]);
return 0;
}
void z_sys_mem_pool_block_free(struct sys_mem_pool_base *p, u32_t level,
u32_t block)
{
size_t lsizes[LVL_ARRAY_SZ(p->n_levels)];
int i;
/* As in z_sys_mem_pool_block_alloc(), we build a table of level sizes
* to avoid having to store it in precious RAM bytes.
* Overhead here is somewhat higher because block_free()
* doesn't inherently need to traverse all the larger
* sublevels.
*/
lsizes[0] = _ALIGN4(p->max_sz);
for (i = 1; i <= level; i++) {
lsizes[i] = _ALIGN4(lsizes[i-1] / 4);
}
block_free(p, level, lsizes, block);
}
/*
* Functions specific to user-mode blocks
*/
void *sys_mem_pool_alloc(struct sys_mem_pool *p, size_t size)
{
struct sys_mem_pool_block *blk;
u32_t level, block;
char *ret;
k_mutex_lock(p->mutex, K_FOREVER);
size += sizeof(struct sys_mem_pool_block);
if (z_sys_mem_pool_block_alloc(&p->base, size, &level, &block,
(void **)&ret)) {
ret = NULL;
goto out;
}
blk = (struct sys_mem_pool_block *)ret;
blk->level = level;
blk->block = block;
blk->pool = p;
ret += sizeof(*blk);
out:
k_mutex_unlock(p->mutex);
return ret;
}
void sys_mem_pool_free(void *ptr)
{
struct sys_mem_pool_block *blk;
struct sys_mem_pool *p;
if (ptr == NULL) {
return;
}
blk = (struct sys_mem_pool_block *)((char *)ptr - sizeof(*blk));
p = blk->pool;
k_mutex_lock(p->mutex, K_FOREVER);
z_sys_mem_pool_block_free(&p->base, blk->level, blk->block);
k_mutex_unlock(p->mutex);
}