mirror of https://github.com/ArduPilot/ardupilot
120 lines
3.0 KiB
C++
120 lines
3.0 KiB
C++
#include "AP_MultiHeap.h"
|
|
#include <AP_HAL/AP_HAL_Boards.h>
|
|
|
|
#if ENABLE_HEAP && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
|
|
|
|
/*
|
|
on systems other than chibios we map the allocation to the system
|
|
malloc/free functions
|
|
*/
|
|
|
|
#include <AP_InternalError/AP_InternalError.h>
|
|
#include <stdlib.h>
|
|
|
|
/*
|
|
low level allocator interface using system malloc
|
|
*/
|
|
/*
|
|
default functions for heap management for lua scripting for systems
|
|
that don't have the ability to create separable heaps
|
|
*/
|
|
|
|
struct heap_allocation_header {
|
|
struct heap *hp;
|
|
uint32_t allocation_size; // size of allocated block, not including this header
|
|
};
|
|
|
|
#define HEAP_MAGIC 0x5681ef9f
|
|
|
|
struct heap {
|
|
uint32_t magic;
|
|
uint32_t max_heap_size;
|
|
uint32_t current_heap_usage;
|
|
};
|
|
|
|
void *MultiHeap::heap_create(uint32_t size)
|
|
{
|
|
struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
|
|
if (new_heap != nullptr) {
|
|
new_heap->max_heap_size = size;
|
|
}
|
|
new_heap->magic = HEAP_MAGIC;
|
|
return (void *)new_heap;
|
|
}
|
|
|
|
void MultiHeap::heap_destroy(void *heap_ptr)
|
|
{
|
|
struct heap *heapp = (struct heap*)heap_ptr;
|
|
if (heapp->magic != HEAP_MAGIC) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
|
return;
|
|
}
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
if (heapp->current_heap_usage != 0) {
|
|
// lua should guarantee that there is no memory still
|
|
// allocated when we destroy the heap. Throw an error in SITL
|
|
// if this proves not to be true
|
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
|
}
|
|
#endif
|
|
|
|
// free the heap structure
|
|
free(heapp);
|
|
}
|
|
|
|
/*
|
|
allocate memory on a specific heap
|
|
*/
|
|
void *MultiHeap::heap_allocate(void *heap_ptr, uint32_t size)
|
|
{
|
|
if (heap_ptr == nullptr || size == 0) {
|
|
return nullptr;
|
|
}
|
|
struct heap *heapp = (struct heap*)heap_ptr;
|
|
if (heapp->magic != HEAP_MAGIC) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
|
return nullptr;
|
|
}
|
|
|
|
if (heapp->current_heap_usage + size > heapp->max_heap_size) {
|
|
// fail the allocation as we don't have the memory. Note that
|
|
// we don't simulate the fragmentation that we would get with
|
|
// HAL_ChibiOS
|
|
return nullptr;
|
|
}
|
|
|
|
auto *header = (heap_allocation_header *)malloc(size + sizeof(heap_allocation_header));
|
|
if (header == nullptr) {
|
|
// can't allocate the new memory
|
|
return nullptr;
|
|
}
|
|
header->hp = heapp;
|
|
header->allocation_size = size;
|
|
|
|
heapp->current_heap_usage += size;
|
|
|
|
return header + 1;
|
|
}
|
|
|
|
/*
|
|
free memory from a previous heap_allocate call
|
|
*/
|
|
void MultiHeap::heap_free(void *ptr)
|
|
{
|
|
// extract header
|
|
auto *header = ((struct heap_allocation_header *)ptr)-1;
|
|
auto *heapp = header->hp;
|
|
if (heapp->magic != HEAP_MAGIC) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
|
return;
|
|
}
|
|
const uint32_t size = header->allocation_size;
|
|
heapp->current_heap_usage -= size;
|
|
|
|
// free the memory
|
|
free(header);
|
|
}
|
|
|
|
#endif // ENABLE_HEAP && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
|