libutils/isoc/bget_malloc.c

/*
 * Copyright (c) 2014, STMicroelectronics International N.V.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#define PROTOTYPES

/*
 *  BGET CONFIGURATION
 *  ==================
 */
/* #define BGET_ENABLE_ALL_OPTIONS */
#ifdef BGET_ENABLE_OPTION
#define TestProg    20000	/* Generate built-in test program
				   if defined.  The value specifies
				   how many buffer allocation attempts
				   the test program should make. */
#endif


#define SizeQuant   8		/* Buffer allocation size quantum:
				   all buffers allocated are a
				   multiple of this size.  This
				   MUST be a power of two. */

#ifdef BGET_ENABLE_OPTION
#define BufDump     1		/* Define this symbol to enable the
				   bpoold() function which dumps the
				   buffers in a buffer pool. */

#define BufValid    1		/* Define this symbol to enable the
				   bpoolv() function for validating
				   a buffer pool. */

#define DumpData    1		/* Define this symbol to enable the
				   bufdump() function which allows
				   dumping the contents of an allocated
				   or free buffer. */

#define BufStats    1		/* Define this symbol to enable the
				   bstats() function which calculates
				   the total free space in the buffer
				   pool, the largest available
				   buffer, and the total space
				   currently allocated. */

#define FreeWipe    1		/* Wipe free buffers to a guaranteed
				   pattern of garbage to trip up
				   miscreants who attempt to use
				   pointers into released buffers. */

#define BestFit     1		/* Use a best fit algorithm when
				   searching for space for an
				   allocation request.  This uses
				   memory more efficiently, but
				   allocation will be much slower. */

#define BECtl       1		/* Define this symbol to enable the
				   bectl() function for automatic
				   pool space control.  */
#endif

#ifdef MEM_DEBUG
#undef NDEBUG
#define DumpData    1
#define BufValid    1
#define FreeWipe    1
#endif

#if defined(CFG_TEE_CORE_DEBUG) && CFG_TEE_CORE_DEBUG != 0
#define BufStats    1
#endif

#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <malloc.h>
#include "bget.c"		/* this is ugly, but this is bget */
#include <util.h>

#ifdef __KERNEL__
/* Compiling for TEE Core */
#include <kernel/mutex.h>

static struct mutex malloc_mu = MUTEX_INITIALIZER;

static void malloc_lock(void)
{
	mutex_lock(&malloc_mu);
}

static void malloc_unlock(void)
{
	mutex_unlock(&malloc_mu);
}

#else /*__KERNEL__*/
/* Compiling for TA */
static void malloc_lock(void)
{
}

static void malloc_unlock(void)
{
}
#endif /*__KERNEL__*/

struct malloc_pool {
	void *buf;
	size_t len;
};

static struct malloc_pool *malloc_pool;
static size_t malloc_pool_len;

#ifdef BufStats
static size_t max_alloc_heap;

static void raw_malloc_save_max_alloced_size(void)
{
	if (totalloc > max_alloc_heap)
		max_alloc_heap = totalloc;
}

void malloc_reset_max_allocated(void)
{
	malloc_lock();
	max_alloc_heap = 0;
	malloc_unlock();
}

size_t malloc_get_max_allocated(void)
{
	size_t r;

	malloc_lock();
	r = max_alloc_heap;
	malloc_unlock();
	return r;
}

size_t malloc_get_allocated(void)
{
	size_t r;

	malloc_lock();
	r = totalloc;
	malloc_unlock();
	return r;
}

#else /* BufStats */

static void raw_malloc_save_max_alloced_size(void)
{
}

void malloc_reset_max_allocated(void)
{
}

size_t malloc_get_max_allocated(void)
{
	return 0;
}

size_t malloc_get_allocated(void)
{
	return 0;
}
#endif /* BufStats */

size_t malloc_get_heap_size(void)
{
	size_t n;
	size_t s = 0;

	malloc_lock();

	for (n = 0; n < malloc_pool_len; n++)
		s += malloc_pool[n].len;

	malloc_unlock();

	return s;
}

