ardupilot/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c

284 lines
6.8 KiB
C

/*
* Copyright (C) Siddharth Bharat Purohit 2017
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
wrappers for allocation functions
Relies on linker wrap options
Note that not all functions that have been wrapped are implemented
here. The others are wrapped to ensure the function is not used
without an implementation. If we need them then we can implement as
needed.
*/
#include <stdio.h>
#include <string.h>
#include <hal.h>
#include <ch.h>
#include <stdarg.h>
#include "stm32_util.h"
#define MEM_REGION_FLAG_DMA_OK 1
#define MEM_REGION_FLAG_FAST 2
#define MEM_REGION_FLAG_SDCARD 4
static const struct memory_region {
void *address;
uint32_t size;
uint32_t flags;
} memory_regions[] = { HAL_MEMORY_REGIONS };
// the first memory region is already setup as the ChibiOS
// default heap, so we will index from 1 in the allocators
#define NUM_MEMORY_REGIONS (sizeof(memory_regions)/sizeof(memory_regions[0]))
#if CH_CFG_USE_HEAP == TRUE
static memory_heap_t heaps[NUM_MEMORY_REGIONS];
#define MIN_ALIGNMENT 8
#if defined(STM32H7)
#define DMA_ALIGNMENT 32
#else
#define DMA_ALIGNMENT 8
#endif
// size of memory reserved for dma-capable alloc
#ifndef DMA_RESERVE_SIZE
#define DMA_RESERVE_SIZE 4096
#endif
#if DMA_RESERVE_SIZE != 0
static memory_heap_t dma_reserve_heap;
#endif
/*
initialise memory handling
*/
void malloc_init(void)
{
uint8_t i;
for (i=1; i<NUM_MEMORY_REGIONS; i++) {
chHeapObjectInit(&heaps[i], memory_regions[i].address, memory_regions[i].size);
}
#if DMA_RESERVE_SIZE != 0
/*
create a DMA reserve heap, to ensure we keep some memory for DMA
safe memory allocations
*/
void *dma_reserve = malloc_dma(DMA_RESERVE_SIZE);
osalDbgAssert(dma_reserve != NULL, "DMA reserve");
chHeapObjectInit(&dma_reserve_heap, dma_reserve, DMA_RESERVE_SIZE);
#endif
}
static void *malloc_flags(size_t size, uint32_t flags)
{
if (size == 0) {
return NULL;
}
const uint8_t dma_flags = (MEM_REGION_FLAG_DMA_OK | MEM_REGION_FLAG_SDCARD);
const uint8_t alignment = (flags&dma_flags?DMA_ALIGNMENT:MIN_ALIGNMENT);
void *p = NULL;
uint8_t i;
if (flags & dma_flags) {
// allocate multiple of DMA alignment
size = (size + (DMA_ALIGNMENT-1)) & ~(DMA_ALIGNMENT-1);
}
// if no flags are set or this is a DMA request and default heap
// is DMA safe then start with default heap
if (flags == 0 || (flags == MEM_REGION_FLAG_DMA_OK &&
(memory_regions[0].flags & MEM_REGION_FLAG_DMA_OK))) {
p = chHeapAllocAligned(NULL, size, alignment);
if (p) {
goto found;
}
}
// try with matching flags
for (i=1; i<NUM_MEMORY_REGIONS; i++) {
if ((flags & MEM_REGION_FLAG_DMA_OK) &&
!(memory_regions[i].flags & MEM_REGION_FLAG_DMA_OK)) {
continue;
}
if ((flags & MEM_REGION_FLAG_SDCARD) &&
!(memory_regions[i].flags & MEM_REGION_FLAG_SDCARD)) {
continue;
}
if ((flags & MEM_REGION_FLAG_FAST) &&
!(memory_regions[i].flags & MEM_REGION_FLAG_FAST)) {
continue;
}
p = chHeapAllocAligned(&heaps[i], size, alignment);
if (p) {
goto found;
}
}
// if this is a not a DMA request then we can fall back to any heap
if (!(flags & dma_flags)) {
for (i=1; i<NUM_MEMORY_REGIONS; i++) {
p = chHeapAllocAligned(&heaps[i], size, alignment);
if (p) {
goto found;
}
}
// try default heap
p = chHeapAllocAligned(NULL, size, alignment);
if (p) {
goto found;
}
}
#if DMA_RESERVE_SIZE != 0
// fall back to DMA reserve
p = chHeapAllocAligned(&dma_reserve_heap, size, alignment);
if (p) {
memset(p, 0, size);
return p;
}
#endif
// failed
return NULL;
found:
memset(p, 0, size);
return p;
}
/*
allocate normal memory
*/
void *malloc(size_t size)
{
return malloc_flags(size, 0);
}
/*
allocate DMA-safe memory
*/
void *malloc_dma(size_t size)
{
return malloc_flags(size, MEM_REGION_FLAG_DMA_OK);
}
/*
allocate DMA-safe memory for microSD transfers. This is only
different on H7 where SDMMC IDMA can't use SRAM4
*/
void *malloc_sdcard_dma(size_t size)
{
#if defined(STM32H7)
return malloc_flags(size, MEM_REGION_FLAG_SDCARD);
#else
return malloc_flags(size, MEM_REGION_FLAG_DMA_OK);
#endif
}
/*
allocate fast memory
*/
void *malloc_fastmem(size_t size)
{
return malloc_flags(size, MEM_REGION_FLAG_FAST);
}
void *calloc(size_t nmemb, size_t size)
{
return malloc(nmemb * size);
}
void free(void *ptr)
{
if(ptr != NULL) {
chHeapFree(ptr);
}
}
/*
return total available memory in bytes
*/
size_t mem_available(void)
{
size_t totalp = 0;
uint8_t i;
// get memory available on main heap
chHeapStatus(NULL, &totalp, NULL);
// we also need to add in memory that is not yet allocated to the heap
totalp += chCoreGetStatusX();
// now our own heaps
for (i=1; i<NUM_MEMORY_REGIONS; i++) {
size_t available = 0;
chHeapStatus(&heaps[i], &available, NULL);
totalp += available;
}
#if DMA_RESERVE_SIZE != 0
// and reserve DMA heap
size_t available = 0;
chHeapStatus(&dma_reserve_heap, &available, NULL);
totalp += available;
#endif
return totalp;
}
/*
allocate a thread on any available heap
*/
thread_t *thread_create_alloc(size_t size,
const char *name, tprio_t prio,
tfunc_t pf, void *arg)
{
thread_t *ret;
// first try default heap
ret = chThdCreateFromHeap(NULL, size, name, prio, pf, arg);
if (ret != NULL) {
return ret;
}
// now try other heaps
uint8_t i;
for (i=1; i<NUM_MEMORY_REGIONS; i++) {
ret = chThdCreateFromHeap(&heaps[i], size, name, prio, pf, arg);
if (ret != NULL) {
return ret;
}
}
return NULL;
}
#endif // CH_CFG_USE_HEAP
/*
flush all memory. Used in chSysHalt()
*/
void memory_flush_all(void)
{
uint8_t i;
for (i=0; i<NUM_MEMORY_REGIONS; i++) {
cacheBufferFlush(memory_regions[i].address, memory_regions[i].size);
}
}