From 1ea7355d85e316aadfd90468b3e808bb3dc95ee9 Mon Sep 17 00:00:00 2001 From: gdisirio Date: Sun, 16 Aug 2009 13:07:24 +0000 Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@1073 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/kernel/src/chheap.c | 276 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 276 insertions(+) create mode 100644 os/kernel/src/chheap.c (limited to 'os/kernel/src/chheap.c') 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 . +*/ + +/** + * @file chheap.c + * @brief Heap code. + * @addtogroup Heap + * @{ + */ + +#include + +#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 + +#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 */ + +/** @} */ -- cgit v1.2.3