From f2e8d126e56904a9e34d6cca7d46bb203f8f25f7 Mon Sep 17 00:00:00 2001
From: Olaf Rempel <razzor@kopf-tisch.de>
Date: Wed, 6 Feb 2008 02:15:22 +0100
Subject: [PATCH] dynamic memory allocation

---
 src/static_alloc.c | 120 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 120 insertions(+)

diff --git a/src/static_alloc.c b/src/static_alloc.c
index ef61fd4..afc5e1e 100644
--- a/src/static_alloc.c
+++ b/src/static_alloc.c
@@ -17,6 +17,7 @@
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
 #include <stdint.h>
+#include <stdio.h>
 #include "AT91SAM7S256.h"
 #include "board.h"
 #include "static_alloc.h"
@@ -58,3 +59,122 @@ uint32_t next_powerof2(uint32_t value)
 
 	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);
+
+	/* struct at begin of memory block */
+	struct memchunk *newblock = static_alloc(size);
+
+	/* struct at end of memory block */
+	struct memchunk *endblock = (void *)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 *)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);
+}