Fixed bug #888.

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

4 files changed, 108 insertions, 32 deletions

diff --git a/os/common/oslib/include/chmemcore.h b/os/common/oslib/include/chmemcore.h
index 539d71ecf..401e9b1a9 100644
--- a/os/common/oslib/include/chmemcore.h
+++ b/os/common/oslib/include/chmemcore.h
@@ -68,7 +68,21 @@
 /**

  * @brief   Memory get function.

  */

-typedef void *(*memgetfunc_t)(size_t size, unsigned align);

+typedef void *(*memgetfunc_t)(size_t size, unsigned align, size_t offset);

+

+/**

+ * @brief   Type of memory core object.

+ */

+typedef struct {

+  /**

+   * @brief   Next free address.

+   */

+  uint8_t *nextmem;

+  /**

+   * @brief   Final address.

+   */

+  uint8_t *endmem;

+} memcore_t;

 

 /*===========================================================================*/

 /* Module macros.                                                            */

@@ -78,12 +92,20 @@ typedef void *(*memgetfunc_t)(size_t size, unsigned align);
 /* External declarations.                                                    */

 /*===========================================================================*/

 

+#if !defined(__DOXYGEN__)

+extern memcore_t ch_memcore;

+#endif

+

 #ifdef __cplusplus

 extern "C" {

 #endif

   void _core_init(void);

-  void *chCoreAllocAlignedI(size_t size, unsigned align);

-  void *chCoreAllocAligned(size_t size, unsigned align);

+  void *chCoreAllocAlignedWithOffsetI(size_t size,

+                                      unsigned align,

+                                      size_t offset);

+  void *chCoreAllocAlignedWithOffset(size_t size,

+                                     unsigned align,

+                                     size_t offset);

   size_t chCoreGetStatusX(void);

 #ifdef __cplusplus

 }

