Line data    Source code

       1             : #include "AP_MultiHeap.h"
       2             : #include <AP_HAL/AP_HAL_Boards.h>
       3             : 
       4             : #if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
       5             : 
       6             : /*
       7             :   on systems other than chibios we map the allocation to the system
       8             :   malloc/free functions
       9             :  */
      10             : 
      11             : #include <AP_InternalError/AP_InternalError.h>
      12             : #include <stdlib.h>
      13             : 
      14             : /*
      15             :   low level allocator interface using system malloc
      16             :  */
      17             : /*
      18             :   default functions for heap management for lua scripting for systems
      19             :   that don't have the ability to create separable heaps
      20             :  */
      21             : 
      22             : struct heap_allocation_header {
      23             :     struct heap *hp;
      24             :     uint32_t allocation_size; // size of allocated block, not including this header
      25             : };
      26             : 
      27             : #define HEAP_MAGIC 0x5681ef9f
      28             : 
      29             : struct heap {
      30             :     uint32_t magic;
      31             :     uint32_t max_heap_size;
      32             :     uint32_t current_heap_usage;
      33             : };
      34             : 
      35          99 : void *MultiHeap::heap_create(uint32_t size)
      36             : {
      37          99 :     struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
      38          99 :     if (new_heap != nullptr) {
      39          99 :         new_heap->magic = HEAP_MAGIC;
      40          99 :         new_heap->max_heap_size = size;
      41             :     }
      42          99 :     return (void *)new_heap;
      43             : }
      44             : 
      45          58 : void MultiHeap::heap_destroy(void *heap_ptr)
      46             : {
      47          58 :     struct heap *heapp = (struct heap*)heap_ptr;
      48          58 :     if (heapp->magic != HEAP_MAGIC) {
      49           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      50           0 :         return;
      51             :     }
      52             : 
      53             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      54          57 :     if (heapp->current_heap_usage != 0) {
      55             :         // lua should guarantee that there is no memory still
      56             :         // allocated when we destroy the heap. Throw an error in SITL
      57             :         // if this proves not to be true
      58           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      59             :     }
      60             : #endif
      61             : 
      62             :     // free the heap structure
      63          58 :     free(heapp);
      64             : }
      65             : 
      66             : /*
      67             :   allocate memory on a specific heap
      68             :  */
      69     1924481 : void *MultiHeap::heap_allocate(void *heap_ptr, uint32_t size)
      70             : {
      71     1924481 :     if (heap_ptr == nullptr || size == 0) {
      72           0 :         return nullptr;
      73             :     }
      74     1924481 :     struct heap *heapp = (struct heap*)heap_ptr;
      75     1924481 :     if (heapp->magic != HEAP_MAGIC) {
      76           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      77           0 :         return nullptr;
      78             :     }
      79             : 
      80     1924481 :     if (heapp->current_heap_usage + size > heapp->max_heap_size) {
      81             :         // fail the allocation as we don't have the memory. Note that
      82             :         // we don't simulate the fragmentation that we would get with
      83             :         // HAL_ChibiOS
      84           3 :         return nullptr;
      85             :     }
      86             : 
      87     1924478 :     auto *header = (heap_allocation_header *)malloc(size + sizeof(heap_allocation_header));
      88     1924478 :     if (header == nullptr) {
      89             :         // can't allocate the new memory
      90           0 :         return nullptr;
      91             :     }
      92     1924478 :     header->hp = heapp;
      93     1924478 :     header->allocation_size = size;
      94             : 
      95     1924478 :     heapp->current_heap_usage += size;
      96             : 
      97     1924478 :     return header + 1;
      98             : }
      99             : 
     100             : /*
     101             :   free memory from a previous heap_allocate call
     102             :  */
     103     1900638 : void MultiHeap::heap_free(void *ptr)
     104             : {
     105             :     // extract header
     106     1900638 :     auto *header = ((struct heap_allocation_header *)ptr)-1;
     107     1900638 :     auto *heapp = header->hp;
     108     1900638 :     if (heapp->magic != HEAP_MAGIC) {
     109           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     110           0 :         return;
     111             :     }
     112     1900638 :     const uint32_t size = header->allocation_size;
     113     1900638 :     heapp->current_heap_usage -= size;
     114             : 
     115             :     // free the memory
     116     1900638 :     free(header);
     117             : }
     118             : 
     119             : #endif // CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS