diff --git a/libraries/AP_Common/MultiHeap.cpp b/libraries/AP_Common/MultiHeap.cpp index 75033db1a156c..3f6389c28f39d 100644 --- a/libraries/AP_Common/MultiHeap.cpp +++ b/libraries/AP_Common/MultiHeap.cpp @@ -7,22 +7,16 @@ #include #include #include +#include +#include #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; iallocate_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; } diff --git a/libraries/AP_Common/MultiHeap.h b/libraries/AP_Common/MultiHeap.h index e7aaff15b6539..a79147918113c 100644 --- a/libraries/AP_Common/MultiHeap.h +++ b/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; }; diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index b54b890dceced..3382de24b7c20 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/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"); diff --git a/libraries/AP_Scripting/AP_Scripting.h b/libraries/AP_Scripting/AP_Scripting.h index f7139775c02f6..4f25e0235bdb5 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/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 _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 diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp index 98960a11ceba6..f7f7c8cd7d71c 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/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); diff --git a/libraries/AP_Scripting/lua_scripts.h b/libraries/AP_Scripting/lua_scripts.h index 04001f39bfb60..7832c35955b83 100644 --- a/libraries/AP_Scripting/lua_scripts.h +++ b/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);