src/bsp/lk/lib/mempool/mempool.c

/*
 * Copyright (c) 2017 MediaTek Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files
 * (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge,
 * publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <assert.h>
#include <err.h>
#include <kernel/mutex.h>
#include <lib/console.h>
#include <lib/mempool.h>
#include <list.h>
#include <malloc.h>
#include <stdlib.h>
#include <string.h>
#include <trace.h>

#define LOCAL_TRACE 0

struct mem_chunk {
    struct list_node node;
    void *start;
    size_t len;
    bool free;
};

struct mempool {
    struct list_node chunk_list;
    void *start;
    size_t len;
};

static struct mempool pool[MAX_MEMPOOL_TYPE] = {
    { LIST_INITIAL_VALUE(pool[0].chunk_list), NULL, 0 },
    { LIST_INITIAL_VALUE(pool[1].chunk_list), NULL, 0 }
};

static mutex_t memlock = MUTEX_INITIAL_VALUE(memlock);

int mempool_init(void *mem, size_t size, uint32_t type)
{
    int i;
    int ret;
    struct mem_chunk *chunk;

    LTRACEF("pool init %p, size %zx, type %d\n", mem, size, type);

    /* check input arg, mem address should be aligned to a cache line */
    if (!mem || !IS_ALIGNED(mem, CACHE_LINE) ||
            !size || (type >= MAX_MEMPOOL_TYPE))
        return ERR_INVALID_ARGS;

    ret = NO_ERROR;
    mutex_acquire(&memlock);
    /* check if the mem address already inited */
    for (i = 0; i < MAX_MEMPOOL_TYPE; i++) {
        list_for_every_entry(&pool[i].chunk_list, chunk,
                             struct mem_chunk, node) {
            if (chunk->start <= mem && ((chunk->start + chunk->len) > mem)) {
                ret = ERR_ALREADY_EXISTS;
                goto exit;
            }
        }
    }

    chunk = (struct mem_chunk *)malloc(sizeof(struct mem_chunk));
    if (!chunk) {
        ret = ERR_NO_MEMORY;
        goto exit;
    }

    chunk->start = mem;
    chunk->len = size;
    chunk->free = true;
    list_add_tail(&pool[type].chunk_list, &chunk->node);

exit:
    mutex_release(&memlock);
    return ret;
}

void *mempool_alloc(size_t size, uint32_t type)
{
    bool found;
    uint32_t i, s_type, e_type;
    size_t alloc_size;
    struct mem_chunk *chunk, *new_chunk;

    LTRACEF("pool alloc size %zx, type %d\n", size, type);

    if (!size ||
            ((type >= MAX_MEMPOOL_TYPE) && (type != MEMPOOL_ANY)))
        return NULL;

    alloc_size = ROUNDUP(size, CACHE_LINE);
    found = false;

    s_type = e_type = type;
    if (type == MEMPOOL_ANY) {
        s_type = 0;
        e_type = MAX_MEMPOOL_TYPE - 1;
    }

    mutex_acquire(&memlock);
    for (i = s_type; i <= e_type; i++) {
        list_for_every_entry(&pool[i].chunk_list, chunk,
                             struct mem_chunk, node) {
            if (chunk->len < alloc_size || !chunk->free)
                continue;

            found = true;
            break;
        }

        if (found)
            break;
    }

    new_chunk = NULL;
    if (found) {
        /* if the chunk len happend to equal to alloc size, just return it */
        if (chunk->len == alloc_size) {
            chunk->free = false;
            new_chunk = chunk;
        } else {
            new_chunk = (struct mem_chunk *)malloc(sizeof(struct mem_chunk));
            if (new_chunk) {
                new_chunk->start = chunk->start;
                new_chunk->len = alloc_size;
                new_chunk->free = false;
                chunk->start = chunk->start + alloc_size;
                chunk->len -= alloc_size;
                list_add_before(&chunk->node, &new_chunk->node);
            }
        }
    }

    mutex_release(&memlock);
    return new_chunk ? new_chunk->start : NULL;
}

