diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 7d56981c5c4c3..3f11aa6696eaf 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -76,6 +76,12 @@ void* Util::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) return malloc_dma(size); } else if (mem_type == AP_HAL::Util::MEM_FAST) { return malloc_fastmem(size); + } else if (mem_type == AP_HAL::Util::MEM_FILESYSTEM) { +#if defined(STM32H7) + return malloc_axi_sram(size); +#else + return malloc_dma(size); +#endif } else { return calloc(1, size); } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c index 590855bd59ed4..f1f735ab8f353 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c @@ -21,23 +21,6 @@ #include #include "bouncebuffer.h" -#if defined(STM32H7) -// always use a bouncebuffer on H7, to ensure alignment and padding -#define IS_DMA_SAFE(addr) false -#elif defined(STM32F732xx) -// always use bounce buffer on F732 -#define IS_DMA_SAFE(addr) false -#elif defined(STM32F7) -// on F76x we only consider first half of DTCM memory as DMA safe, 2nd half is used as fast memory for EKF -// on F74x we only have 64k of DTCM -#define IS_DMA_SAFE(addr) ((((uint32_t)(addr)) & ((0xFFFFFFFF & ~(64*1024U-1)) | 1U)) == 0x20000000) -#elif defined(STM32F1) -#define IS_DMA_SAFE(addr) true -#else -// this checks an address is in main memory and 16 bit aligned -#define IS_DMA_SAFE(addr) ((((uint32_t)(addr)) & 0xF0000001) == 0x20000000) -#endif - // Enable when trying to check if you are not just listening yourself #define ENABLE_ECHO_SAFE 0 @@ -64,7 +47,7 @@ void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_b */ bool bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, uint32_t size) { - if (!bouncebuffer || IS_DMA_SAFE(*buf)) { + if (!bouncebuffer || mem_is_dma_safe(*buf, size, bouncebuffer->on_axi_sram)) { // nothing needs to be done return true; } @@ -113,7 +96,7 @@ void bouncebuffer_finish_read(struct bouncebuffer_t *bouncebuffer, const uint8_t */ bool bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t **buf, uint32_t size) { - if (!bouncebuffer || IS_DMA_SAFE(*buf)) { + if (!bouncebuffer || mem_is_dma_safe(*buf, size, bouncebuffer->on_axi_sram)) { // nothing needs to be done return true; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index d9b30d5a69379..dada2e1c433c6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -616,3 +616,44 @@ void ff_memfree(void* mblock) free(mblock); } #endif // USE_POSIX + +/* + return true if a memory region is safe for a DMA operation + */ +bool mem_is_dma_safe(const void *addr, uint32_t size, bool filesystem_op) +{ + (void)filesystem_op; +#if defined(STM32F1) + // F1 is always OK + (void)addr; + (void)size; + return true; +#else + uint32_t flags = MEM_REGION_FLAG_DMA_OK; +#if defined(STM32H7) + if (((uint32_t)addr) & 0x1F) { + return false; + } + if (filesystem_op) { + flags = MEM_REGION_FLAG_AXI_BUS; + } +#elif defined(STM32F4) + if (((uint32_t)addr) & 0x01) { + return false; + } +#else + if (((uint32_t)addr) & 0x07) { + return false; + } +#endif + for (uint8_t i=0; i= (uint32_t)memory_regions[i].address && + ((uint32_t)addr + size) <= ((uint32_t)memory_regions[i].address + memory_regions[i].size)) { + return true; + } + } + } + return false; +#endif // STM32F1 +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 0a56342d256fd..5a24edd59fe97 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h @@ -43,6 +43,7 @@ void *malloc_axi_sram(size_t size); void *malloc_fastmem(size_t size); void *malloc_eth_safe(size_t size); thread_t *thread_create_alloc(size_t size, const char *name, tprio_t prio, tfunc_t pf, void *arg); +bool mem_is_dma_safe(const void *addr, uint32_t size, bool filesystem_op); struct memory_region { void *address; diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index c8cc8dcf7eba8..922440826b06d 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -73,7 +73,11 @@ bool sdcard_init() if (sdcd.bouncebuffer == nullptr) { // allocate 4k bouncebuffer for microSD to match size in // AP_Logger +#if defined(STM32H7) bouncebuffer_init(&sdcd.bouncebuffer, 4096, true); +#else + bouncebuffer_init(&sdcd.bouncebuffer, 4096, false); +#endif } if (sdcard_running) {