2 files changed, 108 insertions, 3 deletions

diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 135767b26..ae2a645f7 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -58,6 +58,7 @@
 #include <nuttx/sdio.h>
 #include <nuttx/mmcsd.h>
 #include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
 
 #include <stm32.h>
 #include "board_config.h"
@@ -69,6 +70,7 @@
 #include <drivers/drv_led.h>
 
 #include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
 
 /****************************************************************************
  * Pre-Processor Definitions
@@ -96,10 +98,70 @@
  * Protected Functions
  ****************************************************************************/
 
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/* 
+ * The DMA heap size constrains the total number of things that can be 
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file. 
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
 
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
 /************************************************************************************
  * Name: stm32_boardinitialize
  *
@@ -110,7 +172,8 @@
  *
  ************************************************************************************/
 
-__EXPORT void stm32_boardinitialize(void)
+__EXPORT void
+stm32_boardinitialize(void)
 {
 	/* configure SPI interfaces */
 	stm32_spiinitialize();
@@ -170,6 +233,9 @@ __EXPORT int nsh_archinitialize(void)
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 	cpuload_initialize_once();
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
index 47f480758..588d648bd 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/tests_file.c
@@ -52,6 +52,44 @@
 int
 test_file(int argc, char *argv[])
 {
+	const iterations = 10;
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+	uint8_t buf[512];
+	hrt_abstime start, end;
+	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
+
+	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+	memset(buf, 0, sizeof(buf));
+
+	start = hrt_absolute_time();
+	for (unsigned i = 0; i < iterations; i++) {
+		perf_begin(wperf);
+		write(fd, buf, sizeof(buf));
+		perf_end(wperf);
+	}
+	end = hrt_absolute_time();
+
+	close(fd);
+
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+	perf_print_counter(wperf);
+	perf_free(wperf);
+
+	return 0;
+}
+#if 0
+int
+test_file(int argc, char *argv[])
+{
+	const iterations = 1024;
+
 	/* check if microSD card is mounted */
 	struct stat buffer;
 	if (stat("/fs/microsd/", &buffer)) {
@@ -67,7 +105,7 @@ test_file(int argc, char *argv[])
 	memset(buf, 0, sizeof(buf));
 
 	start = hrt_absolute_time();
-	for (unsigned i = 0; i < 1024; i++) {
+	for (unsigned i = 0; i < iterations; i++) {
 		perf_begin(wperf);
 		write(fd, buf, sizeof(buf));
 		perf_end(wperf);
@@ -78,7 +116,7 @@ test_file(int argc, char *argv[])
 
 	unlink("/fs/microsd/testfile");
 
-	warnx("512KiB in %llu microseconds", end - start);
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
 	perf_print_counter(wperf);
 	perf_free(wperf);
 
@@ -112,3 +150,4 @@ test_file(int argc, char *argv[])
 
 	return 0;
 }
+#endif