void mempool_free(void *ptr)
{
    int i;
    bool found;
    struct mem_chunk *chunk, *prev, *next;

    LTRACEF("pool free %p\n", ptr);

    if (NULL == ptr)
        return;

    /* walk through list to find matched chunk */
    found = false;
    mutex_acquire(&memlock);
    for (i = 0; i < MAX_MEMPOOL_TYPE; i++) {
        list_for_every_entry(&pool[i].chunk_list, chunk,
                             struct mem_chunk, node) {
            if (!chunk->free && (chunk->start == ptr)) {
                found = true;
                break;
            }
        }

        if (found) {
            /* merge with adjecent chunk if possible */
            prev = list_prev_type(&pool[i].chunk_list, &chunk->node,
                                  struct mem_chunk, node);
            if (prev && prev->free && ((prev->start + prev->len) == ptr)) {
                chunk->len += prev->len;
                chunk->start = prev->start;
                list_delete(&prev->node);
                free(prev);
                prev = NULL;
            }

            next = list_next_type(&pool[i].chunk_list, &chunk->node,
                                  struct mem_chunk, node);
            if (next && next->free &&
                    ((chunk->start + chunk->len) == next->start)) {
                chunk->len += next->len;
                list_delete(&next->node);
                free(next);
                next = NULL;
            }

            chunk->free = true;

            break;
        }
    }

    mutex_release(&memlock);
}

void mempool_clear(void)
{
    int i;
    struct mem_chunk *chunk;
    struct mem_chunk *temp;

    /* delete every node in the list */
    mutex_acquire(&memlock);
    for (i = 0; i < MAX_MEMPOOL_TYPE; i++) {
        list_for_every_entry_safe(&pool[i].chunk_list, chunk, temp,
                                  struct mem_chunk, node) {
            list_delete(&chunk->node);
            free(chunk);
            chunk = NULL;
        }
        pool[i].start = NULL;
        pool[i].len = 0;
    }

    mutex_release(&memlock);
}

#if LK_DEBUGLEVEL > 1

#include <lib/console.h>

static int cmd_mempool(int argc, const cmd_args *argv);
static void show_usage(const char *cmd);

STATIC_COMMAND_START
STATIC_COMMAND("mempool", "mempool debug commands", &cmd_mempool)
STATIC_COMMAND_END(mempool);

static void show_usage(const char *cmd)
{
    printf("usage:\n");
    printf("\t%s init <address> <size> <type>\n", cmd);
    printf("\t%s info\n", cmd);
    printf("\t%s alloc <size> <type>\n", cmd);
    printf("\t%s free <address>\n", cmd);
}

static void mempool_dump(void)
{
    int i;
    struct mem_chunk *chunk;

    for (i = 0; i < MAX_MEMPOOL_TYPE; i++) {
        printf("dump mempool type %d\n", i);
        list_for_every_entry(&pool[i].chunk_list, chunk,
                             struct mem_chunk, node) {
            printf("start %p, len %zx, free %d, type %d\n",
                   chunk->start, chunk->len, chunk->free, i);
        }
    }
}

static int cmd_mempool(int argc, const cmd_args *argv)
{
    int ret;
    void *p;

    if (argc < 2) {
notenoughargs:
        printf("not enough arguments\n");
usage:
        show_usage(argv[0].str);
        return -1;
    }

    if (strcmp(argv[1].str, "init") == 0) {
        if (argc < 5)
            goto notenoughargs;

        ret = mempool_init((void *)argv[2].u, argv[3].u, argv[4].i);
        if (ret != NO_ERROR)
            printf("mempool_init failed, ret %d\n", ret);
    } else if (strcmp(argv[1].str, "info") == 0) {
        mempool_dump();
    } else if (strcmp(argv[1].str, "alloc") == 0) {
        if (argc < 4)
            goto notenoughargs;

        p = mempool_alloc(argv[2].u, argv[3].i);
        if (!p) {
            printf("mempool alloc failed, size %lu, type %ld\n",
                   argv[2].u, argv[3].i);
            mempool_dump();
        }
    } else if (strcmp(argv[1].str, "free") == 0) {
        if (argc < 3)
            goto notenoughargs;

        mempool_free((void *)(uintptr_t)argv[2].u);
    } else {
        printf("unrecognized command\n");
        goto usage;
    }

    return 0;
}
#endif