#ifdef BufValid
static void raw_malloc_validate_pools(void)
{
	size_t n;

	for (n = 0; n < malloc_pool_len; n++)
		bpoolv(malloc_pool[n].buf);
}
#else
static void raw_malloc_validate_pools(void)
{
}
#endif

struct bpool_iterator {
	struct bfhead *next_buf;
	size_t pool_idx;
};

static void bpool_foreach_iterator_init(struct bpool_iterator *iterator)
{
	iterator->pool_idx = 0;
	iterator->next_buf = BFH(malloc_pool[0].buf);
}

static bool bpool_foreach_pool(struct bpool_iterator *iterator, void **buf,
		size_t *len, bool *isfree)
{
	struct bfhead *b = iterator->next_buf;
	bufsize bs = b->bh.bsize;

	if (bs == ESent)
		return false;

	if (bs < 0) {
		/* Allocated buffer */
		bs = -bs;

		*isfree = false;
	} else {
		/* Free Buffer */
		*isfree = true;

		/* Assert that the free list links are intact */
		assert(b->ql.blink->ql.flink == b);
		assert(b->ql.flink->ql.blink == b);
	}

	*buf = (uint8_t *)b + sizeof(struct bhead);
	*len = bs - sizeof(struct bhead);

	iterator->next_buf = BFH((uint8_t *)b + bs);
	return true;
}

static bool bpool_foreach(struct bpool_iterator *iterator, void **buf)
{
	while (true) {
		size_t len;
		bool isfree;

		if (bpool_foreach_pool(iterator, buf, &len, &isfree)) {
			if (isfree)
				continue;
			return true;
		}

		if ((iterator->pool_idx + 1) >= malloc_pool_len)
			return false;

		iterator->pool_idx++;
		iterator->next_buf = BFH(malloc_pool[iterator->pool_idx].buf);
	}
}

/* Convenience macro for looping over all allocated buffers */
#define BPOOL_FOREACH(iterator, bp) \
		for (bpool_foreach_iterator_init((iterator)); \
			bpool_foreach((iterator), (bp));)

static void *raw_malloc(size_t hdr_size, size_t ftr_size, size_t pl_size)
{
	void *ptr;
	size_t s = hdr_size + ftr_size + pl_size;

	malloc_lock();

	/*
	 * Make sure that malloc has correct alignment of returned buffers.
	 * The assumption is that uintptr_t will be as wide as the largest
	 * required alignment of any type.
	 */
	COMPILE_TIME_ASSERT(SizeQuant >= sizeof(uintptr_t));

	raw_malloc_validate_pools();

	/* Check wrapping */
	if (s < pl_size)
		return NULL;

	/* BGET doesn't like 0 sized allocations */
	if (!s)
		s++;

	ptr = bget(s);
	raw_malloc_save_max_alloced_size();

	malloc_unlock();
	return ptr;
}

static void raw_free(void *ptr)
{
	malloc_lock();

	raw_malloc_validate_pools();

	if (ptr)
		brel(ptr);

	malloc_unlock();
}

static void *raw_calloc(size_t hdr_size, size_t ftr_size, size_t pl_nmemb,
		size_t pl_size)
{
	size_t s = hdr_size + ftr_size + pl_nmemb * pl_size;
	void *ptr;

	malloc_lock();

	raw_malloc_validate_pools();

	/* Check wrapping */
	if (s < pl_nmemb || s < pl_size)
		return NULL;

	/* BGET doesn't like 0 sized allocations */
	if (!s)
		s++;

	ptr = bgetz(s);
	raw_malloc_save_max_alloced_size();

	malloc_unlock();

	return ptr;
}

