mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 07:28:29 -04:00
HAL_ChibiOS: switched to using DTCM memory for DMA
this uses SRAM1 and SRAM2 for main memory, which enables the use of the data cache for faster operation, and using DTCM for all DMA operations.
This commit is contained in:
parent
8b1db792ee
commit
eec4a12cc2
@ -37,6 +37,7 @@ using namespace ChibiOS;
|
||||
extern "C" {
|
||||
size_t mem_available(void);
|
||||
void *malloc_ccm(size_t size);
|
||||
void *malloc_dtcm(size_t size);
|
||||
};
|
||||
|
||||
/**
|
||||
@ -54,6 +55,11 @@ uint32_t Util::available_memory(void)
|
||||
|
||||
void* Util::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type)
|
||||
{
|
||||
#if defined(DTCM_RAM_SIZE) && defined(DTCM_BASE_ADDRESS)
|
||||
if (mem_type == AP_HAL::Util::MEM_DMA_SAFE) {
|
||||
return malloc_dtcm(size);
|
||||
}
|
||||
#endif
|
||||
if (mem_type == AP_HAL::Util::MEM_FAST) {
|
||||
return try_alloc_from_ccm_ram(size);
|
||||
} else {
|
||||
|
@ -21,10 +21,13 @@
|
||||
#include "Semaphores.h"
|
||||
#include "ToneAlarm.h"
|
||||
|
||||
#if defined(STM32F7) && STM32_DMA_CACHE_HANDLING == TRUE
|
||||
// on F7 we check we are in the DTCM region, and 16 bit aligned
|
||||
#define IS_DMA_SAFE(addr) ((uint32_t(addr) & 0xFFFE0001) == 0x20000000)
|
||||
#else
|
||||
// this checks an address is in main memory and 16 bit aligned
|
||||
// on the STM32F7 we assume all memory is DMA safe, and call dma_flush()
|
||||
// and dma_invalidate() when needed
|
||||
#define IS_DMA_SAFE(addr) ((uint32_t(addr) & 0xF0000001) == 0x20000000)
|
||||
#endif
|
||||
|
||||
class ChibiOS::Util : public AP_HAL::Util {
|
||||
public:
|
||||
|
@ -30,13 +30,7 @@
|
||||
#include <chheap.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#if defined(STM32F7) && STM32_DMA_CACHE_HANDLING == TRUE
|
||||
// to allow for dma management functions we need buffers to be
|
||||
// aligned to the cache line size
|
||||
#define MIN_ALIGNMENT 32
|
||||
#else
|
||||
#define MIN_ALIGNMENT 8
|
||||
#endif
|
||||
|
||||
#if defined(CCM_RAM_SIZE)
|
||||
#ifndef CCM_BASE_ADDRESS
|
||||
@ -46,6 +40,11 @@ static memory_heap_t ccm_heap;
|
||||
static bool ccm_heap_initialised = false;
|
||||
#endif
|
||||
|
||||
#if defined(DTCM_RAM_SIZE) && defined(DTCM_BASE_ADDRESS)
|
||||
static memory_heap_t dtcm_heap;
|
||||
static bool dtcm_heap_initialised = false;
|
||||
#endif
|
||||
|
||||
void *malloc_ccm(size_t size)
|
||||
{
|
||||
void *p = NULL;
|
||||
@ -64,6 +63,24 @@ void *malloc_ccm(size_t size)
|
||||
return p;
|
||||
}
|
||||
|
||||
void *malloc_dtcm(size_t size)
|
||||
{
|
||||
void *p = NULL;
|
||||
#if defined(DTCM_RAM_SIZE)
|
||||
if (!dtcm_heap_initialised) {
|
||||
dtcm_heap_initialised = true;
|
||||
chHeapObjectInit(&dtcm_heap, (void *)DTCM_BASE_ADDRESS, DTCM_RAM_SIZE*1024);
|
||||
}
|
||||
p = chHeapAllocAligned(&dtcm_heap, size, CH_HEAP_ALIGNMENT);
|
||||
if (p != NULL) {
|
||||
memset(p, 0, size);
|
||||
}
|
||||
#else
|
||||
(void)size;
|
||||
#endif
|
||||
return p;
|
||||
}
|
||||
|
||||
void *malloc(size_t size)
|
||||
{
|
||||
if (size == 0) {
|
||||
@ -72,11 +89,23 @@ void *malloc(size_t size)
|
||||
void *p = chHeapAllocAligned(NULL, size, MIN_ALIGNMENT);
|
||||
if (p) {
|
||||
memset(p, 0, size);
|
||||
} else {
|
||||
// fall back to CCM memory when main memory full
|
||||
p = malloc_ccm(size);
|
||||
return p;
|
||||
}
|
||||
return p;
|
||||
if (!p) {
|
||||
// fall back to CCM memory
|
||||
p = malloc_ccm(size);
|
||||
if (p) {
|
||||
return p;
|
||||
}
|
||||
}
|
||||
if (!p) {
|
||||
// fall back to DTCM memory
|
||||
p = malloc_dtcm(size);
|
||||
if (p) {
|
||||
return p;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void *calloc(size_t nmemb, size_t size)
|
||||
@ -108,6 +137,12 @@ size_t mem_available(void)
|
||||
chHeapStatus(&ccm_heap, &ccm_available, NULL);
|
||||
totalp += ccm_available;
|
||||
#endif
|
||||
|
||||
#if defined(DTCM_RAM_SIZE) && defined(DTCM_BASE_ADDRESS)
|
||||
size_t dtcm_available = 0;
|
||||
chHeapStatus(&dtcm_heap, &dtcm_available, NULL);
|
||||
totalp += dtcm_available;
|
||||
#endif
|
||||
|
||||
return totalp;
|
||||
}
|
||||
|
@ -5,9 +5,12 @@
|
||||
# but without the TFT interface
|
||||
MCU STM32F7xx STM32F767xx
|
||||
|
||||
# we treat DTCM, SRAM1 and SRAM2 as a single memory region, but
|
||||
# use DMA management operations for all DMA transfers
|
||||
RAM_SIZE_KB 512
|
||||
# use SRAM1 and SRAM2 as main memory, giving the fastest possible
|
||||
# memory. Use DTCM for DMA memory as it needs no data cache operations
|
||||
RAM_SIZE_KB 384
|
||||
RAM_BASE_ADDRESS 0x20020000
|
||||
define DTCM_RAM_SIZE 128
|
||||
define DTCM_BASE_ADDRESS 0x20000000
|
||||
|
||||
# crystal frequency
|
||||
OSCILLATOR_HZ 16000000
|
||||
|
@ -310,10 +310,16 @@ def get_config(name, column=0, required=True, default=None, type=None, spaces=Fa
|
||||
ret = config[name][column]
|
||||
|
||||
if type is not None:
|
||||
try:
|
||||
ret = type(ret)
|
||||
except Exception:
|
||||
error("Badly formed config value %s (got %s)" % (name, ret))
|
||||
if type == int and ret.startswith('0x'):
|
||||
try:
|
||||
ret = int(ret,16)
|
||||
except Exception:
|
||||
error("Badly formed config value %s (got %s)" % (name, ret))
|
||||
else:
|
||||
try:
|
||||
ret = type(ret)
|
||||
except Exception:
|
||||
error("Badly formed config value %s (got %s)" % (name, ret))
|
||||
return ret
|
||||
|
||||
def enable_can(f):
|
||||
@ -410,6 +416,7 @@ def write_ldscript(fname):
|
||||
|
||||
# ram size
|
||||
ram_size = get_config('RAM_SIZE_KB', default=192, type=int)
|
||||
ram_base = get_config('RAM_BASE_ADDRESS', default=0x20000000, type=int)
|
||||
|
||||
flash_base = 0x08000000 + flash_reserve_start * 1024
|
||||
flash_length = flash_size - (flash_reserve_start + flash_reserve_end)
|
||||
@ -420,11 +427,11 @@ def write_ldscript(fname):
|
||||
MEMORY
|
||||
{
|
||||
flash : org = 0x%08x, len = %uK
|
||||
ram0 : org = 0x20000000, len = %uk
|
||||
ram0 : org = 0x%08x, len = %uk
|
||||
}
|
||||
|
||||
INCLUDE common.ld
|
||||
''' % (flash_base, flash_length, ram_size))
|
||||
''' % (flash_base, flash_length, ram_base, ram_size))
|
||||
|
||||
def copy_common_linkerscript(outdir, hwdef):
|
||||
dirpath = os.path.dirname(hwdef)
|
||||
|
Loading…
Reference in New Issue
Block a user