Line data    Source code

       1             : /*
       2             :   multiple heap interface, allowing for an allocator that uses
       3             :   multiple underlying heaps to cope with multiple memory regions on
       4             :   STM32 boards
       5             :  */
       6             : 
       7             : #include "AP_MultiHeap.h"
       8             : 
       9             : #include <AP_Math/AP_Math.h>
      10             : #include <AP_Scripting/AP_Scripting_config.h>
      11             : #include <stdio.h>
      12             : 
      13             : /*
      14             :   allow up to 10 heaps
      15             :  */
      16             : #ifndef MAX_HEAPS
      17             : #define MAX_HEAPS 10
      18             : #endif
      19             : 
      20             : extern const AP_HAL::HAL &hal;
      21             : 
      22             : /*
      23             :   create heaps with a total memory size, splitting over at most
      24             :   max_heaps
      25             :  */
      26          99 : bool MultiHeap::create(uint32_t total_size, uint8_t max_heaps, bool _allow_expansion, uint32_t _reserve_size)
      27             : {
      28          99 :     max_heaps = MIN(MAX_HEAPS, max_heaps);
      29          99 :     if (heaps != nullptr) {
      30             :         // don't allow double allocation
      31           0 :         return false;
      32             :     }
      33          99 :     heaps = NEW_NOTHROW Heap[max_heaps];
      34          99 :     if (heaps == nullptr) {
      35           0 :         return false;
      36             :     }
      37          99 :     num_heaps = max_heaps;
      38          99 :     for (uint8_t i=0; i<max_heaps; i++) {
      39          99 :         uint32_t alloc_size = total_size;
      40          99 :         while (alloc_size > 0) {
      41          99 :             heaps[i].hp = heap_create(alloc_size);
      42          99 :             if (heaps[i].hp != nullptr) {
      43          99 :                 total_size -= alloc_size;
      44          99 :                 sum_size += alloc_size;
      45          99 :                 break;
      46             :             }
      47           0 :             alloc_size *= 0.9;
      48             :         }
      49          99 :         if (total_size == 0) {
      50          99 :             break;
      51             :         }
      52             :     }
      53          99 :     if (total_size != 0) {
      54           0 :         destroy();
      55           0 :         return false;
      56             :     }
      57             : 
      58          99 :     allow_expansion = _allow_expansion;
      59          99 :     reserve_size = _reserve_size;
      60             : 
      61          99 :     return true;
      62             : }
      63             : 
      64             : // destroy heap
      65          58 : void MultiHeap::destroy(void)
      66             : {
      67          58 :     if (!available()) {
      68           0 :         return;
      69             :     }
      70         638 :     for (uint8_t i=0; i<num_heaps; i++) {
      71         580 :         if (heaps[i].hp != nullptr) {
      72          58 :             heap_destroy(heaps[i].hp);
      73          58 :             heaps[i].hp = nullptr;
      74             :         }
      75             :     }
      76          58 :     delete[] heaps;
      77          58 :     heaps = nullptr;
      78          58 :     num_heaps = 0;
      79          58 :     sum_size = 0;
      80          58 :     expanded_to = 0;
      81             : }
      82             : 
      83             : // return true if heap is available for operations
      84     3880159 : bool MultiHeap::available(void) const
      85             : {
      86     3880159 :     return heaps != nullptr && heaps[0].hp != nullptr;
      87             : }
      88             : 
      89             : /*
      90             :   allocate memory from a heap
      91             :  */
      92     1924493 : void *MultiHeap::allocate(uint32_t size)
      93             : {
      94     1924493 :     if (!available() || size == 0) {
      95          12 :         return nullptr;
      96             :     }
      97     1924484 :     for (uint8_t i=0; i<num_heaps; i++) {
      98     1924484 :         if (heaps[i].hp == nullptr) {
      99           3 :             break;
     100             :         }
     101     1924481 :         void *newptr = heap_allocate(heaps[i].hp, size);
     102     1924481 :         if (newptr != nullptr) {
     103     1924478 :             last_failed = false;
     104     1924478 :             return newptr;
     105             :         }
     106             :     }
     107           3 :     if (!allow_expansion || !last_failed) {
     108             :         /*
     109             :           we only allow expansion when the last allocation
     110             :           failed. This gives the lua engine a chance to use garbage
     111             :           collection to recover memory
     112             :          */
     113           3 :         last_failed = true;
     114           3 :         return nullptr;
     115             :     }
     116             : 
     117             : #if !AP_SCRIPTING_HEAP_EXPANSION_ALWAYS_ENABLED
     118           0 :     if (!hal.util->get_soft_armed()) {
     119             :         // only expand the available heaps when armed. When disarmed
     120             :         // user should fix their SCR_HEAP_SIZE parameter
     121           0 :         last_failed = true;
     122           0 :         return nullptr;
     123             :     }
     124             : #endif
     125             : 
     126             :     /*
     127             :       MultiHeap (for scripting) is out of memory. We will see if we
     128             :       can add a new heap from available memory if we have at least
     129             :       reserve_size bytes free
     130             :      */
     131           0 :     const uint32_t available = hal.util->available_memory();
     132           0 :     const uint32_t heap_overhead = 128; // conservative value, varies with HAL
     133           0 :     const uint32_t min_size = size + heap_overhead;
     134           0 :     if (available < reserve_size+min_size) {
     135           0 :         last_failed = true;
     136           0 :         return nullptr;
     137             :     }
     138             : 
     139             :     // round up to a minimum of 30k to allocate, and allow for heap overhead
     140           0 :     const uint32_t round_to = 30*1024U;
     141           0 :     const uint32_t alloc_size = MIN(available - reserve_size, MAX(size+heap_overhead, round_to));
     142           0 :     if (alloc_size < min_size) {
     143           0 :         last_failed = true;
     144           0 :         return nullptr;
     145             :     }
     146           0 :     for (uint8_t i=0; i<num_heaps; i++) {
     147           0 :         if (heaps[i].hp == nullptr) {
     148           0 :             heaps[i].hp = heap_create(alloc_size);
     149           0 :             if (heaps[i].hp == nullptr) {
     150           0 :                 last_failed = true;
     151           0 :                 return nullptr;
     152             :             }
     153           0 :             sum_size += alloc_size;
     154           0 :             expanded_to = sum_size;
     155           0 :             void *p = heap_allocate(heaps[i].hp, size);
     156           0 :             last_failed = p == nullptr;
     157           0 :             return p;
     158             :         }
     159             :     }
     160           0 :     last_failed = true;
     161           0 :     return nullptr;
     162             : }
     163             : 
     164             : /*
     165             :   free memory from a heap
     166             :  */
     167     1955412 : void MultiHeap::deallocate(void *ptr)
     168             : {
     169     1955412 :     if (!available() || ptr == nullptr) {
     170       54774 :         return;
     171             :     }
     172     1900638 :     heap_free(ptr);
     173             : }
     174             : 
     175             : /*
     176             :   change size of an allocation, operates like realloc(), but requires
     177             :   the old_size when ptr is not NULL
     178             :  */
     179     3649875 : void *MultiHeap::change_size(void *ptr, uint32_t old_size, uint32_t new_size)
     180             : {
     181     3649875 :     if (new_size == 0) {
     182     1727613 :         deallocate(ptr);
     183     1727613 :         return nullptr;
     184             :     }
     185             :     /*
     186             :       we don't want to require the underlying allocation system to
     187             :       support realloc() and we also want to be able to handle the case
     188             :       of having to move the allocation to a new heap, so we do a
     189             :       simple alloc/copy/deallocate for reallocation
     190             :      */
     191     1922262 :     void *newp = allocate(new_size);
     192     1922262 :     if (ptr == nullptr) {
     193     1696586 :         return newp;
     194             :     }
     195      225676 :     if (newp == nullptr) {
     196           0 :         if (old_size >= new_size) {
     197             :             // Lua assumes that the allocator never fails when osize >= nsize
     198             :             // the best we can do is return the old pointer
     199           0 :             return ptr;
     200             :         }
     201           0 :         return nullptr;
     202             :     }
     203      225676 :     memcpy(newp, ptr, MIN(old_size, new_size));
     204      225676 :     deallocate(ptr);
     205      225676 :     return newp;
     206             : }