static void *raw_realloc(void *ptr, size_t hdr_size, size_t ftr_size,
		size_t pl_size)
{
	size_t s = hdr_size + ftr_size + pl_size;
	void *p;

	/* Check wrapping */
	if (s < pl_size)
		return NULL;

	malloc_lock();

	raw_malloc_validate_pools();

	/* BGET doesn't like 0 sized allocations */
	if (!s)
		s++;

	p = bgetr(ptr, s);
	raw_malloc_save_max_alloced_size();

	malloc_unlock();

	return p;
}

static void create_free_block(struct bfhead *bf, bufsize size, struct bhead *bn)
{
	assert(BH((char *)bf + size) == bn);
	assert(bn->bsize < 0); /* Next block should be allocated */
	/* Next block shouldn't already have free block in front */
	assert(bn->prevfree == 0);

	/* Create the free buf header */
	bf->bh.bsize = size;
	bf->bh.prevfree = 0;

	/* Update next block to point to the new free buf header */
	bn->prevfree = size;

	/* Insert the free buffer on the free list */
	assert(freelist.ql.blink->ql.flink == &freelist);
	assert(freelist.ql.flink->ql.blink == &freelist);
	bf->ql.flink = &freelist;
	bf->ql.blink = freelist.ql.blink;
	freelist.ql.blink = bf;
	bf->ql.blink->ql.flink = bf;
}

static void brel_before(char *orig_buf, char *new_buf)
{
	struct bfhead *bf;
	struct bhead *b;
	bufsize size;
	bufsize orig_size;

	assert(orig_buf < new_buf);
	/* There has to be room for the freebuf header */
	size = (bufsize)(new_buf - orig_buf);
	assert(size >= (SizeQ + sizeof(struct bhead)));

	/* Point to head of original buffer */
	bf = BFH(orig_buf - sizeof(struct bhead));
	orig_size = -bf->bh.bsize; /* negative since it's an allocated buffer */

	/* Point to head of the becoming new allocated buffer */
	b = BH(new_buf - sizeof(struct bhead));

	if (bf->bh.prevfree != 0) {
		/* Previous buffer is free, consolidate with that buffer */
		struct bfhead *bfp;

		/* Update the previous free buffer */
		bfp = BFH((char *)bf - bf->bh.prevfree);
		assert(bfp->bh.bsize == bf->bh.prevfree);
		bfp->bh.bsize += size;

		/* Make a new allocated buffer header */
		b->prevfree = bfp->bh.bsize;
		/* Make it negative since it's an allocated buffer */
		b->bsize = -(orig_size - size);
	} else {
		/*
		 * Previous buffer is allocated, create a new buffer and
		 * insert on the free list.
		 */

		/* Make it negative since it's an allocated buffer */
		b->bsize = -(orig_size - size);

		create_free_block(bf, size, b);
	}

#ifdef BufStats
	totalloc -= size;
	assert(totalloc >= 0);
#endif
}

static void brel_after(char *buf, bufsize size)
{
	struct bhead *b = BH(buf - sizeof(struct bhead));
	struct bhead *bn;
	bufsize new_size = size;
	bufsize free_size;

	/* Select the size in the same way as in bget() */
	if (new_size < SizeQ)
		new_size = SizeQ;
#ifdef SizeQuant
#if SizeQuant > 1
	new_size = (new_size + (SizeQuant - 1)) & (~(SizeQuant - 1));
#endif
#endif
	new_size += sizeof(struct bhead);
	assert(new_size <= -b->bsize);

	/*
	 * Check if there's enough space at the end of the buffer to be
	 * able to free anything.
	 */
	free_size = -b->bsize - new_size;
	if (free_size < SizeQ + sizeof(struct bhead))
		return;

	bn = BH((char *)b - b->bsize);
	/*
	 * Set the new size of the buffer;
	 */
	b->bsize = -new_size;
	if (bn->bsize > 0) {
		/* Next buffer is free, consolidate with that buffer */
		struct bfhead *bfn = BFH(bn);
		struct bfhead *nbf = BFH((char *)b + new_size);
		struct bhead *bnn = BH((char *)bn + bn->bsize);

		assert(bfn->bh.prevfree == 0);
		assert(bnn->prevfree == bfn->bh.bsize);

		/* Construct the new free header */
		nbf->bh.prevfree = 0;
		nbf->bh.bsize = bfn->bh.bsize + free_size;

		/* Update the buffer after this to point to this header */
		bnn->prevfree += free_size;

		/*
		 * Unlink the previous free buffer and link the new free
		 * buffer.
		 */
		assert(bfn->ql.blink->ql.flink == bfn);
		assert(bfn->ql.flink->ql.blink == bfn);

		/* Assing blink and flink from old free buffer */
		nbf->ql.blink = bfn->ql.blink;
		nbf->ql.flink = bfn->ql.flink;

		/* Replace the old free buffer with the new one */
		nbf->ql.blink->ql.flink = nbf;
		nbf->ql.flink->ql.blink = nbf;
	} else {
		/* New buffer is allocated, create a new free buffer */
		create_free_block(BFH((char *)b + new_size), free_size, bn);
	}

#ifdef BufStats
	totalloc -= free_size;
	assert(totalloc >= 0);
#endif

}

