zephyr/subsys/bluetooth/controller/util/mem.c

/*
 * Copyright (c) 2016 Nordic Semiconductor ASA
 * Copyright (c) 2016 Vinayak Kariappa Chettimada
 *
 * Licensed 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.
 */

#include <stdint.h>
#include <string.h>

#include "defines.h"

#include "mem.h"

void mem_init(void *mem_pool, uint16_t mem_size, uint16_t mem_count,
	      void **mem_head)
{
	*mem_head = mem_pool;

	/* Store free mem_count after the list's next pointer */
	memcpy(((uint8_t *)mem_pool + sizeof(mem_pool)),
		 (uint8_t *)&mem_count, sizeof(mem_count));

	/* Initialize next pointers to form a free list,
	 * next pointer is stored in the first 32-bit of each block
	 */
	memset(((uint8_t *)mem_pool + (mem_size * (--mem_count))), 0,
		sizeof(mem_pool));
	while (mem_count--) {
		uint32_t next;

		next = (uint32_t)((uint8_t *) mem_pool +
				(mem_size * (mem_count + 1)));
		memcpy(((uint8_t *)mem_pool + (mem_size * mem_count)),
			 (void *)&next, sizeof(next));
	}
}

void *mem_acquire(void **mem_head)
{
	if (*mem_head) {
		uint16_t free_count;
		void *mem;

		/* Get the free count from the list and decrement it */
		memcpy((void *)&free_count,
			 ((uint8_t *)*mem_head + sizeof(mem_head)),
			 sizeof(free_count));
		free_count--;

		mem = *mem_head;
		memcpy(mem_head, mem, sizeof(*mem_head));

		/* Store free mem_count after the list's next pointer */
		if (*mem_head) {
			memcpy(((uint8_t *)*mem_head + sizeof(mem_head)),
				 (uint8_t *)&free_count, sizeof(free_count));
		}

		return mem;
	}

	return NULL;
}

void mem_release(void *mem, void **mem_head)
{
	uint16_t free_count = 0;

	/* Get the free count from the list and increment it */
	if (*mem_head) {
		memcpy(&free_count, ((uint8_t *)*mem_head + sizeof(mem_head)),
		       sizeof(free_count));
	}
	free_count++;

	memcpy(mem, mem_head, sizeof(mem));
	*mem_head = mem;

	/* Store free mem_count after the list's next pointer */
	memcpy(((uint8_t *)*mem_head + sizeof(mem_head)),
		 (uint8_t *)&free_count, sizeof(free_count));
}

uint16_t mem_free_count_get(void *mem_head)
{
	uint16_t free_count = 0;

	/* Get the free count from the list */
	if (mem_head) {
		memcpy(&free_count, ((uint8_t *)mem_head + sizeof(mem_head)),
		       sizeof(free_count));
	}

	return free_count;
}

void *mem_get(void *mem_pool, uint16_t mem_size, uint16_t index)
{
	return ((void *)((uint8_t *)mem_pool + (mem_size * index)));
}

uint16_t mem_index_get(void *mem, void *mem_pool, uint16_t mem_size)
{
	return ((uint16_t)((uint8_t *)mem - (uint8_t *)mem_pool) /
		mem_size);
}

void mem_rcopy(uint8_t *dst, uint8_t const *src, uint16_t len)
{
	src += len;
	while (len--) {
		*dst++ = *--src;
	}
}

uint8_t mem_is_zero(uint8_t *src, uint16_t len)
{
	while (len--) {
		if (*src++) {
			return 1;
		}
	}

	return 0;
}

uint32_t mem_ut(void)
{
#define BLOCK_SIZE  ALIGN4(10)
#define BLOCK_COUNT 10
	uint8_t ALIGNED(4) pool[BLOCK_COUNT][BLOCK_SIZE];
	void *mem_free;
	void *mem_used;
	uint16_t mem_free_count;
	void *mem;

	mem_init(pool, BLOCK_SIZE, BLOCK_COUNT, &mem_free);

	mem_free_count = mem_free_count_get(mem_free);
	if (mem_free_count != BLOCK_COUNT) {
		return 1;
	}

	mem_used = 0;
	while (mem_free_count--) {
		uint16_t mem_free_count_current;

		mem = mem_acquire(&mem_free);
		mem_free_count_current = mem_free_count_get(mem_free);
		if (mem_free_count != mem_free_count_current) {
			return 2;
		}

		memcpy(mem, &mem_used, sizeof(mem));
		mem_used = mem;
	}

	mem = mem_acquire(&mem_free);
	if (mem) {
		return 3;
	}

	while (++mem_free_count < BLOCK_COUNT) {
		uint16_t mem_free_count_current;

		mem = mem_used;
		memcpy(&mem_used, mem, sizeof(void *));
		mem_release(mem, &mem_free);

		mem_free_count_current = mem_free_count_get(mem_free);
		if ((mem_free_count + 1) != mem_free_count_current) {
			return 4;
		}
	}

	if (mem != mem_free) {
		return 5;
	}

	if (mem_free_count_get(mem_free) != BLOCK_COUNT) {
		return 6;
	}

	return 0;
}