desklock/firmware/components/esp_hosted/common/mempool/mempool_ll.c

/*
 * SPDX-FileCopyrightText: 2015-2022 The Apache Software Foundation (ASF)
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * SPDX-FileContributor: 2019-2026 Espressif Systems (Shanghai) CO LTD
 */
/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */
/*
 * NOTICE: File has been changed from original implementation.
 */

#include <string.h>
#include <assert.h>
#include <stdbool.h>
#include "mempool_ll.h"
#include "freertos/portable.h"

#if CONFIG_ESP_HOSTED_USE_MEMPOOL

SemaphoreHandle_t hosted_port_mutex;
#define OS_INIT_CRITICAL() (hosted_port_mutex = xSemaphoreCreateMutex())
#define OS_ENTER_CRITICAL() (xSemaphoreTake((hosted_port_mutex), portMAX_DELAY))
#define OS_EXIT_CRITICAL() (xSemaphoreGive((hosted_port_mutex)))

#define OS_MEM_TRUE_BLOCK_SIZE(bsize)   OS_ALIGN(bsize, OS_ALIGNMENT)
#if MYNEWT_VAL(OS_MEMPOOL_GUARD)
#define OS_MEMPOOL_TRUE_BLOCK_SIZE(mp)                                  \
    (((mp)->mp_flags & OS_MEMPOOL_F_EXT) ?                              \
      OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size) :                       \
      (OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size) + sizeof(os_membuf_t)))
#else
#define OS_MEMPOOL_TRUE_BLOCK_SIZE(mp) OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)
#endif

static STAILQ_HEAD(, os_mempool) g_os_mempool_list =
    STAILQ_HEAD_INITIALIZER(g_os_mempool_list);

#if MYNEWT_VAL(OS_MEMPOOL_POISON)
static uint32_t os_mem_poison = 0xde7ec7ed;

static_assert(sizeof(struct os_memblock) % 4 == 0, "sizeof(struct os_memblock) shall be aligned to 4");
static_assert(sizeof(os_mem_poison) == 4, "sizeof(os_mem_poison) shall be 4");

static void
os_mempool_poison(const struct os_mempool *mp, void *start)
{
	uint32_t *p;
	uint32_t *end;
	int sz;

	sz = OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size);
	p = start;
	end = p + sz / 4;
	p += sizeof(struct os_memblock) / 4;

	while (p < end) {
		*p = os_mem_poison;
		p++;
	}
}

static void
os_mempool_poison_check(const struct os_mempool *mp, void *start)
{
	uint32_t *p;
	uint32_t *end;
	int sz;

	sz = OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size);
	p = start;
	end = p + sz / 4;
	p += sizeof(struct os_memblock) / 4;

	while (p < end) {
		assert(*p == os_mem_poison);
		p++;
	}
}
#else
#define os_mempool_poison(mp, start)
#define os_mempool_poison_check(mp, start)
#endif

#if MYNEWT_VAL(OS_MEMPOOL_GUARD)
#define OS_MEMPOOL_GUARD_PATTERN 0xBAFF1ED1

static void
os_mempool_guard(const struct os_mempool *mp, void *start)
{
	uint32_t *tgt;

	if ((mp->mp_flags & OS_MEMPOOL_F_EXT) == 0) {
		tgt = (uint32_t *)((uintptr_t)start +
				OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size));
		*tgt = OS_MEMPOOL_GUARD_PATTERN;
	}
}

static void
os_mempool_guard_check(const struct os_mempool *mp, void *start)
{
	uint32_t *tgt;

	if ((mp->mp_flags & OS_MEMPOOL_F_EXT) == 0) {
		tgt = (uint32_t *)((uintptr_t)start +
				OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size));
		assert(*tgt == OS_MEMPOOL_GUARD_PATTERN);
	}
}
#else
#define os_mempool_guard(mp, start)
#define os_mempool_guard_check(mp, start)
#endif

static os_error_t
os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks,
		uint32_t block_size, void *membuf, char *name,
		uint8_t flags)
{
	int true_block_size;
	int i;
	uint8_t *block_addr;
	struct os_memblock *block_ptr;

	/* Check for valid parameters */
	if (!mp || (block_size == 0)) {
		return OS_INVALID_PARM;
	}

	if ((!membuf) && (blocks != 0)) {
		return OS_INVALID_PARM;
	}
	OS_INIT_CRITICAL();

	if (membuf != NULL) {
		/* Blocks need to be sized properly and memory buffer should be
		 * aligned
		 */
		if (((uintptr_t)membuf & (OS_ALIGNMENT - 1)) != 0) {
			return OS_MEM_NOT_ALIGNED;
		}
	}

	/* Initialize the memory pool structure */
	mp->mp_block_size = block_size;
	mp->mp_num_free = blocks;
	mp->mp_min_free = blocks;
	mp->mp_flags = flags;
	mp->mp_num_blocks = blocks;
	mp->mp_membuf_addr = (uintptr_t)membuf;
	mp->name = name;
	SLIST_FIRST(mp) = membuf;

	if (blocks > 0) {
		os_mempool_poison(mp, membuf);
		os_mempool_guard(mp, membuf);
		true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp);

		/* Chain the memory blocks to the free list */
		block_addr = (uint8_t *)membuf;
		block_ptr = (struct os_memblock *)block_addr;
		for (i = 1; i < blocks; i++) {
			block_addr += true_block_size;
			os_mempool_poison(mp, block_addr);
			os_mempool_guard(mp, block_addr);
			SLIST_NEXT(block_ptr, mb_next) = (struct os_memblock *)block_addr;
			block_ptr = (struct os_memblock *)block_addr;
		}

		/* Last one in the list should be NULL */
		SLIST_NEXT(block_ptr, mb_next) = NULL;
	}

	STAILQ_INSERT_TAIL(&g_os_mempool_list, mp, mp_list);

	return OS_OK;
}