static void *raw_memalign(size_t hdr_size, size_t ftr_size, size_t alignment,
		size_t size)
{
	size_t s;
	uintptr_t b;

	malloc_lock();

	raw_malloc_validate_pools();

	if (!IS_POWER_OF_TWO(alignment))
		return NULL;

	/*
	 * Normal malloc with headers always returns something SizeQuant
	 * aligned.
	 */
	if (alignment <= SizeQuant)
		return raw_malloc(hdr_size, ftr_size, size);

	s = hdr_size + ftr_size + alignment + size +
	    SizeQ + sizeof(struct bhead);

	/* Check wapping */
	if (s < alignment || s < size)
		return NULL;

	b = (uintptr_t)bget(s);
	if (!b)
		return NULL;

	if ((b + hdr_size) & (alignment - 1)) {
		/*
		 * Returned buffer is not aligned as requested if the
		 * hdr_size is added. Find an offset into the buffer
		 * that is far enough in to the buffer to be able to free
		 * what's in front.
		 */
		uintptr_t p;

		/*
		 * Find the point where the buffer including supplied
		 * header size should start.
		 */
		p = b + hdr_size + alignment;
		p &= ~(alignment - 1);
		p -= hdr_size;
		if ((p - b) < (SizeQ + sizeof(struct bhead)))
			p += alignment;
		assert((p + hdr_size + ftr_size + size) <= (b + s));

		/* Free the front part of the buffer */
		brel_before((void *)b, (void *)p);

		/* Set the new start of the buffer */
		b = p;
	}

	/*
	 * Since b is now aligned, release what we don't need at the end of
	 * the buffer.
	 */
	brel_after((void *)b, hdr_size + ftr_size + size);

	raw_malloc_save_max_alloced_size();

	malloc_unlock();

	return (void *)b;
}

/* Most of the stuff in this function is copied from bgetr() in bget.c */
static bufsize bget_buf_size(void *buf)
{
	bufsize osize;          /* Old size of buffer */
	struct bhead *b;

	b = BH(((char *)buf) - sizeof(struct bhead));
	osize = -b->bsize;
#ifdef BECtl
	if (osize == 0) {
		/*  Buffer acquired directly through acqfcn. */
		struct bdhead *bd;

		bd = BDH(((char *)buf) - sizeof(struct bdhead));
		osize = bd->tsize - sizeof(struct bdhead);
	} else
#endif
		osize -= sizeof(struct bhead);
	assert(osize > 0);
	return osize;
}

#ifdef ENABLE_MDBG

struct mdbg_hdr {
	const char *fname;
	uint16_t line;
	bool ignore;
	uint32_t pl_size;
	uint32_t magic;
};

#define MDBG_HEADER_MAGIC	0xadadadad
#define MDBG_FOOTER_MAGIC	0xecececec

