git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@1073 35acf78f-673a-0410-8e92-d51de3d6d3f4

1 files changed, 276 insertions, 0 deletions

diff --git a/os/kernel/src/chheap.c b/os/kernel/src/chheap.c
new file mode 100644
index 000000000..82b1ba785
--- /dev/null
+++ b/os/kernel/src/chheap.c
@@ -0,0 +1,276 @@
+/*

+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.

+

+    This file is part of ChibiOS/RT.

+

+    ChibiOS/RT 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; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file chheap.c

+ * @brief Heap code.

+ * @addtogroup Heap

+ * @{

+ */

+

+#include <ch.h>

+

+#if CH_USE_HEAP

+

+#if !CH_USE_MALLOC_HEAP

+

+#define MAGIC 0xF5A0

+#define ALIGN_TYPE      void *

+#define ALIGN_MASK      (sizeof(ALIGN_TYPE) - 1)

+#define ALIGN_SIZE(p)   (((size_t)(p) + ALIGN_MASK) & ~ALIGN_MASK)

+

+struct header {

+  union {

+    struct header       *h_next;

+    size_t              h_magic;

+  };

+  size_t                h_size;

+};

+

+static struct {

+  struct header         free;   /* Guaranteed to be not adjacent to the heap */

+#if CH_USE_MUTEXES

+#define H_LOCK()        chMtxLock(&heap.hmtx)

+#define H_UNLOCK()      chMtxUnlock()

+  Mutex                 hmtx;

+#elif CH_USE_SEMAPHORES

+#define H_LOCK()        chSemWait(&heap.hsem)

+#define H_UNLOCK()      chSemSignal(&heap.hsem)

+  Semaphore             hsem;

+#else

+#error "The heap allocator requires mutexes or semaphores to be enabled"

+#endif

+#if CH_HEAP_SIZE > 0

+  union {

+    ALIGN_TYPE          alignment;

+    char                buffer[ALIGN_SIZE(CH_HEAP_SIZE)];

+  };

+#endif

+} heap;

+

+/**

+ * @brief Initializes the allocator subsystem.

+ *

+ * @note Internal use only.

+ */

+void heap_init(void) {

+  struct header *hp;

+

+#if CH_HEAP_SIZE == 0

+  extern char __heap_base__;

+  extern char __heap_end__;

+

+  hp = (void *)&__heap_base__;

+  hp->h_size = &__heap_end__ - &__heap_base__ - sizeof(struct header);

+#else

+  hp = (void *)&heap.buffer[0];

+  hp->h_size = (&heap.buffer[ALIGN_SIZE(CH_HEAP_SIZE)] - &heap.buffer[0]) -

+               sizeof(struct header);

+#endif

+  hp->h_next = NULL;

+  heap.free.h_next = hp;

+  heap.free.h_size = 0;

+#if CH_USE_MUTEXES

+  chMtxInit(&heap.hmtx);

+#else

+  chSemInit(&heap.hsem, 1);

+#endif

+}

+

+/**

+ * @brief Allocates a block of memory from the heap by using the first-fit

+ *        algorithm.

+ * @details The allocated block is guaranteed to be properly aligned for a

+ *          pointer data type.

+ *

+ * @param[in] size the size of the block to be allocated. Note that the

+ *                 allocated block may be a bit bigger than the requested

+ *                 size for alignment and fragmentation reasons.

+ * @return A pointer to the allocated block.

+ * @retval NULL if the block cannot be allocated.

+ */

+void *chHeapAlloc(size_t size) {

+  struct header *qp, *hp, *fp;

+

+  size = ALIGN_SIZE(size);

+  qp = &heap.free;

+  H_LOCK();

+

+  while (qp->h_next != NULL) {

+    hp = qp->h_next;

+    if (hp->h_size >= size) {

+      if (hp->h_size < size + sizeof(struct header)) {

+        /* Gets the whole block even if it is slightly bigger than the

+           requested size because the fragment would be too small to be

+           useful */

+        qp->h_next = hp->h_next;

+      }

+      else {

+        /* Block bigger enough, must split it */

+        fp = (void *)((char *)(hp) + sizeof(struct header) + size);

+        fp->h_next = hp->h_next;

+        fp->h_size = hp->h_size - sizeof(struct header) - size;

+        qp->h_next = fp;

+        hp->h_size = size;

+      }

+      hp->h_magic = MAGIC;

+

+      H_UNLOCK();

+      return (void *)(hp + 1);

+    }

+    qp = hp;

+  }

+

+  H_UNLOCK();

+  return NULL;

+}