static os_error_t
os_mempool_init(struct os_mempool *mp, uint16_t blocks, uint32_t block_size,
		void *membuf, char *name)
{
	return os_mempool_init_internal(mp, blocks, block_size, membuf, name, 0);
}

#if 0
static os_error_t
os_mempool_ext_init(struct os_mempool_ext *mpe, uint16_t blocks,
		uint32_t block_size, void *membuf, char *name)
{
	int rc;

	rc = os_mempool_init_internal(&mpe->mpe_mp, blocks, block_size, membuf,
			name, OS_MEMPOOL_F_EXT);
	if (rc != 0) {
		return rc;
	}

	mpe->mpe_put_cb = NULL;
	mpe->mpe_put_arg = NULL;

	return 0;
}
#endif

static os_error_t
os_mempool_unregister(struct os_mempool *mp)
{
	struct os_mempool *prev;
	struct os_mempool *next;
	struct os_mempool *cur;

	/* Remove the mempool from the global stailq.  This is done manually rather
	 * than with `STAILQ_REMOVE` to allow for a graceful failure if the mempool
	 * isn't found.
	 */

	prev = NULL;
	STAILQ_FOREACH(cur, &g_os_mempool_list, mp_list) {
		if (cur == mp) {
			break;
		}
		prev = cur;
	}

	if (cur == NULL) {
		return OS_INVALID_PARM;
	}

	if (prev == NULL) {
		STAILQ_REMOVE_HEAD(&g_os_mempool_list, mp_list);
	} else {
		next = STAILQ_NEXT(cur, mp_list);
		if (next == NULL) {
			g_os_mempool_list.stqh_last = &STAILQ_NEXT(prev, mp_list);
		}

		STAILQ_NEXT(prev, mp_list) = next;
	}

	return 0;
}

#if 0
static os_error_t
os_mempool_clear(struct os_mempool *mp)
{
	struct os_memblock *block_ptr;
	int true_block_size;
	uint8_t *block_addr;
	uint16_t blocks;

	if (!mp) {
		return OS_INVALID_PARM;
	}

	true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp);

	/* cleanup the memory pool structure */
	mp->mp_num_free = mp->mp_num_blocks;
	mp->mp_min_free = mp->mp_num_blocks;
	os_mempool_poison(mp, (void *)mp->mp_membuf_addr);
	os_mempool_guard(mp, (void *)mp->mp_membuf_addr);
	SLIST_FIRST(mp) = (void *)mp->mp_membuf_addr;

	/* Chain the memory blocks to the free list */
	block_addr = (uint8_t *)mp->mp_membuf_addr;
	block_ptr = (struct os_memblock *)block_addr;
	blocks = mp->mp_num_blocks;

	while (blocks > 1) {
		block_addr += true_block_size;
		os_mempool_poison(mp, block_addr);
		os_mempool_guard(mp, block_addr);
		SLIST_NEXT(block_ptr, mb_next) = (struct os_memblock *)block_addr;
		block_ptr = (struct os_memblock *)block_addr;
		--blocks;
	}

	/* Last one in the list should be NULL */
	SLIST_NEXT(block_ptr, mb_next) = NULL;

	return OS_OK;
}

static int
os_memblock_from(const struct os_mempool *mp, const void *block_addr)
{
	uint32_t true_block_size;
	uintptr_t baddr;
	uintptr_t end;

	baddr = (uintptr_t)block_addr;
	true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp);
	end = mp->mp_membuf_addr + (mp->mp_num_blocks * true_block_size);

	/* Check that the block is in the memory buffer range. */
	if ((baddr < mp->mp_membuf_addr) || (baddr >= end)) {
		return 0;
	}

	/* All freed blocks should be on true block size boundaries! */
	if (((baddr - mp->mp_membuf_addr) % true_block_size) != 0) {
		return 0;
	}

	return 1;
}

static bool
os_mempool_is_sane(const struct os_mempool *mp)
{
	struct os_memblock *block;

	/* Verify that each block in the free list belongs to the mempool. */
	SLIST_FOREACH(block, mp, mb_next) {
		if (!os_memblock_from(mp, block)) {
			return false;
		}
		os_mempool_poison_check(mp, block);
		os_mempool_guard_check(mp, block);
	}

	return true;
}
#endif

