blob: 6f2166246e7ccc6c6281483614974f26ab98250c [file] [log] [blame]
/*
* Copyright (c) 2016 Nordic Semiconductor ASA
* Copyright (c) 2016 Vinayak Kariappa Chettimada
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/types.h>
#include <string.h>
#include "util.h"
#include "mem.h"
void mem_init(void *mem_pool, u16_t mem_size, u16_t mem_count,
void **mem_head)
{
*mem_head = mem_pool;
/* Store free mem_count after the list's next pointer at an aligned
* memory location to ensure atomic read/write (in ARM for now).
*/
*((u16_t *)MROUND((u8_t *)mem_pool + sizeof(mem_pool))) = mem_count;
/* Initialize next pointers to form a free list,
* next pointer is stored in the first 32-bit of each block
*/
memset(((u8_t *)mem_pool + (mem_size * (--mem_count))), 0,
sizeof(mem_pool));
while (mem_count--) {
u32_t next;
next = (u32_t)((u8_t *) mem_pool +
(mem_size * (mem_count + 1)));
memcpy(((u8_t *)mem_pool + (mem_size * mem_count)),
(void *)&next, sizeof(next));
}
}
void *mem_acquire(void **mem_head)
{
if (*mem_head) {
u16_t free_count;
void *head;
void *mem;
/* Get the free count from the list and decrement it */
free_count = *((u16_t *)MROUND((u8_t *)*mem_head +
sizeof(mem_head)));
free_count--;
mem = *mem_head;
memcpy(&head, mem, sizeof(head));
/* Store free mem_count after the list's next pointer */
if (head) {
*((u16_t *)MROUND((u8_t *)head + sizeof(head))) =
free_count;
}
*mem_head = head;
return mem;
}
return NULL;
}
void mem_release(void *mem, void **mem_head)
{
u16_t free_count = 0;
/* Get the free count from the list and increment it */
if (*mem_head) {
free_count = *((u16_t *)MROUND((u8_t *)*mem_head +
sizeof(mem_head)));
}
free_count++;
memcpy(mem, mem_head, sizeof(mem));
/* Store free mem_count after the list's next pointer */
*((u16_t *)MROUND((u8_t *)mem + sizeof(mem))) = free_count;
*mem_head = mem;
}
u16_t mem_free_count_get(void *mem_head)
{
u16_t free_count = 0;
/* Get the free count from the list */
if (mem_head) {
free_count = *((u16_t *)MROUND((u8_t *)mem_head +
sizeof(mem_head)));
}
return free_count;
}
void *mem_get(void *mem_pool, u16_t mem_size, u16_t index)
{
return ((void *)((u8_t *)mem_pool + (mem_size * index)));
}
u16_t mem_index_get(void *mem, void *mem_pool, u16_t mem_size)
{
return ((u16_t)((u8_t *)mem - (u8_t *)mem_pool) / mem_size);
}
void mem_rcopy(u8_t *dst, u8_t const *src, u16_t len)
{
src += len;
while (len--) {
*dst++ = *--src;
}
}
u8_t mem_nz(u8_t *src, u16_t len)
{
while (len--) {
if (*src++) {
return 1;
}
}
return 0;
}
u32_t mem_ut(void)
{
#define BLOCK_SIZE MROUND(10)
#define BLOCK_COUNT 10
u8_t MALIGN(4) pool[BLOCK_COUNT][BLOCK_SIZE];
void *mem_free;
void *mem_used;
u16_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--) {
u16_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) {
u16_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;
}