AP_Scripting: allow expansion of memory for scripting at runtime

libraries/AP_Common/MultiHeap.cpp

@@ -7,22 +7,16 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Util.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_InternalError/AP_InternalError.h>
 
 #include "MultiHeap.h"
 
 /*
-  on ChibiOS allow up to 4 heaps. On other HALs only allow a single
-  heap. This is needed as hal.util->heap_realloc() needs to have the
-  property that heap_realloc(heap, ptr, 0) must not care if ptr comes
-  from the given heap. This is true on ChibiOS, but is not true on
-  other HALs
+  allow up to 10 heaps
  */
 #ifndef MAX_HEAPS
-#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-#define MAX_HEAPS 4
-#else
-#define MAX_HEAPS 1
-#endif
+#define MAX_HEAPS 10
 #endif
 
 extern const AP_HAL::HAL &hal;
@@ -31,7 +25,7 @@ extern const AP_HAL::HAL &hal;
   create heaps with a total memory size, splitting over at most
   max_heaps
  */
-bool MultiHeap::create(uint32_t total_size, uint8_t max_heaps)
+bool MultiHeap::create(uint32_t total_size, uint8_t max_heaps, bool _allow_expansion, uint32_t _reserve_size)
 {
     max_heaps = MIN(MAX_HEAPS, max_heaps);
     if (heaps != nullptr) {
@@ -61,6 +55,10 @@ bool MultiHeap::create(uint32_t total_size, uint8_t max_heaps)
         destroy();
         return false;
     }
+
+    allow_expansion = _allow_expansion;
+    reserve_size = _reserve_size;
+
     return true;
 }
 
@@ -87,6 +85,22 @@ bool MultiHeap::available(void) const
     return heaps != nullptr && heaps[0] != nullptr;
 }
 
+/*
+  allocate memory from a specific heap
+ */
+void *MultiHeap::allocate_from_heap(void *heap, uint32_t size)
+{
+    struct alloc_header *ptr = (struct alloc_header *)hal.util->heap_realloc(heap, nullptr, 0, size+sizeof(alloc_header));
+    if (ptr == nullptr) {
+        return nullptr;
+    }
+    ptr->heap = heap;
+#ifdef HAL_DEBUG_BUILD
+    ptr->size = size;
+#endif
+    return (void*)(ptr+1);
+}
+
 /*
   allocate memory from a heap
  */
@@ -99,11 +113,43 @@ void *MultiHeap::allocate(uint32_t size)
         if (heaps[i] == nullptr) {
             break;
         }
-        void *newptr = hal.util->heap_realloc(heaps[i], nullptr, 0, size);
+        void *newptr = allocate_from_heap(heaps[i], size);
         if (newptr != nullptr) {
             return newptr;
         }
     }
+    if (!allow_expansion) {
+        return nullptr;
+    }
+
+    if (!hal.util->get_soft_armed()) {
+        // only expand the available heaps when armed. When disarmed
+        // user should fix their SCR_HEAP_SIZE parameter
+        return nullptr;
+    }
+
+    /*
+      vehicle is armed and MultiHeap (for scripting) is out of
+      memory. We will see if we can add a new heap from available
+      memory if we have at least reserve_size bytes free
+     */
+    if (hal.util->available_memory() < reserve_size) {
+        return nullptr;
+    }
+    const uint32_t heap_overhead = 128; // conservative value, varies with HAL
+
+    // round up to multiple of 30k to allocate, and allow for heap overhead
+    const uint32_t alloc_size = MAX(size+heap_overhead, 30*1024U);
+    for (uint8_t i=0; i<num_heaps; i++) {
+        if (heaps[i] == nullptr) {
+            heaps[i] = hal.util->allocate_heap_memory(alloc_size);
+            if (heaps[i] == nullptr) {
+                return nullptr;
+            }
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Expanded scripting heap by %u bytes", unsigned(alloc_size));
+            return allocate_from_heap(heaps[i], size);
+        }
+    }
     return nullptr;
 }
 
@@ -112,12 +158,11 @@ void *MultiHeap::allocate(uint32_t size)
  */
 void MultiHeap::deallocate(void *ptr)
 {
-    if (!available()) {
+    if (!available() || ptr == nullptr) {
         return;
     }
-    // NOTE! this relies on either there being a single heap or heap_realloc()
-    // not using the first argument when size is zero.
-    hal.util->heap_realloc(heaps[0], ptr, 0, 0);
+    struct alloc_header *h = ((struct alloc_header *)ptr)-1U;
+    hal.util->heap_realloc(h->heap, h, 0, 0);
 }
 
 /*
@@ -132,12 +177,25 @@ void *MultiHeap::change_size(void *ptr, uint32_t old_size, uint32_t new_size)
     if (old_size == 0 || ptr == nullptr) {
         return allocate(new_size);
     }
+#ifdef HAL_DEBUG_BUILD
+    if (ptr != nullptr) {
+        struct alloc_header *h = ((struct alloc_header *)ptr)-1U;
+        if (h->size != old_size) {
+            INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+        }
+    }
+#endif
     /*
-      we cannot know which heap this came from, so we rely on the fact
-      that on ChibiOS the underlying call doesn't use the heap
-      argument and on other HALs we only have one heap. We pass
-      through old_size and new_size so that we can validate the lua
-      old_size value
-    */
-    return hal.util->heap_realloc(heaps[0], ptr, old_size, new_size);
+      always allocate again and copy. This allows memory to move
+      between heaps
+     */
+    void *newptr = allocate(new_size);
+    if (newptr == nullptr) {
+        return nullptr;
+    }
+    if (ptr != nullptr) {
+        memcpy(newptr, ptr, MIN(old_size, new_size));
+    }
+    deallocate(ptr);
+    return newptr;
 }