+

+#define LIMIT(p) (struct header *)((char *)(p) + \

+                                   sizeof(struct header) + \

+                                   (p)->h_size)

+

+/**

+ * @brief Frees a previously allocated memory block.

+ *

+ * @param[in] p the memory block pointer

+ */

+void chHeapFree(void *p) {

+  struct header *qp, *hp;

+

+  chDbgCheck(p != NULL, "chHeapFree");

+

+  hp = (struct header *)p - 1;

+  chDbgAssert(hp->h_magic == MAGIC,

+              "chHeapFree(), #1",

+              "it is not magic");

+  qp = &heap.free;

+  H_LOCK();

+

+  while (TRUE) {

+

+    chDbgAssert((hp < qp) || (hp >= LIMIT(qp)),

+                "chHeapFree(), #2",

+                "within free block");

+

+    if (((qp == &heap.free) || (hp > qp)) &&

+        ((qp->h_next == NULL) || (hp < qp->h_next))) {

+      /* Insertion after qp */

+      hp->h_next = qp->h_next;

+      qp->h_next = hp;

+      /* Verifies if the newly inserted block should be merged */

+      if (LIMIT(hp) == hp->h_next) {

+        /* Merge with the next block */

+        hp->h_size += hp->h_next->h_size + sizeof(struct header);

+        hp->h_next = hp->h_next->h_next;

+      }

+      if ((LIMIT(qp) == hp)) {  /* Cannot happen when qp == &heap.free */

+        /* Merge with the previous block */

+        qp->h_size += hp->h_size + sizeof(struct header);

+        qp->h_next = hp->h_next;

+      }

+

+      H_UNLOCK();

+      return;

+    }

+    qp = qp->h_next;

+  }

+}

+

+/**

+ * @brief Reports the heap status.

+ *

+ * @param[in] sizep pointer to a variable that will receive the total

+ *                  fragmented free space

+ * @return The number of fragments in the heap.

+ * @note This function is meant to be used in the test suite, it should not be

+ *       really useful for the application code.

+ * @note This function is not implemented when the @p CH_USE_MALLOC_HEAP

+ *       configuration option is used (it always returns zero).

+ */

+size_t chHeapStatus(size_t *sizep) {

+  struct header *qp;

+  size_t n, sz;

+

+  H_LOCK();

+

+  sz = 0;

+  for (n = 0, qp = &heap.free; qp->h_next; n++, qp = qp->h_next)

+    sz += qp->h_next->h_size;

+  if (sizep)

+    *sizep = sz;

+

+  H_UNLOCK();

+  return n;

+}

+

+#else /* CH_USE_MALLOC_HEAP */

+

+#include <stdlib.h>

+

+#if CH_USE_MUTEXES

+#define H_LOCK()        chMtxLock(&hmtx)

+#define H_UNLOCK()      chMtxLock(&hmtx)

+static Mutex            hmtx;

+#elif CH_USE_SEMAPHORES

+#define H_LOCK()        chSemWait(&hsem)

+#define H_UNLOCK()      chSemSignal(&hsem)

+static Semaphore        hsem;

+#else

+#error "The heap allocator requires mutexes or semaphores to be enabled"

+#endif

+

+void heap_init(void) {

+

+#if CH_USE_MUTEXES

+  chMtxInit(&hmtx);

+#else

+  chSemInit(&hsem, 1);

+#endif

+}

+

+void *chHeapAlloc(size_t size) {

+  void *p;

+

+  H_LOCK();

+  p = malloc(size);

+  H_UNLOCK();

+  return p;

+}

+

+void chHeapFree(void *p) {

+

+  chDbgCheck(p != NULL, "chHeapFree");

+

+  H_LOCK();

+  free(p);

+  H_UNLOCK();

+}

+

+size_t chHeapStatus(size_t *sizep) {

+

+  if (sizep)

+    *sizep = 0;

+  return 0;

+}

+

+#endif /* CH_USE_MALLOC_HEAP */

+

+#endif /* CH_USE_HEAP */

+

+/** @} */