/***************************************************************************
 *   sam7fc - dynamic Memory Allocation                                    *
 *                                                                         *
 *   Copyright (C) 02/2008 by Olaf Rempel                                  *
 *   razzor@kopf-tisch.de                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; version 2 of the License                *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <stdint.h>
#include <stdio.h>
#include "AT91SAM7S256.h"
#include "board.h"
#include "memalloc.h"

// TODO: make generic version and find better place
#define ALIGN(x) (((x) +3) & ~3)

/* extern symbols, defined in ldscript */
extern void * __bss_end__;
extern void * __stack_top__;

static uint32_t static_alloc_pos;

void * static_alloc(uint32_t size)
{
	if (static_alloc_pos == 0)
		static_alloc_pos = ALIGN((uint32_t)&__bss_end__);

	uint32_t retval = static_alloc_pos;

	size = ALIGN(size);
	static_alloc_pos += size;

	return (void *)retval;
}

uint32_t static_alloc_used(void)
{
	return static_alloc_pos - (uint32_t)&__bss_end__;
}

// TODO: find a better place
uint32_t next_powerof2(uint32_t value)
{
	uint32_t i;

	value--;
        for (i = 1; i < 32; i = i * 2)
                value |= value >> i;

	return value +1;
}

/*
 * (pos->next) - (pos) = size_of_the_chunk
 * we use the lower 2 bits of the pointers to store some information.
 * (since they point to 4byte aligned structs == are always zero)
 */
#define FLAG_CHUNKINUSE		0x01
#define FLAG_ENDOFBLOCK		0x02
#define FLAG_MASK		0x03


#define SET_FLAGS(p, f)	((typeof (p))((uint32_t)(p) | (f)))
#define GET_FLAGS(p)	((uint32_t)((p)->next) & FLAG_MASK)
#define GET_NEXT(p)	((typeof (p))((uint32_t)((p)->next) & ~(FLAG_MASK)))

struct memchunk {
	struct memchunk *next;
	uint8_t data[0];
};

static struct memchunk first_memchunk;
static struct memchunk *last_endblock;

/* add a new block to the dynamic memory allocator */
static struct memchunk * alloc_add(uint32_t size)
{
	size = ALIGN(size);

	printf("alloc_add(%ld)\n\r", size);

	/* struct at begin of memory block */
	struct memchunk *newblock = static_alloc(size);

	/* struct at end of memory block */
	struct memchunk *endblock = (void *)((uint32_t)newblock + size - sizeof(struct memchunk));

	/* link between them */
	newblock->next = endblock;

	/* next pointer of last block goes to NULL */
	endblock->next = SET_FLAGS(NULL, FLAG_ENDOFBLOCK);

	/* first block add? */
	if (first_memchunk.next == NULL) {
		first_memchunk.next = newblock;
		last_endblock = endblock;

	} else {
		/* the old endblock points to the new block, but keeps its flag */
		last_endblock->next = SET_FLAGS(newblock, FLAG_ENDOFBLOCK);
		last_endblock = endblock;
	}

	return newblock;
}

void * alloc(uint32_t size)
{
	size = ALIGN(size) + sizeof(struct memchunk);

	/* found & search are always real pointers (without flags) */
	struct memchunk *found = NULL;
	struct memchunk *search = GET_NEXT(&first_memchunk);

	while (search && GET_NEXT(search)) {
		/* already used chunk OR a endofblock structure */
		if (GET_FLAGS(search) & (FLAG_CHUNKINUSE | FLAG_ENDOFBLOCK))
			goto try_next;

		uint32_t nextflags = GET_FLAGS(GET_NEXT(search)) & (FLAG_CHUNKINUSE | FLAG_ENDOFBLOCK);

		/* next chunk is unused AND an endofblock structure AND is not the last */
		if (nextflags == FLAG_ENDOFBLOCK && GET_NEXT(search) != last_endblock) {
			search->next = SET_FLAGS(GET_NEXT(GET_NEXT(search)), 0);
			continue;

		/* next chunk is unused -> chain them and then retry */
		} else if (nextflags == 0) {
			search->next = SET_FLAGS(GET_NEXT(GET_NEXT(search)), 0);
			continue;
		}

		uint32_t length = (uint32_t)GET_NEXT(search) - (uint32_t)search;
		/* perfect match */
		if (length == size) {
			found = search;
			break;

		/* chunk is bigger than needed, remember smallest */
		} else if (length >= size) {
			if (!found || (((uint32_t)GET_NEXT(found) - (uint32_t)found) > length))
				found = search;
		}

try_next:
		search = GET_NEXT(search);
	}

	/* nothing useful found, add a new block of memory */
	if (!found)
		found = alloc_add(MAX(1024, size));

	/* if we split the chunk, the remaining piece must be large enough */
	if (((uint32_t)GET_NEXT(found) - (uint32_t)found) > (size + 2 * sizeof(struct memchunk))) {
		struct memchunk *end = (void *)((uint32_t)found + size);
		end->next = SET_FLAGS(GET_NEXT(found), 0);
		found->next = SET_FLAGS(end, FLAG_CHUNKINUSE);

	} else {
		found->next = SET_FLAGS(GET_NEXT(found), FLAG_CHUNKINUSE);
	}

	return (void *)(found +1);
}

void free(void *p)
{
	/* mark the chunk as unused and keep the pointer */
	struct memchunk *chunk = (struct memchunk *)p -1;
	chunk->next = SET_FLAGS(GET_NEXT(chunk), 0);
}