@@ -95,6 +117,45 @@ extern "C" {
 

 /**

  * @brief   Allocates a memory block.

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

+ *          specified alignment.

+ *

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

+ * @param[in] align     desired memory alignment

+ * @return              A pointer to the allocated memory block.

+ * @retval NULL         allocation failed, core memory exhausted.

+ *

+ * @iclass

+ */

+static inline void *chCoreAllocAlignedI(size_t size, unsigned align) {

+

+  return chCoreAllocAlignedWithOffsetI(size, align, 0U);

+}

+

+/**

+ * @brief   Allocates a memory block.

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

+ *          specified alignment.

+ *

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

+ * @param[in] align     desired memory alignment

+ * @return              A pointer to the allocated memory block.

+ * @retval NULL         allocation failed, core memory exhausted.

+ *

+ * @api

+ */

+static inline void *chCoreAllocAligned(size_t size, unsigned align) {

+  void *p;

+

+  chSysLock();

+  p = chCoreAllocAlignedWithOffsetI(size, align, 0U);

+  chSysUnlock();

+

+  return p;

+}

+

+/**

+ * @brief   Allocates a memory block.

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

  *          pointer data type.

  *

@@ -106,7 +167,7 @@ extern "C" {
  */

 static inline void *chCoreAllocI(size_t size) {

 

-  return chCoreAllocAlignedI(size, PORT_NATURAL_ALIGN);

+  return chCoreAllocAlignedWithOffsetI(size, PORT_NATURAL_ALIGN, 0U);

 }

 

 /**

@@ -122,7 +183,7 @@ static inline void *chCoreAllocI(size_t size) {
  */

 static inline void *chCoreAlloc(size_t size) {

 

-  return chCoreAllocAligned(size, PORT_NATURAL_ALIGN);

+  return chCoreAllocAlignedWithOffsetI(size, PORT_NATURAL_ALIGN, 0U);

 }

 

 #endif /* CH_CFG_USE_MEMCORE == TRUE */

diff --git a/os/common/oslib/src/chheap.c b/os/common/oslib/src/chheap.c
index f95fbfe1f..ee21b802c 100644
--- a/os/common/oslib/src/chheap.c
+++ b/os/common/oslib/src/chheap.c
@@ -106,7 +106,7 @@ static memory_heap_t default_heap;
  */

 void _heap_init(void) {

 

-  default_heap.provider = chCoreAllocAligned;

+  default_heap.provider = chCoreAllocAlignedWithOffset;

   H_NEXT(&default_heap.header) = NULL;

   H_PAGES(&default_heap.header) = 0;

 #if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)

@@ -164,7 +164,7 @@ void chHeapObjectInit(memory_heap_t *heapp, void *buf, size_t size) {
  * @api

  */

 void *chHeapAllocAligned(memory_heap_t *heapp, size_t size, unsigned align) {

-  heap_header_t *qp, *hp;

+  heap_header_t *qp, *hp, *ahp;

   size_t pages;

 

   chDbgCheck((size > 0U) && MEM_IS_VALID_ALIGNMENT(align));

@@ -188,7 +188,6 @@ void *chHeapAllocAligned(memory_heap_t *heapp, size_t size, unsigned align) {
   /* Start of the free blocks list.*/

   qp = &heapp->header;

   while (H_NEXT(qp) != NULL) {

-    heap_header_t *ahp;

 

     /* Next free block.*/

     hp = H_NEXT(qp);

@@ -261,13 +260,16 @@ void *chHeapAllocAligned(memory_heap_t *heapp, size_t size, unsigned align) {
   /* More memory is required, tries to get it from the associated provider

      else fails.*/

   if (heapp->provider != NULL) {

-    hp = heapp->provider((pages + 1U) * CH_HEAP_ALIGNMENT, align);

-    if (hp != NULL) {

+    ahp = heapp->provider((pages + 1U) * CH_HEAP_ALIGNMENT,

+                          align,

+                          sizeof (heap_header_t));

+    if (ahp != NULL) {

+      hp = ahp - 1U;

       H_HEAP(hp) = heapp;

       H_SIZE(hp) = size;

 

       /*lint -save -e9087 [11.3] Safe cast.*/

-      return (void *)H_BLOCK(hp);

+      return (void *)ahp;

       /*lint -restore*/

     }

   }

diff --git a/os/common/oslib/src/chmemcore.c b/os/common/oslib/src/chmemcore.c
index c094ac2aa..a1323b674 100644
--- a/os/common/oslib/src/chmemcore.c
+++ b/os/common/oslib/src/chmemcore.c
@@ -52,6 +52,11 @@
 /* Module exported variables.                                                */

 /*===========================================================================*/

 

+/**

+ * @brief   Memory core descriptor.

+ */

+memcore_t ch_memcore;

+

 /*===========================================================================*/

 /* Module local types.                                                       */

 /*===========================================================================*/

@@ -60,9 +65,6 @@
 /* Module local variables.                                                   */

 /*===========================================================================*/

 

-static uint8_t *nextmem;

-static uint8_t *endmem;

-

 /*===========================================================================*/

 /* Module local functions.                                                   */

 /*===========================================================================*/

@@ -82,63 +84,74 @@ void _core_init(void) {
   extern uint8_t __heap_end__[];

 

   /*lint -save -e9033 [10.8] Required cast operations.*/

-  nextmem = __heap_base__;

-  endmem  = __heap_end__;

+  ch_memcore.nextmem = __heap_base__;

+  ch_memcore.endmem  = __heap_end__;

   /*lint restore*/

 #else

   static uint8_t static_heap[CH_CFG_MEMCORE_SIZE];

 

-  nextmem = &static_heap[0];

-  endmem  = &static_heap[CH_CFG_MEMCORE_SIZE];

+  ch_memcore.nextmem = &static_heap[0];

+  ch_memcore.endmem  = &static_heap[CH_CFG_MEMCORE_SIZE];

 #endif

 }

 

 /**

  * @brief   Allocates a memory block.

- * @details The allocated block is guaranteed to be properly aligned to the

- *          specified alignment.

+ * @details This function allocates a block of @p offset + @p size bytes. The

+ *          returned pointer has @p offset bytes before its address and

+ *          @p size bytes after.

  *

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

  * @param[in] align     desired memory alignment

+ * @param[in] offset    aligned pointer offset

  * @return              A pointer to the allocated memory block.

  * @retval NULL         allocation failed, core memory exhausted.

  *

  * @iclass

  */

-void *chCoreAllocAlignedI(size_t size, unsigned align) {

-  uint8_t *p;

+void *chCoreAllocAlignedWithOffsetI(size_t size,

+                                    unsigned align,

+                                    size_t offset) {

+  uint8_t *p, *next;

 

   chDbgCheckClassI();

   chDbgCheck(MEM_IS_VALID_ALIGNMENT(align));

 

   size = MEM_ALIGN_NEXT(size, align);

-  p = (uint8_t *)MEM_ALIGN_NEXT(nextmem, align);

+  p = (uint8_t *)MEM_ALIGN_NEXT(ch_memcore.nextmem + offset, align);

+  next = p + size;

 

-  if (((size_t)endmem - (size_t)p) < size) {

+  /* Considering also the case where there is numeric overflow.*/

+  if ((next > ch_memcore.endmem) || (next < ch_memcore.nextmem)) {

     return NULL;

   }

-  nextmem = p + size;

+

+  ch_memcore.nextmem = next;

 

   return p;

 }

 

 /**

  * @brief   Allocates a memory block.

- * @details The allocated block is guaranteed to be properly aligned to the

- *          specified alignment.

+ * @details This function allocates a block of @p offset + @p size bytes. The

+ *          returned pointer has @p offset bytes before its address and

+ *          @p size bytes after.

  *

- * @param[in] size      the size of the block to be allocated

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

  * @param[in] align     desired memory alignment

+ * @param[in] offset    aligned pointer offset

  * @return              A pointer to the allocated memory block.

  * @retval NULL         allocation failed, core memory exhausted.

  *

  * @api

  */

-void *chCoreAllocAligned(size_t size, unsigned align) {

+void *chCoreAllocAlignedWithOffset(size_t size,

+                                   unsigned align,

+                                   size_t offset) {

   void *p;

 

   chSysLock();

-  p = chCoreAllocAlignedI(size, align);

+  p = chCoreAllocAlignedWithOffsetI(size, align, offset);

   chSysUnlock();

 

   return p;

@@ -154,7 +167,7 @@ void *chCoreAllocAligned(size_t size, unsigned align) {
 size_t chCoreGetStatusX(void) {

 

   /*lint -save -e9033 [10.8] The cast is safe.*/

-  return (size_t)(endmem - nextmem);

+  return (size_t)(ch_memcore.endmem - ch_memcore.nextmem);

   /*lint -restore*/

 }

 #endif /* CH_CFG_USE_MEMCORE == TRUE */

diff --git a/os/common/oslib/src/chmempools.c b/os/common/oslib/src/chmempools.c
index 384e2f7ee..fe08c45cf 100644
--- a/os/common/oslib/src/chmempools.c
+++ b/os/common/oslib/src/chmempools.c
@@ -130,7 +130,7 @@ void *chPoolAllocI(memory_pool_t *mp) {
     mp->next = mp->next->next;

   }

   else if (mp->provider != NULL) {

-    objp = mp->provider(mp->object_size, PORT_NATURAL_ALIGN); /* TODO: Alignment is not properly handled */

+    objp = mp->provider(mp->object_size, PORT_NATURAL_ALIGN, 0U); /* TODO: Alignment is not properly handled */

   }

   /*lint -restore*/