blob: 4d80615f49cf99e251205c38e29324378cbaa3ed [file] [log] [blame]
/*
* Copyright (c) 2017 Intel Corporation
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <kernel.h>
#include <ksched.h>
#include <wait_q.h>
#include <init.h>
#include <string.h>
/* Linker-defined symbols bound the static pool structs */
extern struct k_mem_pool _k_mem_pool_list_start[];
extern struct k_mem_pool _k_mem_pool_list_end[];
s64_t _tick_get(void);
static struct k_mem_pool *get_pool(int id)
{
return &_k_mem_pool_list_start[id];
}
static int pool_id(struct k_mem_pool *pool)
{
return pool - &_k_mem_pool_list_start[0];
}
static void *block_ptr(struct k_mem_pool *p, size_t lsz, int block)
{
return p->buf + lsz * block;
}
static int block_num(struct k_mem_pool *p, void *block, int sz)
{
return (block - p->buf) / sz;
}
static bool level_empty(struct k_mem_pool *p, int l)
{
return sys_dlist_is_empty(&p->levels[l].free_list);
}
/* 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 k_mem_pool *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 k_mem_pool *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 k_mem_pool *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 k_mem_pool *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 k_mem_pool *p)
{
return p->n_max * p->max_sz;
}
static bool block_fits(struct k_mem_pool *p, void *block, size_t bsz)
{
return (block + bsz - 1 - p->buf) < buf_size(p);
}
static void init_mem_pool(struct k_mem_pool *p)
{
int i;
size_t buflen = p->n_max * p->max_sz, sz = p->max_sz;
u32_t *bits = p->buf + buflen;
sys_dlist_init(&p->wait_q);
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);
}
}
int init_static_pools(struct device *unused)
{
ARG_UNUSED(unused);
struct k_mem_pool *p;
for (p = _k_mem_pool_list_start; p < _k_mem_pool_list_end; p++) {
init_mem_pool(p);
}
return 0;
}
SYS_INIT(init_static_pools, PRE_KERNEL_1, CONFIG_KERNEL_INIT_PRIORITY_OBJECTS);
/* A note on synchronization: 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.
*/
static void *alloc_block(struct k_mem_pool *p, int l, size_t lsz)
{
sys_dnode_t *block;
int key = irq_lock();
block = sys_dlist_get(&p->levels[l].free_list);
if (block) {
clear_free_bit(p, l, block_num(p, block, lsz));
}
irq_unlock(key);
return block;
}
static void free_block(struct k_mem_pool *p, int level, size_t *lsizes, int bn)
{
int i, key, lsz = lsizes[level];
void *block = block_ptr(p, lsz, bn);
key = irq_lock();
set_free_bit(p, level, bn);
if (level && partner_bits(p, level, bn) == 0xf) {
for (i = 0; i < 4; i++) {
int b = (bn & ~3) + i;
clear_free_bit(p, level, b);
if (b != bn &&
block_fits(p, block_ptr(p, lsz, b), lsz)) {
sys_dlist_remove(block_ptr(p, lsz, b));
}
}
irq_unlock(key);
free_block(p, level-1, lsizes, bn / 4); /* tail recursion! */
return;
}
if (block_fits(p, block, lsz)) {
sys_dlist_append(&p->levels[level].free_list, block);
}
irq_unlock(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
* free_block() but without the need to check adjacent bits or
* recombine, and returns the remaining smaller block.
*/
static void *break_block(struct k_mem_pool *p, void *block,
int l, size_t *lsizes)
{
int i, bn, key;
key = irq_lock();
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);
}
}
irq_unlock(key);
return block;
}
static int pool_alloc(struct k_mem_pool *p, struct k_mem_block *block,
size_t size)
{
size_t lsizes[p->n_levels];
int i, alloc_l = -1, free_l = -1, from_l;
void *blk = NULL;
/* 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 (!level_empty(p, i)) {
free_l = i;
}
}
if (alloc_l < 0 || free_l < 0) {
block->data = NULL;
return -ENOMEM;
}
/* Iteratively break the smallest enclosing block... */
blk = alloc_block(p, free_l, lsizes[free_l]);
if (!blk) {
/* This can happen if we race with another allocator.
* It's OK, just back out and the timeout code will
* retry. Note mild overloading: -EAGAIN isn't for
* propagation to the caller, it's to tell the loop in
* k_mem_pool_alloc() to try again synchronously. But
* it means exactly what it says.
*/
return -EAGAIN;
}
for (from_l = free_l; from_l < alloc_l; from_l++) {
blk = break_block(p, blk, from_l, lsizes);
}
/* ... until we have something to return */
block->data = blk;
block->id.pool = pool_id(p);
block->id.level = alloc_l;
block->id.block = block_num(p, block->data, lsizes[alloc_l]);
return 0;
}
int k_mem_pool_alloc(struct k_mem_pool *p, struct k_mem_block *block,
size_t size, s32_t timeout)
{
int ret, key;
s64_t end = 0;
__ASSERT(!(_is_in_isr() && timeout != K_NO_WAIT), "");
if (timeout > 0) {
end = _tick_get() + _ms_to_ticks(timeout);
}
while (1) {
ret = pool_alloc(p, block, size);
if (ret == 0 || timeout == K_NO_WAIT ||
ret == -EAGAIN || (ret && ret != -ENOMEM)) {
return ret;
}
key = irq_lock();
_pend_current_thread(&p->wait_q, timeout);
_Swap(key);
if (timeout != K_FOREVER) {
timeout = end - _tick_get();
if (timeout < 0) {
break;
}
}
}
return -EAGAIN;
}
void k_mem_pool_free(struct k_mem_block *block)
{
int i, key, need_sched = 0;
struct k_mem_pool *p = get_pool(block->id.pool);
size_t lsizes[p->n_levels];
/* As in k_mem_pool_alloc(), we build a table of level sizes
* to avoid having to store it in precious RAM bytes.
* Overhead here is somewhat higher because free_block()
* doesn't inherently need to traverse all the larger
* sublevels.
*/
lsizes[0] = _ALIGN4(p->max_sz);
for (i = 1; i <= block->id.level; i++) {
lsizes[i] = _ALIGN4(lsizes[i-1] / 4);
}
free_block(get_pool(block->id.pool), block->id.level,
lsizes, block->id.block);
/* Wake up anyone blocked on this pool and let them repeat
* their allocation attempts
*/
key = irq_lock();
while (!sys_dlist_is_empty(&p->wait_q)) {
struct k_thread *th = (void *)sys_dlist_peek_head(&p->wait_q);
_unpend_thread(th);
_abort_thread_timeout(th);
_ready_thread(th);
need_sched = 1;
}
if (need_sched && !_is_in_isr()) {
_reschedule_threads(key);
} else {
irq_unlock(key);
}
}
#if (CONFIG_HEAP_MEM_POOL_SIZE > 0)
/*
* Heap is defined using HEAP_MEM_POOL_SIZE configuration option.
*
* This module defines the heap memory pool and the _HEAP_MEM_POOL symbol
* that has the address of the associated memory pool struct.
*/
K_MEM_POOL_DEFINE(_heap_mem_pool, 64, CONFIG_HEAP_MEM_POOL_SIZE, 1, 4);
#define _HEAP_MEM_POOL (&_heap_mem_pool)
void *k_malloc(size_t size)
{
struct k_mem_block block;
/*
* get a block large enough to hold an initial (hidden) block
* descriptor, as well as the space the caller requested
*/
size += sizeof(struct k_mem_block);
if (k_mem_pool_alloc(_HEAP_MEM_POOL, &block, size, K_NO_WAIT) != 0) {
return NULL;
}
/* save the block descriptor info at the start of the actual block */
memcpy(block.data, &block, sizeof(struct k_mem_block));
/* return address of the user area part of the block to the caller */
return (char *)block.data + sizeof(struct k_mem_block);
}
void k_free(void *ptr)
{
if (ptr != NULL) {
/* point to hidden block descriptor at start of block */
ptr = (char *)ptr - sizeof(struct k_mem_block);
/* return block to the heap memory pool */
k_mem_pool_free(ptr);
}
}
#endif