/* TODO make this a per thread variable */
static enum mdbg_mode mdbg_mode = MDBG_MODE_DYNAMIC;

static size_t mdbg_get_ftr_size(size_t pl_size)
{
	size_t ftr_pad = ROUNDUP(pl_size, sizeof(uint32_t)) - pl_size;

	return ftr_pad + sizeof(uint32_t);
}


static uint32_t *mdbg_get_footer(struct mdbg_hdr *hdr)
{
	uint32_t *footer;

	footer = (uint32_t *)((uint8_t *)(hdr + 1) + hdr->pl_size +
			      mdbg_get_ftr_size(hdr->pl_size));
	footer--;
	return footer;
}

static void mdbg_update_hdr(struct mdbg_hdr *hdr, const char *fname,
		int lineno, size_t pl_size)
{
	uint32_t *footer;

	hdr->fname = fname;
	hdr->line = lineno;
	hdr->pl_size = pl_size;
	hdr->magic = MDBG_HEADER_MAGIC;
	hdr->ignore = mdbg_mode == MDBG_MODE_STATIC;

	footer = mdbg_get_footer(hdr);
	*footer = MDBG_FOOTER_MAGIC;
}

void *mdbg_malloc(const char *fname, int lineno, size_t size)
{
	struct mdbg_hdr *hdr;

	COMPILE_TIME_ASSERT(sizeof(struct mdbg_hdr) == sizeof(uint32_t) * 4);

	hdr = raw_malloc(sizeof(struct mdbg_hdr),
			  mdbg_get_ftr_size(size), size);
	if (hdr) {
		mdbg_update_hdr(hdr, fname, lineno, size);
		hdr++;
	}
	return hdr;
}

static void assert_header(struct mdbg_hdr *hdr)
{
	assert(hdr->magic == MDBG_HEADER_MAGIC);
	assert(*mdbg_get_footer(hdr) == MDBG_FOOTER_MAGIC);
}

void mdbg_free(void *ptr)
{
	struct mdbg_hdr *hdr = ptr;

	if (hdr) {
		hdr--;
		assert_header(hdr);
		hdr->magic = 0;
		*mdbg_get_footer(hdr) = 0;
		raw_free(hdr);
	}
}

void *mdbg_calloc(const char *fname, int lineno, size_t nmemb, size_t size)
{
	struct mdbg_hdr *hdr;

	hdr = raw_calloc(sizeof(struct mdbg_hdr),
			  mdbg_get_ftr_size(nmemb * size), nmemb, size);
	if (hdr) {
		mdbg_update_hdr(hdr, fname, lineno, nmemb * size);
		hdr++;
	}
	return hdr;
}

void *mdbg_realloc(const char *fname, int lineno, void *ptr, size_t size)
{
	struct mdbg_hdr *hdr = ptr;

	if (hdr) {
		hdr--;
		assert_header(hdr);
	}
	hdr = raw_realloc(hdr, sizeof(struct mdbg_hdr),
			   mdbg_get_ftr_size(size), size);
	if (hdr) {
		mdbg_update_hdr(hdr, fname, lineno, size);
		hdr++;
	}
	return hdr;
}

void *mdbg_memalign(const char *fname, int lineno, size_t alignment,
		size_t size)
{
	struct mdbg_hdr *hdr;

	hdr = raw_memalign(sizeof(struct mdbg_hdr), mdbg_get_ftr_size(size),
			   alignment, size);
	if (hdr) {
		mdbg_update_hdr(hdr, fname, lineno, size);
		hdr++;
	}
	return hdr;
}


static void *get_payload_start_size(void *raw_buf, size_t *size)
{
	struct mdbg_hdr *hdr = raw_buf;

	assert(bget_buf_size(hdr) >= hdr->pl_size);
	*size = hdr->pl_size;
	return hdr + 1;
}