static void *
os_memblock_get(struct os_mempool *mp)
{
	struct os_memblock *block;

	/* Check to make sure they passed in a memory pool (or something) */
	block = NULL;
	if (mp) {
		OS_ENTER_CRITICAL();
		/* Check for any free */
		if (mp->mp_num_free) {
			/* Get a free block */
			block = SLIST_FIRST(mp);

			/* Set new free list head */
			SLIST_FIRST(mp) = SLIST_NEXT(block, mb_next);

			/* Decrement number free by 1 */
			mp->mp_num_free--;
			if (mp->mp_min_free > mp->mp_num_free) {
				mp->mp_min_free = mp->mp_num_free;
			}
		}
		OS_EXIT_CRITICAL();

		if (block) {
			os_mempool_poison_check(mp, block);
			os_mempool_guard_check(mp, block);
		}
	}

	return (void *)block;
}

static os_error_t
os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr)
{
	struct os_memblock *block;

	os_mempool_guard_check(mp, block_addr);
	os_mempool_poison(mp, block_addr);

	block = (struct os_memblock *)block_addr;
	OS_ENTER_CRITICAL();

	/* Chain current free list pointer to this block; make this block head */
	SLIST_NEXT(block, mb_next) = SLIST_FIRST(mp);
	SLIST_FIRST(mp) = block;

	/* XXX: Should we check that the number free <= number blocks? */
	/* Increment number free */
	mp->mp_num_free++;

	OS_EXIT_CRITICAL();

	return OS_OK;
}

static os_error_t
os_memblock_put(struct os_mempool *mp, void *block_addr)
{
	struct os_mempool_ext *mpe;
	os_error_t ret;
#if MYNEWT_VAL(OS_MEMPOOL_CHECK)
	struct os_memblock *block;
	int sr;
#endif

	/* Make sure parameters are valid */
	if ((mp == NULL) || (block_addr == NULL)) {
		ret = OS_INVALID_PARM;
		goto done;
	}

#if MYNEWT_VAL(OS_MEMPOOL_CHECK)
	/* Check that the block we are freeing is a valid block! */
	assert(os_memblock_from(mp, block_addr));

	/*
	 * Check for duplicate free.
	 */
	OS_ENTER_CRITICAL();
	SLIST_FOREACH(block, mp, mb_next) {
		assert(block != (struct os_memblock *)block_addr);
	}
	OS_EXIT_CRITICAL();

#endif
	/* If this is an extended mempool with a put callback, call the callback
	 * instead of freeing the block directly.
	 */
	if (mp->mp_flags & OS_MEMPOOL_F_EXT) {
		mpe = (struct os_mempool_ext *)mp;
		if (mpe->mpe_put_cb != NULL) {
			ret = mpe->mpe_put_cb(mpe, block_addr, mpe->mpe_put_arg);
			goto done;
		}
	}

	/* No callback; free the block. */
	ret = os_memblock_put_from_cb(mp, block_addr);

done:
	return ret;
}

#if 0
static struct os_mempool *
os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi)
{
	struct os_mempool *cur;

	if (mp == NULL) {
		cur = STAILQ_FIRST(&g_os_mempool_list);
	} else {
		cur = STAILQ_NEXT(mp, mp_list);
	}

	if (cur == NULL) {
		return (NULL);
	}

	omi->omi_block_size = cur->mp_block_size;
	omi->omi_num_blocks = cur->mp_num_blocks;
	omi->omi_num_free = cur->mp_num_free;
	omi->omi_min_free = cur->mp_min_free;
	omi->omi_name[0] = '\0';
	strncat(omi->omi_name, cur->name, sizeof(omi->omi_name) - 1);

	return (cur);
}

static struct os_mempool *
os_mempool_get(const char *mempool_name, struct os_mempool_info *info)
{
	struct os_mempool *mp;

	mp = STAILQ_FIRST(&g_os_mempool_list);
	while (mp) {
		if (strcmp(mempool_name, mp->name) == 0) {
			break;
		}
		mp = STAILQ_NEXT(mp, mp_list);
	}

	if (mp != NULL && info != NULL) {
		info->omi_block_size = mp->mp_block_size;
		info->omi_num_blocks = mp->mp_num_blocks;
		info->omi_num_free = mp->mp_num_free;
		info->omi_min_free = mp->mp_min_free;
		info->omi_name[0] = '\0';
		strncat(info->omi_name, mp->name, sizeof(info->omi_name) - 1);
	}

	return mp;
}
#endif

static struct mempool_ops_t opts = {
	.mempool_init = os_mempool_init,
	.mempool_unregister = os_mempool_unregister,
	.memblock_get = os_memblock_get,
	.memblock_put = os_memblock_put,
};

struct mempool_ops_t * os_mempool_get_ops(void)
{
	return &opts;
}

#endif // CONFIG_ESP_HOSTED_USE_MEMPOOL