OP-1804: Add realloc.

flight/pios/common/libraries/msheap/msheap.c

@@ -194,13 +194,18 @@ msheap_init(heap_handle_t *heap, void *base, void *limit)
 }
 
 void *
-msheap_alloc(heap_handle_t *heap, uint32_t size)
+msheap_alloc(heap_handle_t *heap, void *ptr, uint32_t size)
 {
     marker_t    cursor;
     marker_t    best;
+    uint32_t    copy_data = 0;
+    uint16_t    old_size = 0;
 
     ASSERT(3, msheap_check(heap));
 
+    if (size == 0)
+        return 0;
+
     /* convert the passed-in size to the number of marker-size units we need to allocate */
     size += marker_size;
     size = round_up(size, marker_size);
@@ -210,6 +215,39 @@ msheap_alloc(heap_handle_t *heap, uint32_t size)
     if (size > heap->heap_free)
         return 0;
 
+    /* realloc */
+    if (ptr != 0) {
+
+        best = (marker_t)ptr - 1;
+        ASSERT(0, region_check(heap, best));
+        ASSERT(3, msheap_check(heap));
+
+#ifdef HEAP_REALLOC_FREE_UNUSED_AREA
+
+        if (best->next.size == size)
+            goto done;
+
+        if (best->next.size > size) {
+            /* this region is free, mark it accordingly */
+            best->next.free = 1;
+            (best + best->next.size)->prev.free = 1;
+
+            traceFREE( ptr, best->next.size );
+
+            /* account for space we are freeing */
+            heap->heap_free += best->next.size;
+
+            goto split;
+        }
+#else
+        if (best->next.size >= size)
+            goto done;
+#endif
+        old_size = best->next.size;
+        msheap_free(heap, ptr);
+        copy_data = 1;
+    }
+
     /* simple single-pass best-fit search */
 restart:
     cursor = heap->free_hint;
@@ -242,13 +280,28 @@ restart:
         /* no space */
         return 0;
     }
+#ifdef HEAP_REALLOC_FREE_UNUSED_AREA
+split:
+#endif
 
     /* split the free region to make space */
     split_region(heap, best, size);
 
     /* update free space counter */
     heap->heap_free -= size;
+
+done:
     traceMALLOC( (void *)(best + 1), size );
+
+    /* Copy data that might be reused */
+    if (copy_data && ptr) {
+        size = old_size;
+        size = size - 1;
+        size *= marker_size;
+        for(uint32_t i=0 ; i < size; i++)
+            ((uint8_t *)(best + 1))[i] = ((uint8_t *)ptr)[i];
+    }
+
     /* and return a pointer to the allocated region */
     return (void *)(best + 1);
 }

flight/pios/common/libraries/msheap/msheap.h

@@ -116,7 +116,7 @@ extern void msheap_init(heap_handle_t *heap, void *base, void *limit);
  *
  * @param   size        The number of bytes required (more may be allocated).
  */
-extern void *msheap_alloc(heap_handle_t *heap, uint32_t size);
+extern void *msheap_alloc(heap_handle_t *heap, void *ptr, uint32_t size);
 
 /**
  * Free memory back to the heap.

flight/pios/common/libraries/msheap/pios_msheap.c

@@ -74,15 +74,15 @@ heap_handle_t fast_heap;
 extern void vApplicationMallocFailedHook(void) __attribute__((weak));
 
 void *
-pios_general_malloc(size_t s, bool use_fast_heap)
+pios_general_malloc(void *ptr, size_t s, bool use_fast_heap)
 {
 	void *p;
 
 	vPortEnterCritical();
 	if(use_fast_heap){
-		p = msheap_alloc(&fast_heap, s);
+		p = msheap_alloc(&fast_heap, ptr, s);
 	} else {
-		p = msheap_alloc(&sram_heap, s);
+		p = msheap_alloc(&sram_heap, ptr, s);
 	}
 	vPortExitCritical();
 
@@ -95,13 +95,13 @@ pios_general_malloc(size_t s, bool use_fast_heap)
 void *
 pvPortMalloc(size_t s)
 {
-	return pios_general_malloc(s, true);
+	return pios_general_malloc(NULL, s, true);
 }
 
 void *
 pvPortMallocStack(size_t s)
 {
-	return pios_general_malloc(s, false);
+	return pios_general_malloc(NULL, s, false);
 }
 
 void
@@ -154,7 +154,7 @@ malloc(size_t size)
 		heap_init_done = 1;
 	}
 
-	return msheap_alloc(sram_heap, size);
+	return msheap_alloc(NULL, sram_heap, size);
 }
 
 void

flight/pios/common/pios_mem.c

@@ -29,17 +29,32 @@
 
 #ifdef PIOS_TARGET_PROVIDES_FAST_HEAP
 // relies on pios_general_malloc to perform the allocation (i.e. pios_msheap.c)
-extern void *pios_general_malloc(size_t size, bool fastheap);
+extern void *pios_general_malloc(void *ptr, size_t size, bool fastheap);
 
 void *pios_fastheapmalloc(size_t size)
 {
-    return pios_general_malloc(size, true);
+    return pios_general_malloc(NULL, size, true);
 }
 
 
 void *pios_malloc(size_t size)
 {
-    return pios_general_malloc(size, false);
+    return pios_general_malloc(NULL, size, false);
+}
+
+void *pios_realloc(__attribute__((unused)) void *ptr, __attribute__((unused)) size_t size)
+{
+#ifdef PIOS_INCLUDE_REALLOC
+    return pios_general_malloc(ptr, size, false);
+
+#else
+    // Not allowed to use realloc without the proper define PIOS_INCLUDE_REALLOC set
+    while (1) {
+        ;
+    }
+    return NULL;
+
+#endif
 }
 
 void pios_free(void *p)
@@ -47,7 +62,7 @@ void pios_free(void *p)
     vPortFree(p);
 }
 
-#else
+#else /* ifdef PIOS_TARGET_PROVIDES_FAST_HEAP */
 // demand to pvPortMalloc implementation
 void *pios_fastheapmalloc(size_t size)
 {
@@ -60,6 +75,21 @@ void *pios_malloc(size_t size)
     return pvPortMalloc(size);
 }
 
+void *pios_realloc(__attribute__((unused)) void *ptr, __attribute__((unused)) size_t size)
+{
+#ifdef PIOS_INCLUDE_REALLOC
+    return pvPortMalloc(size);
+
+#else
+    // Not allowed to use realloc without the proper define PIOS_INCLUDE_REALLOC set
+    while (1) {
+        ;
+    }
+    return NULL;
+
+#endif
+}
+
 void pios_free(void *p)
 {
     vPortFree(p);

flight/pios/inc/pios_mem.h

@@ -32,6 +32,8 @@ void *pios_fastheapmalloc(size_t size);
 
 void *pios_malloc(size_t size);
 
+void *pios_realloc(void *ptr, size_t size);
+
 void pios_free(void *p);
 
 #endif /* PIOS_MEM_H */