Merge TinyOS 2.1.1 into master.

[tinyos-2.x.git] / support / sdk / c / blip / lib6lowpan / ip_malloc.c

diff --git a/support/sdk/c/blip/lib6lowpan/ip_malloc.c b/support/sdk/c/blip/lib6lowpan/ip_malloc.c
new file mode 100644 (file)
index 0000000..9149efd
--- /dev/null
+++ b/support/sdk/c/blip/lib6lowpan/ip_malloc.c
@@ -0,0 +1,118 @@
+/*
+ * "Copyright (c) 2008, 2009 The Regents of the University  of California.
+ * All rights reserved."
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
+ * CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
+ *
+ */
+#ifndef NO_IP_MALLOC
+#include <stdint.h>
+#include <stdio.h>
+#include "ip_malloc.h"
+
+uint8_t heap[IP_MALLOC_HEAP_SIZE];
+
+void ip_malloc_init() {
+  bndrt_t *b = (bndrt_t *)heap;
+  *b = IP_MALLOC_HEAP_SIZE  & IP_MALLOC_LEN;
+}
+
+void *ip_malloc(uint16_t sz) {
+  bndrt_t *cur = (bndrt_t *)heap;
+  sz += sizeof(bndrt_t) * 2;
+  sz += (sz % IP_MALLOC_ALIGN);
+
+  while (((*cur & IP_MALLOC_LEN) < sz || (*cur & IP_MALLOC_INUSE) != 0) 
+         && (uint8_t *)cur - heap < IP_MALLOC_HEAP_SIZE) {
+    cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
+  }
+
+  if ((uint8_t *)cur < heap + IP_MALLOC_HEAP_SIZE) {
+    uint16_t oldsize = *cur & IP_MALLOC_LEN;
+    bndrt_t *next;
+    sz -= sizeof(bndrt_t);
+    next = ((bndrt_t *)(((uint8_t *)cur) + sz));
+
+    *cur = (sz & IP_MALLOC_LEN) | IP_MALLOC_INUSE;
+    *next = (oldsize - sz) & IP_MALLOC_LEN;
+
+    return cur + 1;
+  } else return NULL;
+}
+
+void ip_free(void *ptr) {
+  bndrt_t *prev = NULL, *cur, *next = NULL;
+  cur = (bndrt_t *)heap;
+
+  while (cur + 1 != ptr && (uint8_t *)cur - heap < IP_MALLOC_HEAP_SIZE) {
+    prev = cur;
+    cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
+  }
+  if (cur + 1 == ptr) {
+    next = (bndrt_t *)((*cur & IP_MALLOC_LEN) + ((uint8_t *)cur));
+
+    *cur &= ~IP_MALLOC_INUSE;
+    if ((((uint8_t *)next) - heap) < IP_MALLOC_HEAP_SIZE && 
+        (*next & IP_MALLOC_INUSE) == 0) {
+      *cur = (*cur & IP_MALLOC_LEN) + (*next & IP_MALLOC_LEN);
+    }
+    if (prev != NULL && (*prev & IP_MALLOC_INUSE) == 0) {
+      *prev = (*prev & IP_MALLOC_LEN) + (*cur & IP_MALLOC_LEN);
+    }
+  }
+}
+
+uint16_t ip_malloc_freespace() {
+  uint16_t ret = 0;
+  bndrt_t *cur = (bndrt_t *)heap;
+
+  while ((uint8_t *)cur - heap < IP_MALLOC_HEAP_SIZE) {
+    if ((*cur & IP_MALLOC_INUSE) == 0)
+      ret += *cur & IP_MALLOC_LEN;
+    cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
+  }
+  return ret;
+}
+
+#ifdef PC
+void dump_heap() {
+  int i;
+  for (i = 0; i < IP_MALLOC_HEAP_SIZE; i++) {
+    printf("0x%x ", heap[i]);
+    if (i % 8 == 7) printf("  ");
+    if (i % 16 == 15) printf ("\n");
+    if (i > 64) break;
+  }
+  printf("\n");
+}
+
+void ip_print_heap() {
+  bndrt_t *cur = (bndrt_t *)heap;
+  while (((uint8_t *)cur)  - heap < IP_MALLOC_HEAP_SIZE) {
+    printf ("heap region start: 0x%x length: %i used: %i\n", 
+            cur, (*cur & IP_MALLOC_LEN), (*cur & IP_MALLOC_INUSE) >> 15);
+    if ((*cur & IP_MALLOC_LEN) == 0) {
+      printf("ERROR: zero length cell detected!\n");
+      dump_heap();
+      exit(1);
+    }
+    cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
+
+  }
+}
+#endif
+#endif