void mdbg_check(int bufdump)
{
	struct bpool_iterator itr;
	void *b;

	raw_malloc_validate_pools();

	BPOOL_FOREACH(&itr, &b) {
		struct mdbg_hdr *hdr = (struct mdbg_hdr *)b;

		assert_header(hdr);

		if (bufdump > 0 || !hdr->ignore) {
			const char *fname = hdr->fname;

			if (!fname)
				fname = "unknown";

			DMSG("%s buffer: %d bytes %s:%d\n",
				hdr->ignore ? "Ignore" : "Orphaned",
				hdr->pl_size, fname, hdr->line);
		}
	}

}

enum mdbg_mode mdbg_set_mode(enum mdbg_mode mode)
{
	enum mdbg_mode old_mode = mdbg_mode;

	mdbg_mode = mode;
	return old_mode;
}

#else

void *malloc(size_t size)
{
	return raw_malloc(0, 0, size);
}

void free(void *ptr)
{
	raw_free(ptr);
}

void *calloc(size_t nmemb, size_t size)
{
	return raw_calloc(0, 0, nmemb, size);
}

void *realloc(void *ptr, size_t size)
{
	return raw_realloc(ptr, 0, 0, size);
}

void *memalign(size_t alignment, size_t size)
{
	return raw_memalign(0, 0, alignment, size);
}

static void *get_payload_start_size(void *ptr, size_t *size)
{
	*size = bget_buf_size(ptr);
	return ptr;
}

#endif


void malloc_init(void *buf, size_t len)
{
	/* Must not be called twice */
	assert(!malloc_pool);

	malloc_add_pool(buf, len);
}

void malloc_add_pool(void *buf, size_t len)
{
	void *p;
	size_t l;
	uintptr_t start = (uintptr_t)buf;
	uintptr_t end = start + len;
	enum mdbg_mode old_mode = mdbg_set_mode(MDBG_MODE_STATIC);

	start = ROUNDUP(start, SizeQuant);
	end = ROUNDDOWN(end, SizeQuant);
	assert(start < end);

	bpool((void *)start, end - start);

	l = malloc_pool_len + 1;
	p = realloc(malloc_pool, sizeof(struct malloc_pool) * l);
	assert(p);
	malloc_pool = p;
	malloc_pool[malloc_pool_len].buf = (void *)start;
	malloc_pool[malloc_pool_len].len = end - start;
	malloc_pool_len = l;
	mdbg_set_mode(old_mode);
}

bool malloc_buffer_is_within_alloced(void *buf, size_t len)
{
	struct bpool_iterator itr;
	void *b;
	uint8_t *start_buf = buf;
	uint8_t *end_buf = start_buf + len;
	bool ret = false;

	malloc_lock();

	raw_malloc_validate_pools();

	/* Check for wrapping */
	if (start_buf > end_buf)
		goto out;

	BPOOL_FOREACH(&itr, &b) {
		uint8_t *start_b;
		uint8_t *end_b;
		size_t s;

		start_b = get_payload_start_size(b, &s);
		end_b = start_b + s;

		if (start_buf >= start_b && end_buf <= end_b) {
			ret = true;
			goto out;
		}
	}

out:
	malloc_unlock();

	return ret;
}

bool malloc_buffer_overlaps_heap(void *buf, size_t len)
{
	uintptr_t buf_start = (uintptr_t) buf;
	uintptr_t buf_end = buf_start + len;
	size_t n;
	bool ret = false;

	malloc_lock();

	raw_malloc_validate_pools();

	for (n = 0; n < malloc_pool_len; n++) {
		uintptr_t pool_start = (uintptr_t)malloc_pool[n].buf;
		uintptr_t pool_end = pool_start + malloc_pool[n].len;

		if (buf_start > buf_end || pool_start > pool_end) {
			ret = true;	/* Wrapping buffers, shouldn't happen */
			goto out;
		}

		if (buf_end > pool_start || buf_start < pool_end) {
			ret = true;
			goto out;
		}
	}

out:
	malloc_unlock();
	return ret;
}