libraries/AP_Common/MultiHeap.h

@@ -9,7 +9,7 @@ class MultiHeap {
     /*
       allocate/deallocate heaps
      */
-    bool create(uint32_t total_size, uint8_t max_heaps);
+    bool create(uint32_t total_size, uint8_t max_heaps, bool allow_expansion, uint32_t reserve_size);
     void destroy(void);
 
     // return true if the heap is available for operations
@@ -23,7 +23,19 @@ class MultiHeap {
     // size, unlike realloc()
     void *change_size(void *ptr, uint32_t old_size, uint32_t new_size);
 
+    struct alloc_header {
+        void *heap;
+#ifdef HAL_DEBUG_BUILD
+        uint32_t size;
+#endif
+    };
+
 private:
+
+    void *allocate_from_heap(void *heap, uint32_t size);
+
     uint8_t num_heaps;
     void **heaps;
+    bool allow_expansion;
+    uint32_t reserve_size;
 };

libraries/AP_Scripting/AP_Scripting.cpp

@@ -359,7 +359,7 @@ void AP_Scripting::thread(void) {
                 GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Scripting: %s", "restarted");
                 break;
             }
-            if ((_debug_options.get() & uint8_t(lua_scripts::DebugLevel::NO_SCRIPTS_TO_RUN)) != 0) {
+            if (option_is_set(DebugOption::NO_SCRIPTS_TO_RUN)) {
                 GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Scripting: %s", "stopped");
             }
         }
@@ -399,7 +399,7 @@ void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd
 
 bool AP_Scripting::arming_checks(size_t buflen, char *buffer) const
 {
-    if (!enabled() || ((_debug_options.get() & uint8_t(lua_scripts::DebugLevel::DISABLE_PRE_ARM)) != 0)) {
+    if (!enabled() || option_is_set(DebugOption::DISABLE_PRE_ARM)) {
         return true;
     }
 
@@ -497,9 +497,7 @@ void AP_Scripting::update() {
 // Check if DEBUG_OPTS bit has been set to save current checksum values to params
 void AP_Scripting::save_checksum() {
 
-    const uint8_t opts = _debug_options.get();
-    const uint8_t save_bit = uint8_t(lua_scripts::DebugLevel::SAVE_CHECKSUM);
-    if ((opts & save_bit) == 0) {
+    if (!option_is_set(DebugOption::SAVE_CHECKSUM)) {
         // Bit not set, nothing to do
         return;
     }
@@ -509,7 +507,7 @@ void AP_Scripting::save_checksum() {
     _required_running_checksum.set_and_save(lua_scripts::get_running_checksum() & checksum_param_mask);
 
     // Un-set debug option bit
-    _debug_options.set_and_save(opts & ~save_bit);
+    option_clear(DebugOption::SAVE_CHECKSUM);
 
     GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Scripting: %s", "saved checksums");
 

libraries/AP_Scripting/AP_Scripting.h

@@ -144,6 +144,16 @@ class AP_Scripting
     command_block_list *mavlink_command_block_list;
     HAL_Semaphore mavlink_command_block_list_sem;
 
+    enum class DebugOption : uint8_t {
+        NO_SCRIPTS_TO_RUN = 1U << 0,
+        RUNTIME_MSG = 1U << 1,
+        SUPPRESS_SCRIPT_LOG = 1U << 2,
+        LOG_RUNTIME = 1U << 3,
+        DISABLE_PRE_ARM = 1U << 4,
+        SAVE_CHECKSUM = 1U << 5,
+        DISABLE_HEAP_EXPANSION = 1U << 6,
+    };
+
 private:
 
     bool repl_start(void);
@@ -180,6 +190,14 @@ class AP_Scripting
 
     AP_Enum<ThreadPriority> _thd_priority;
 
+    bool option_is_set(DebugOption option) const {
+        return (uint8_t(_debug_options.get()) & uint8_t(option)) != 0;
+    }
+
+    void option_clear(DebugOption option) {
+        _debug_options.set_and_save(_debug_options.get() & ~uint8_t(option));
+    }
+
     bool _thread_failed; // thread allocation failed
     bool _init_failed;  // true if memory allocation failed
     bool _restart; // true if scripts should be restarted

libraries/AP_Scripting/lua_scripts.cpp

@@ -40,12 +40,13 @@ uint32_t lua_scripts::loaded_checksum;
 uint32_t lua_scripts::running_checksum;
 HAL_Semaphore lua_scripts::crc_sem;
 
-lua_scripts::lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, const AP_Int8 &debug_options, struct AP_Scripting::terminal_s &_terminal)
+lua_scripts::lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, AP_Int8 &debug_options, struct AP_Scripting::terminal_s &_terminal)
     : _vm_steps(vm_steps),
       _debug_options(debug_options),
      terminal(_terminal)
 {
-    _heap.create(heap_size, 4);
+    const bool allow_heap_expansion = !option_is_set(AP_Scripting::DebugOption::DISABLE_HEAP_EXPANSION);
+    _heap.create(heap_size, 10, allow_heap_expansion, 20*1024);
 }
 
 lua_scripts::~lua_scripts() {
@@ -130,14 +131,14 @@ int lua_scripts::atpanic(lua_State *L) {
 // helper for print and log of runtime stats
 void lua_scripts::update_stats(const char *name, uint32_t run_time, int total_mem, int run_mem)
 {
-    if ((_debug_options.get() & uint8_t(DebugLevel::RUNTIME_MSG)) != 0) {
+    if (option_is_set(AP_Scripting::DebugOption::RUNTIME_MSG)) {
         GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: Time: %u Mem: %d + %d",
                                             (unsigned int)run_time,
                                             (int)total_mem,
                                             (int)run_mem);
     }
 #if HAL_LOGGING_ENABLED
-    if ((_debug_options.get() & uint8_t(DebugLevel::LOG_RUNTIME)) != 0) {
+    if (option_is_set(AP_Scripting::DebugOption::LOG_RUNTIME)) {
         struct log_Scripting pkt {
             LOG_PACKET_HEADER_INIT(LOG_SCRIPTING_MSG),
             time_us      : AP_HAL::micros64(),
@@ -291,7 +292,7 @@ void lua_scripts::load_all_scripts_in_dir(lua_State *L, const char *dirname) {
         reschedule_script(script);
 
 #if HAL_LOGGER_FILE_CONTENTS_ENABLED
-        if ((_debug_options.get() & uint8_t(DebugLevel::SUPPRESS_SCRIPT_LOG)) == 0) {
+        if (!option_is_set(AP_Scripting::DebugOption::SUPPRESS_SCRIPT_LOG)) {
             AP::logger().log_file_content(filename);
         }
 #endif
@@ -569,7 +570,7 @@ void lua_scripts::run(void) {
                 hal.scheduler->delay(scripts->next_run_ms - now_ms);
             }
 
-            if ((_debug_options.get() & uint8_t(DebugLevel::RUNTIME_MSG)) != 0) {
+            if (option_is_set(AP_Scripting::DebugOption::RUNTIME_MSG)) {
                 GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: Running %s", scripts->name);
             }
             // copy name for logging, cant do it after as script reschedule moves the pointers
@@ -598,7 +599,7 @@ void lua_scripts::run(void) {
             lua_gc(L, LUA_GCCOLLECT, 0);
 
         } else {
-            if ((_debug_options.get() & uint8_t(DebugLevel::NO_SCRIPTS_TO_RUN)) != 0) {
+            if (option_is_set(AP_Scripting::DebugOption::NO_SCRIPTS_TO_RUN)) {
                 GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: No scripts to run");
             }
             hal.scheduler->delay(1000);

libraries/AP_Scripting/lua_scripts.h

@@ -34,7 +34,7 @@
 class lua_scripts
 {
 public:
-    lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, const AP_Int8 &debug_options, struct AP_Scripting::terminal_s &_terminal);
+    lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, AP_Int8 &debug_options, struct AP_Scripting::terminal_s &_terminal);
 
     ~lua_scripts();
 
@@ -48,15 +48,6 @@ class lua_scripts
 
     static bool overtime; // script exceeded it's execution slot, and we are bailing out
 
-    enum class DebugLevel {
-        NO_SCRIPTS_TO_RUN = 1U << 0,
-        RUNTIME_MSG = 1U << 1,
-        SUPPRESS_SCRIPT_LOG = 1U << 2,
-        LOG_RUNTIME = 1U << 3,
-        DISABLE_PRE_ARM = 1U << 4,
-        SAVE_CHECKSUM = 1U << 5,
-    };
-
 private:
 
     void create_sandbox(lua_State *L);
@@ -110,7 +101,11 @@ class lua_scripts
     lua_State *lua_state;
 
     const AP_Int32 & _vm_steps;
-    const AP_Int8 & _debug_options;
+    AP_Int8 & _debug_options;
+
+    bool option_is_set(AP_Scripting::DebugOption option) const {
+        return (uint8_t(_debug_options.get()) & uint8_t(option)) != 0;
+    }
 
     static void *alloc(void *ud, void *ptr, size_t osize, size_t nsize);